Skip to content

Commit 0fe137a

Browse files
koide3koide3
committed
add test
1 parent 3d19abf commit 0fe137a

16 files changed

+259
-61
lines changed

‎.gitignore

+1
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,3 @@
1+
.vscode/*
12
build/*
23
README.html

‎CMakeLists.txt

+11-17
Original file line numberDiff line numberDiff line change
@@ -7,21 +7,20 @@ set(CMAKE_CXX_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
77

88
set(CMAKE_BUILD_TYPE "Release")
99

10+
find_package(GTest)
1011
find_package(catkin)
12+
1113
find_package(PCL REQUIRED)
1214
find_package(Eigen3 REQUIRED)
1315

14-
# for test
15-
find_package(GTest REQUIRED)
16-
1716
find_package(OpenMP)
1817
if (OPENMP_FOUND)
1918
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
2019
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
2120
endif()
2221

2322
# if you want to use GPU-powered implementation, uncomment the following line
24-
find_package(CUDA)
23+
# find_package(CUDA)
2524
if(CUDA_FOUND)
2625
include_directories(${CUDA_INCLUDE_DIRS})
2726
link_directories(${CUDA_LIBRARY_DIRS})
@@ -32,10 +31,8 @@ endif()
3231
###################################
3332
if(catkin_FOUND)
3433
catkin_package(
35-
INCLUDE_DIRS include thirdparty/Sophus
34+
INCLUDE_DIRS include
3635
LIBRARIES fast_gicp
37-
# CATKIN_DEPENDS other_catkin_pkg
38-
# DEPENDS system_lib
3936
)
4037
endif()
4138

@@ -49,39 +46,29 @@ add_library(fast_gicp
4946
src/fast_gicp/gicp/fast_gicp_st.cpp
5047
src/fast_gicp/gicp/fast_vgicp.cpp
5148
)
52-
5349
target_link_libraries(fast_gicp
5450
${PCL_LIBRARIES}
5551
)
5652
target_include_directories(fast_gicp PUBLIC
5753
include
5854
${PCL_INCLUDE_DIRS}
5955
${EIGEN3_INCLUDE_DIR}
60-
${catkin_INCLUDE_DIRS}
6156
)
6257

6358
add_executable(gicp_align src/align.cpp)
6459
add_dependencies(gicp_align fast_gicp)
6560
target_link_libraries(gicp_align
66-
${catkin_LIBRARIES}
6761
${PCL_LIBRARIES}
6862
fast_gicp
6963
)
7064

7165
add_executable(gicp_kitti src/kitti.cpp)
7266
add_dependencies(gicp_kitti fast_gicp)
7367
target_link_libraries(gicp_kitti
74-
${catkin_LIBRARIES}
7568
${PCL_LIBRARIES}
7669
fast_gicp
7770
)
7871

79-
if(GTEST_FOUND)
80-
add_executable(gicp_test src/test/gicp_test.cpp)
81-
target_link_libraries(gicp_test ${GTEST_BOTH_LIBRARIES} ${PCL_LIBRARIES} fast_gicp)
82-
gtest_add_tests(TARGET gicp_test WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
83-
endif()
84-
8572
if(CUDA_FOUND)
8673
set(CUDA_NVCC_FLAGS "--expt-relaxed-constexpr")
8774
add_definitions(-DUSE_VGICP_CUDA)
@@ -112,3 +99,10 @@ if(CUDA_FOUND)
11299
)
113100
add_dependencies(fast_gicp fast_vgicp_cuda)
114101
endif()
102+
103+
if(GTEST_FOUND)
104+
add_executable(gicp_test src/test/gicp_test.cpp)
105+
add_dependencies(gicp_test fast_gicp)
106+
target_link_libraries(gicp_test ${GTEST_LIBRARIES} ${PCL_LIBRARIES} fast_gicp)
107+
gtest_add_tests(TARGET gicp_test WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} EXTRA_ARGS "${CMAKE_SOURCE_DIR}/data")
108+
endif()

‎data/relative.txt

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
0.999941 0.0108432 -0.000635437 0.485657
2+
-0.0108468 0.999924 -0.00587782 0.10642
3+
0.000571654 0.00588436 0.999983 -0.0131581
4+
0 0 0 1

‎docker/focal/Dockerfile

+14
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
FROM ros:focal
2+
3+
RUN apt-get update && apt-get install --no-install-recommends -y \
4+
&& apt-get install --no-install-recommends -y wget nano build-essential \
5+
git
6+
# && apt-get clean \
7+
# && rm -rf /var/lib/apt/lists/*
8+
9+
WORKDIR /root
10+
RUN git clone https://github.com/PointCloudLibrary/pcl.git
11+
12+
WORKDIR /root
13+
14+
CMD ["bash"]

‎docker/melodic/Dockerfile

+5-3
Original file line numberDiff line numberDiff line change
@@ -2,9 +2,9 @@ FROM ros:melodic
22

33
RUN apt-get update && apt-get install --no-install-recommends -y \
44
&& apt-get install --no-install-recommends -y wget nano build-essential \
5-
ros-melodic-pcl-ros \
6-
&& apt-get clean \
7-
&& rm -rf /var/lib/apt/lists/*
5+
ros-melodic-pcl-ros \
6+
&& apt-get clean \
7+
&& rm -rf /var/lib/apt/lists/*
88

99

1010
RUN mkdir -p /root/catkin_ws/src
@@ -19,6 +19,8 @@ WORKDIR /root/catkin_ws
1919
RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_make'
2020
RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
2121

22+
RUN /root/catkin_ws/devel/lib/fast_gicp/gicp_test /root/catkin_ws/src/fast_gicp/data
23+
2224
WORKDIR /
2325

2426
ENTRYPOINT ["/ros_entrypoint.sh"]

‎docker/melodic_llvm/Dockerfile

+5-3
Original file line numberDiff line numberDiff line change
@@ -2,9 +2,9 @@ FROM ros:melodic
22

33
RUN apt-get update && apt-get install --no-install-recommends -y \
44
&& apt-get install --no-install-recommends -y wget nano build-essential \
5-
libomp-dev clang lld ros-melodic-pcl-ros \
6-
&& apt-get clean \
7-
&& rm -rf /var/lib/apt/lists/*
5+
libomp-dev clang lld ros-melodic-pcl-ros \
6+
&& apt-get clean \
7+
&& rm -rf /var/lib/apt/lists/*
88

99
RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50
1010

@@ -20,6 +20,8 @@ WORKDIR /root/catkin_ws
2020
RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; CC=clang CXX=clang++ catkin_make'
2121
RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
2222

23+
RUN /root/catkin_ws/devel/lib/fast_gicp/gicp_test /root/catkin_ws/src/fast_gicp/data
24+
2325
WORKDIR /
2426

2527
ENTRYPOINT ["/ros_entrypoint.sh"]

‎docker/noetic/Dockerfile

+5-4
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,9 @@ FROM ros:noetic
22

33
RUN apt-get update && apt-get install --no-install-recommends -y \
44
&& apt-get install --no-install-recommends -y wget nano build-essential \
5-
git ros-noetic-pcl-ros \
6-
&& apt-get clean \
7-
&& rm -rf /var/lib/apt/lists/*
8-
5+
git ros-noetic-pcl-ros \
6+
&& apt-get clean \
7+
&& rm -rf /var/lib/apt/lists/*
98

109
RUN mkdir -p /root/catkin_ws/src
1110
WORKDIR /root/catkin_ws/src
@@ -19,6 +18,8 @@ WORKDIR /root/catkin_ws
1918
RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make'
2019
RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
2120

21+
RUN /root/catkin_ws/devel/lib/fast_gicp/gicp_test /root/catkin_ws/src/fast_gicp/data
22+
2223
WORKDIR /
2324

2425
ENTRYPOINT ["/ros_entrypoint.sh"]

‎docker/noetic_llvm/Dockerfile

+6-4
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,10 @@ FROM ros:noetic
22

33
RUN apt-get update && apt-get install --no-install-recommends -y \
44
&& apt-get install --no-install-recommends -y wget nano build-essential \
5-
git clang lld libomp-dev \
6-
ros-noetic-pcl-ros \
7-
&& apt-get clean \
8-
&& rm -rf /var/lib/apt/lists/*
5+
git clang lld libomp-dev \
6+
ros-noetic-pcl-ros \
7+
&& apt-get clean \
8+
&& rm -rf /var/lib/apt/lists/*
99

1010
RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50
1111

@@ -21,6 +21,8 @@ WORKDIR /root/catkin_ws
2121
RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; CC=clang CXX=clang++ catkin_make'
2222
RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
2323

24+
RUN /root/catkin_ws/devel/lib/fast_gicp/gicp_test /root/catkin_ws/src/fast_gicp/data
25+
2426
WORKDIR /
2527

2628
ENTRYPOINT ["/ros_entrypoint.sh"]

‎include/fast_gicp/gicp/fast_gicp.hpp

+3-10
Original file line numberDiff line numberDiff line change
@@ -41,23 +41,16 @@ class FastGICP : public LsqRegistration<PointSource, PointTarget> {
4141
virtual ~FastGICP() override;
4242

4343
void setNumThreads(int n);
44-
4544
void setCorrespondenceRandomness(int k);
46-
4745
void setRegularizationMethod(RegularizationMethod method);
4846

49-
virtual void swapSourceAndTarget();
50-
51-
virtual void clearSource();
52-
53-
virtual void clearTarget();
47+
virtual void swapSourceAndTarget() override;
48+
virtual void clearSource() override;
49+
virtual void clearTarget() override;
5450

5551
virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;
56-
5752
virtual void setSourceCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs);
58-
5953
virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
60-
6154
virtual void setTargetCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs);
6255

6356
const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& getSourceCovariances() const {

‎include/fast_gicp/gicp/fast_vgicp_cuda.hpp

-3
Original file line numberDiff line numberDiff line change
@@ -52,13 +52,10 @@ class FastVGICPCuda : public FastVGICP<PointSource, PointTarget> {
5252
void setNearesetNeighborSearchMethod(NearestNeighborMethod method);
5353

5454
virtual void swapSourceAndTarget() override;
55-
5655
virtual void clearSource() override;
57-
5856
virtual void clearTarget() override;
5957

6058
virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;
61-
6259
virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
6360

6461
protected:

‎include/fast_gicp/gicp/impl/fast_gicp_impl.hpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,6 @@
11
#ifndef FAST_GICP_FAST_GICP_IMPL_HPP
22
#define FAST_GICP_FAST_GICP_IMPL_HPP
33

4-
#include <boost/format.hpp>
54
#include <fast_gicp/so3/so3.hpp>
65

76
namespace fast_gicp {
@@ -160,7 +159,7 @@ double FastGICP<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& tr
160159
double sum_errors = 0.0;
161160
std::vector<Eigen::Matrix<double, 6, 6>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 6>>> Hs(num_threads_);
162161
std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1>>> bs(num_threads_);
163-
for(int i=0; i<num_threads_; i++) {
162+
for(int i = 0; i < num_threads_; i++) {
164163
Hs[i].setZero();
165164
bs[i].setZero();
166165
}

‎include/fast_gicp/gicp/impl/fast_gicp_st_impl.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -26,8 +26,8 @@ void FastGICPSingleThread<PointSource, PointTarget>::computeTransformation(Point
2626

2727
template<typename PointSource, typename PointTarget>
2828
void FastGICPSingleThread<PointSource, PointTarget>::update_correspondences(const Eigen::Isometry3d& x) {
29-
assert(source_covs.size() == input_->size());
30-
assert(target_covs.size() == target_->size());
29+
assert(source_covs_.size() == input_->size());
30+
assert(target_covs_.size() == target_->size());
3131

3232
Eigen::Isometry3f trans = x.template cast<float>();
3333

‎include/fast_gicp/gicp/impl/fast_vgicp_impl.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@ void FastVGICP<PointSource, PointTarget>::swapSourceAndTarget() {
4747
input_.swap(target_);
4848
source_kdtree_.swap(target_kdtree_);
4949
source_covs_.swap(target_covs_);
50+
voxelmap_.reset();
5051
}
5152

5253
template<typename PointSource, typename PointTarget>
@@ -72,7 +73,7 @@ void FastVGICP<PointSource, PointTarget>::update_correspondences(const Eigen::Is
7273
auto offsets = neighbor_offsets(search_method_);
7374

7475
std::vector<std::vector<std::pair<int, GaussianVoxel::Ptr>>> corrs(num_threads_);
75-
for(auto& c: corrs) {
76+
for(auto& c : corrs) {
7677
c.reserve((input_->size() * offsets.size()) / num_threads_);
7778
}
7879

‎include/fast_gicp/gicp/lsq_registration.hpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -39,11 +39,13 @@ class LsqRegistration : public pcl::Registration<PointSource, PointTarget, float
3939
virtual ~LsqRegistration();
4040

4141
void setRotationEpsilon(double eps);
42-
4342
void setInitialLambdaFactor(double init_lambda_factor);
44-
4543
void setDebugPrint(bool lm_debug_print);
4644

45+
virtual void swapSourceAndTarget() {}
46+
virtual void clearSource() {}
47+
virtual void clearTarget() {}
48+
4749
protected:
4850
virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
4951

‎src/kitti.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -85,9 +85,9 @@ int main(int argc, char** argv) {
8585

8686
// registration method
8787
// you should fine-tune hyper-parameters (e.g., voxel resolution, max correspondence distance) for the best result
88-
// fast_gicp::FastGICP<pcl::PointXYZ, pcl::PointXYZ> gicp;
88+
fast_gicp::FastGICP<pcl::PointXYZ, pcl::PointXYZ> gicp;
8989
// fast_gicp::FastVGICP<pcl::PointXYZ, pcl::PointXYZ> gicp;
90-
fast_gicp::FastVGICPCuda<pcl::PointXYZ, pcl::PointXYZ> gicp;
90+
// fast_gicp::FastVGICPCuda<pcl::PointXYZ, pcl::PointXYZ> gicp;
9191
gicp.setMaxCorrespondenceDistance(1.0);
9292

9393
// set initial frame as target

0 commit comments

Comments
 (0)