-
Notifications
You must be signed in to change notification settings - Fork 5
/
reg_ALSTLS.h
159 lines (132 loc) · 7.67 KB
/
reg_ALSTLS.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
//
// This file is for the general implements of ICP and its variants for fine registration
// Dependent 3rd Libs: PCL (>1.7)
// Author: Yue Pan et al. @ WHU LIESMARS
//
#ifndef REG_ALS_TLS
#define REG_ALS_TLS
#include <Eigen/dense>
#include "utility.h"
using namespace utility;
using namespace std;
using namespace Eigen;
template <typename PointT>
class ICPs
{
public:
/**
* \brief Point-to-Point metric ICP
* \param[in] SourceCloud : A pointer of the Source Point Cloud (Each point of it is used to find the nearest neighbor as correspondence)
* \param[in] TargetCloud : A pointer of the Target Point Cloud
* \param[out] TransformedSource : A pointer of the Source Point Cloud after registration [ Transformed ]
* \param[out] transformationS2T : The transformation matrix (4*4) of Source Point Cloud for this registration
* \param[in] max_iter : A parameter controls the max iteration number for the registration
* \param[in] use_reciprocal_correspondence : A parameter controls whether use reciprocal correspondence or not (bool)
* \param[in] use_trimmed_rejector : A parameter controls whether use trimmed correspondence rejector or not (bool)
* \param[in] thre_dis : A parameter used to estimate the approximate overlap ratio of Source Point Cloud. It acts as the search radius of overlapping estimation.
*/
void icp_reg(const typename pcl::PointCloud<PointT>::Ptr & SourceCloud,
const typename pcl::PointCloud<PointT>::Ptr & TargetCloud,
typename pcl::PointCloud<PointT>::Ptr & TransformedSource,
Eigen::Matrix4f & transformationS2T,
int max_iter,
bool use_reciprocal_correspondence,
bool use_trimmed_rejector,
float thre_dis);
/**
* \brief Point-to-Plane metric ICP
* \param[in] SourceCloud : A pointer of the Source Point Cloud (Each point of it is used to find the nearest neighbor as correspondence)
* \param[in] TargetCloud : A pointer of the Target Point Cloud
* \param[out] TransformedSource : A pointer of the Source Point Cloud after registration [ Transformed ]
* \param[out] transformationS2T : The transformation matrix (4*4) of Source Point Cloud for this registration
* \param[in] max_iter : A parameter controls the max iteration number for the registration
* \param[in] use_reciprocal_correspondence : A parameter controls whether use reciprocal correspondence or not (bool)
* \param[in] use_trimmed_rejector : A parameter controls whether use trimmed correspondence rejector or not (bool)
* \param[in] thre_dis : A parameter used to estimate the approximate overlap ratio of Source Point Cloud. It acts as the search radius of overlapping estimation.
*/
// Tips: The Source and Target Point Cloud must have normal (You need to calculated it outside this method). In this case, PointT should be something like PointXYZ**Normal
void ptplicp_reg(const typename pcl::PointCloud<PointT>::Ptr & SourceCloud,
const typename pcl::PointCloud<PointT>::Ptr & TargetCloud,
typename pcl::PointCloud<PointT>::Ptr & TransformedSource,
Eigen::Matrix4f & transformationS2T,
int max_iter,
bool use_reciprocal_correspondence,
bool use_trimmed_rejector,
float thre_dis);
/**
* \brief Generalized (Plane-to-Plane) metric ICP
* \param[in] SourceCloud : A pointer of the Source Point Cloud (Each point of it is used to find the nearest neighbor as correspondence)
* \param[in] TargetCloud : A pointer of the Target Point Cloud
* \param[out] TransformedSource : A pointer of the Source Point Cloud after registration [ Transformed ]
* \param[out] transformationS2T : The transformation matrix (4*4) of Source Point Cloud for this registration
* \param[in] k_neighbor_covariance : A parameter controls the number of points used to calculate the approximate covariance of points, which is used to accomplish the General ICP
* \param[in] max_iter : A parameter controls the max iteration number for the registration
* \param[in] use_reciprocal_correspondence : A parameter controls whether use reciprocal correspondence or not (bool)
* \param[in] use_trimmed_rejector : A parameter controls whether use trimmed correspondence rejector or not (bool)
* \param[in] thre_dis : A parameter used to estimate the approximate overlap ratio of Source Point Cloud. It acts as the search radius of overlapping estimation.
*/
// Attention please.
// Problem : It is found that the G-ICP codes in pcl does not support the correspondence rejector because its computeTransformation method is completely different from classic icp's though gicp is inherited from icp.
// In my opinion, this is the main reason for G-ICP's ill behavior. I am working on the G-ICP with trimmed property now.
void gicp_reg(const typename pcl::PointCloud<PointT>::Ptr & SourceCloud,
const typename pcl::PointCloud<PointT>::Ptr & TargetCloud,
typename pcl::PointCloud<PointT>::Ptr & TransformedSource,
Eigen::Matrix4f & transformationS2T,
int k_neighbor_covariance,
int max_iter,
bool use_reciprocal_correspondence,
bool use_trimmed_rejector,
float thre_dis);
/**
* \brief Estimated the approximate overlapping ratio for Cloud1 considering Cloud2
* \param[in] Cloud1 : A pointer of the Point Cloud used for overlap ratio calculation
* \param[in] Cloud2 : A pointer of the Point Cloud overlapped with Cloud1
* \param[out] thre_dis : It acts as the search radius of overlapping estimation
* \return : The estimated overlap ratio [from 0 to 1]
*/
float calOverlap(const typename pcl::PointCloud<PointT>::Ptr & Cloud1,
const typename pcl::PointCloud<PointT>::Ptr & Cloud2,
float thre_dis);
/**
* \brief Transform a Point Cloud using a given transformation matrix
* \param[in] Cloud : A pointer of the Point Cloud before transformation
* \param[out] TransformedCloud : A pointer of the Point Cloud after transformation
* \param[in] transformation : A 4*4 transformation matrix
*/
void transformcloud(typename pcl::PointCloud<PointT>::Ptr & Cloud,
typename pcl::PointCloud<PointT>::Ptr & TransformedCloud,
Eigen::Matrix4f & transformation);
/**
* \brief Calculate the Normal of a given Point Cloud
* \param[in] cloud : A pointer of the Point Cloud
* \param[out] cloud_normals : A pointer of the Point Cloud's Normal
* \param[in] search_radius : The search radius for neighborhood covariance calculation and PCA
*/
void calNormal(const typename pcl::PointCloud<PointT>::Ptr & cloud,
typename pcl::PointCloud<pcl::Normal>::Ptr cloud_normals,
float search_radius);
/**
* \brief Calculate the Covariance of a given Point Cloud
* \param[in] cloud : A pointer of the Point Cloud
* \param[out] cloud_normals : A pointer of the Point Cloud's Normal
* \param[out] covariances : The covariance of the Point Cloud
* \param[in] search_radius : The search radius for neighborhood covariance calculation and PCA
*/
void calCovariance(const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals,
std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> & covariances,
float search_radius);
/**
* \brief Get the Inverse (Not Mathimatically) of a giving 4*4 Transformation Matrix
* \param[in] transformation : A 4*4 transformation matrix ( from Cloud A to Cloud A' )
* \param[out] invtransformation : Inversed 4*4 transformation matrix ( from Cloud A' to Cloud A )
*/
void invTransform(const Eigen::Matrix4f & transformation,
Eigen::Matrix4f & invtransformation);
protected:
void PointcloudwithNormal(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::PointCloud<pcl::Normal>::Ptr & cloudnormal,
pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloudwithnormal);
private:
};
#endif //REG_ALS_TLS