Skip to content

Commit d761642

Browse files
JebediahJebediah
authored
Feature 11 pcl icp (#28)
* Add PCL ICP wrapper * tests and associated fixes * Adjust deps install scripts and top level CMake
1 parent f855c6b commit d761642

File tree

15 files changed

+531
-6
lines changed

15 files changed

+531
-6
lines changed

‎.clang-format

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
BasedOnStyle: Chromium
2-
AccessModifierOffset: -4
2+
AccessModifierOffset: -3
33
AlignAfterOpenBracket: true
44
AlignEscapedNewlinesLeft: true
55
AlignOperands: true

‎.gitignore

+4
Original file line numberDiff line numberDiff line change
@@ -21,3 +21,7 @@ build
2121
# matlab / octave
2222
*.mat
2323
octave-workspace
24+
25+
# clion
26+
.idea/
27+
cmake-build-*/

‎CMakeLists.txt

+10-4
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,11 @@ ENDIF()
99

1010
# INCLUDES
1111
INCLUDE_DIRECTORIES(
12-
wave_estimation/include
13-
wave_kinematics/include
14-
wave_optimization/include
15-
wave_utils/include
12+
wave_estimation/include
13+
wave_kinematics/include
14+
wave_optimization/include
15+
wave_utils/include
16+
wave_matching/include
1617
)
1718

1819
# gtest
@@ -26,14 +27,19 @@ ADD_DEPENDENCIES(wave_optimization wave_utils)
2627
# libwave_utils.a
2728
ADD_SUBDIRECTORY(wave_utils)
2829

30+
# libwave_matching.a
31+
ADD_SUBDIRECTORY(wave_matching)
32+
2933
# libwave.a
3034
ADD_CUSTOM_TARGET(
3135
libwave ALL
3236
COMMAND
3337
${CMAKE_AR} rc libwave.a
3438
${CMAKE_BINARY_DIR}/wave_optimization/libwave_optimization.a
3539
${CMAKE_BINARY_DIR}/wave_utils/libwave_utils.a
40+
${CMAKE_BINARY_DIR}/wave_matching/libwave_matching.a
3641
DEPENDS
3742
wave_optimization
3843
wave_utils
44+
wave_matching
3945
)

‎docs/api/matching/icp.md

+72
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
# wave/matching/icp.hpp
2+
3+
This is a wrap of ICP in PCL
4+
5+
## Type Definitions
6+
7+
namespace wave {
8+
9+
typedef pcl::PointCloud<pcl::PointXYZ>::Ptr PCLPointCloud;
10+
11+
} // end of wave namespace
12+
13+
This type definition is used as a shorthand for the pointcloud object type used
14+
by the scan matching implementations in PCL. A number of those implementations are
15+
wrapped, so they use the same datatype for the pointcloud.
16+
17+
## ICP Matcher
18+
19+
class ICPMatcher : public Matcher<PCLPointCloud> {
20+
public:
21+
explicit ICPMatcher(float resolution);
22+
void setRef(const PCLPointCloud &ref);
23+
void setTarget(const PCLPointCloud &target);
24+
bool match();
25+
26+
private:
27+
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
28+
pcl::VoxelGrid<pcl::PointXYZ> filter;
29+
PCLPointCloud ref, target, final;
30+
};
31+
32+
33+
### Parameters
34+
35+
There are a few parameters that may be changed specific to this algorithm. They
36+
can be set in the yaml config file.
37+
38+
* MaxCorrespondenceDistance: correspondences behind this many distance-units are
39+
discarded
40+
* MaximumIterations: Limits number of ICP iterations
41+
* TransformationEpsilon: Criteria to stop iterating. If the difference between consecutive transformations is less than this, stop.
42+
* EuclideanFitnessEpsilon: Criteria to stop iterating. If the cost function does
43+
not improve by more than this quantity, stop.
44+
45+
### Constructor
46+
47+
explicit ICPMatcher(float resolution);
48+
49+
This constructor takes an argument in order to adjust how much downsampling is
50+
done before matching is attempted. Pointclouds are downsampled using a voxel filter,
51+
the argument is the edge length of each voxel. If resolution is non-positive,
52+
no downsampling is used
53+
54+
### Private Members
55+
56+
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp
57+
58+
This is an instance of the ICP class from PCL
59+
60+
---
61+
62+
pcl::VoxelGrid<pcl::PointXYZ> filter;
63+
64+
This is an instance of a PCL voxel filter. It is used to downsample input.
65+
66+
---
67+
68+
PCLPointCloud ref, target, final;
69+
70+
These are pointers to the reference and target pointclouds. The "final" pointcloud
71+
is not exposed. PCL's ICP class creates an aligned verison of the target pointcloud
72+
after matching, so the "final" member is used as a sink for it.

‎docs/api/matching/matching.md

+112
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
# wave/matching
2+
3+
This module contains classes to perform scan matching/registration. There are (or
4+
will be) a variety of different scan matching algorithms available with a
5+
similar interface.
6+
7+
## Matcher
8+
9+
template <typename T>
10+
class Matcher {
11+
public:
12+
Matcher(float res) : resolution(res) {}
13+
Matcher() {
14+
resolution = -1;
15+
}
16+
17+
const Eigen::Affine3d &getResult() {
18+
return this->result;
19+
};
20+
const Eigen::MatrixXd &getInfo() {
21+
return this->information;
22+
};
23+
float getRes() {
24+
return this->resolution;
25+
};
26+
27+
virtual void setRef(const T &ref) = 0;
28+
virtual void setTarget(const T &target) = 0;
29+
void setup(const T &ref, const T &target) {
30+
this->setRef(ref);
31+
this->setTarget(target);
32+
};
33+
34+
virtual bool match() {
35+
return 0;
36+
}
37+
38+
protected:
39+
float resolution;
40+
Eigen::Affine3d result;
41+
Eigen::MatrixXd information;
42+
};
43+
44+
} // end of wave namespace
45+
46+
This template class is inherited by each of the different matching algorithms.
47+
48+
### Members
49+
50+
float resolution
51+
The edge length (in the same distance-units as those used in the pointcloud) of
52+
a downsampling voxel filter. If no downsampling is happening, this will be -1
53+
54+
---
55+
56+
Eigen::Affine3d result
57+
This is the transformation calculated by the scan registration algorithm. It is
58+
the transformation needed to transform the target pointcloud to the reference
59+
pointcloud in the reference frame of the reference pointcloud.
60+
61+
---
62+
63+
Eigen::MatrixXd information
64+
65+
This is the information matrix calculated by the scan-matching algorithm. The
66+
diagonal elements correpond to translational perturbations in the x, y, and z
67+
directions and angular perturbations on the 3-2-1 euler angles, in that order
68+
and in the frame of the reference pointcloud. This may change as the kinematics
69+
module of libwave progresses.
70+
71+
---
72+
73+
### Constructor
74+
75+
Matcher()
76+
This constructor will set up the instance of the matcher to attempt to match
77+
using the full resolution of each point cloud (if possible).
78+
79+
---
80+
81+
Matcher(float res)
82+
83+
This constructor takes an argument in order to adjust how much downsampling is
84+
done before matching is attempted. Pointclouds are downsampled using a voxel filter,
85+
the argument is the edge length of each voxel.
86+
87+
---
88+
89+
### Methods
90+
91+
virtual void setRef(const T &ref) = 0;
92+
virtual void setTarget(const T &target) = 0;
93+
void setup(const T &ref, const T &target) {
94+
this->setRef(ref);
95+
this->setTarget(target);
96+
};
97+
98+
setRef and setTarget are implemented for each specific matching algorithm and are
99+
how the pointclouds are passed to the matching object. Note that there isn't a
100+
parameter for an initial transform; the initial transform is always assumed to
101+
be identity inside the matcher class. If an initial transform estimate is available,
102+
the target pointcloud should be transformed by it before being passed to the class
103+
as some algorithms require a good initial estimate to perform well.
104+
105+
---
106+
107+
virtual bool match();
108+
109+
Actually performs the match. Any heavy processing is done here.
110+
Returns:
111+
* true if match was successful
112+
* false if match was not successful

‎scripts/install/install_deps.bash

+2-1
Original file line numberDiff line numberDiff line change
@@ -23,4 +23,5 @@ install_dependency "Boost" $SCRIPT_PATH/boost_install.bash
2323
install_dependency "Ceres" $SCRIPT_PATH/ceres_install.bash
2424
install_dependency "Eigen" $SCRIPT_PATH/eigen_install.bash
2525
install_dependency "Yaml-CPP" $SCRIPT_PATH/yaml_cpp_install.bash
26-
install_dependency "OpenCV 2.4" $SCRIPT_PATH/opencv2_install.bash
26+
install_dependency "OpenCV 2.4" $SCRIPT_PATH/opencv2_install.bash
27+
install_dependency "PCL 1.7.2" $SCRIPT_PATH/pcl_install.bash

‎scripts/install/pcl_install.bash

+19
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
#!/bin/bash
2+
set -e # exit on first error
3+
4+
install_dependencies() {
5+
sudo apt-get -y install -qq \
6+
libboost-all-dev \
7+
libflann-dev \
8+
libvtk6-dev \
9+
libeigen3-dev \
10+
libusb-1.0-0-dev
11+
}
12+
13+
echo "Installing PCL ..."
14+
15+
install_dependencies
16+
17+
sudo add-apt-repository -y ppa:v-launchpad-jochen-sprickerhof-de/pcl
18+
sudo apt-get -y update
19+
sudo apt-get -y install -qq libpcl-all

‎wave_matching/CMakeLists.txt

+56
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
CMAKE_MINIMUM_REQUIRED(VERSION 2.8.3)
2+
PROJECT(wave_matching)
3+
4+
find_package(PCL REQUIRED common registration point_types io)
5+
find_package(OpenCV REQUIRED)
6+
7+
add_definitions(${EIGEN_DEFINITIONS}
8+
${PCL_DEFINITIONS})
9+
10+
include_directories(${PCL_INCLUDE_DIRS}
11+
${EIGEN_INCLUDE_DIRS}
12+
/usr/include
13+
/usr/include/eigen3
14+
/usr/local/include
15+
include
16+
)
17+
18+
link_directories(${PCL_LIBRARY_DIRS}
19+
${EIGEN_LIBRARY_DIRS}
20+
/usr/lib
21+
/usr/local/lib
22+
/usr/lib/x86_64-linux-gnu/)
23+
24+
set(DEPS
25+
yaml-cpp
26+
wave_utils
27+
${OpenCV_LIBS}
28+
${PCL_COMMON_LIBRARIES}
29+
${PCL_REGISTRATION_LIBRARIES}
30+
${PCL_POINT_TYPES_LIBRARIES}
31+
${PCL_IO_LIBRARIES})
32+
33+
set(TEST_DEPS
34+
wave_matching
35+
wave_utils
36+
gtest
37+
${CMAKE_THREAD_LIBS_INIT})
38+
39+
add_library(wave_matching STATIC src/icp.cpp)
40+
target_link_libraries(wave_matching ${DEPS})
41+
42+
# Unit tests
43+
SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/tests)
44+
FILE(COPY tests/data DESTINATION ${PROJECT_BINARY_DIR}/tests)
45+
FILE(COPY config DESTINATION ${PROJECT_BINARY_DIR}/tests)
46+
47+
ADD_EXECUTABLE(
48+
# TARGET
49+
icp_tests
50+
# TESTS
51+
tests/icp_tests.cpp
52+
# TEST RUNNER
53+
tests/test_runner.cpp
54+
)
55+
TARGET_LINK_LIBRARIES(icp_tests
56+
${TEST_DEPS})

‎wave_matching/config/icp.yaml

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
icp:
2+
maxCorrespondence: 3
3+
maxIterations: 100
4+
transformationEpsilon: 1e-8
5+
euclideanFitnessEpsilon: 1e-2

‎wave_matching/include/wave/matching/icp.hpp

+30
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
// Class to wrap PCL ICP matcher
2+
3+
#ifndef WAVE_MATCHING_ICP_HPP
4+
#define WAVE_MATCHING_ICP_HPP
5+
6+
#include <pcl/point_types.h>
7+
#include <pcl/point_cloud.h>
8+
#include <pcl/registration/icp.h>
9+
#include <pcl/filters/voxel_grid.h>
10+
#include "wave/matching/matcher.hpp"
11+
12+
namespace wave {
13+
14+
typedef pcl::PointCloud<pcl::PointXYZ>::Ptr PCLPointCloud;
15+
class ICPMatcher : public Matcher<PCLPointCloud> {
16+
public:
17+
explicit ICPMatcher(float resolution);
18+
void setRef(const PCLPointCloud &ref);
19+
void setTarget(const PCLPointCloud &target);
20+
bool match();
21+
22+
private:
23+
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
24+
pcl::VoxelGrid<pcl::PointXYZ> filter;
25+
PCLPointCloud ref, target, final;
26+
};
27+
28+
} // end of wave namespace
29+
30+
#endif // WAVE_MATCHING_ICP_HPP

0 commit comments

Comments
 (0)