40 #ifndef PCL_REGISTRATION_IMPL_GICP_HPP_
41 #define PCL_REGISTRATION_IMPL_GICP_HPP_
43 #include <pcl/registration/boost.h>
44 #include <pcl/registration/exceptions.h>
47 template <
typename Po
intSource,
typename Po
intTarget>
48 template<
typename Po
intT>
void
53 if (k_correspondences_ >
int (cloud->
size ()))
55 PCL_ERROR (
"[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->
size (), k_correspondences_);
60 std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
61 std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);
64 if(cloud_covariances.size () < cloud->
size ())
65 cloud_covariances.resize (cloud->
size ());
67 MatricesVector::iterator matrices_iterator = cloud_covariances.begin ();
68 for(
auto points_iterator = cloud->
begin ();
69 points_iterator != cloud->
end ();
70 ++points_iterator, ++matrices_iterator)
72 const PointT &query_point = *points_iterator;
73 Eigen::Matrix3d &cov = *matrices_iterator;
79 kdtree->
nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
82 for(
int j = 0; j < k_correspondences_; j++) {
83 const PointT &pt = (*cloud)[nn_indecies[j]];
89 cov(0,0) += pt.x*pt.x;
91 cov(1,0) += pt.y*pt.x;
92 cov(1,1) += pt.y*pt.y;
94 cov(2,0) += pt.z*pt.x;
95 cov(2,1) += pt.z*pt.y;
96 cov(2,2) += pt.z*pt.z;
99 mean /=
static_cast<double> (k_correspondences_);
101 for (
int k = 0; k < 3; k++)
102 for (
int l = 0; l <= k; l++)
104 cov(k,l) /=
static_cast<double> (k_correspondences_);
105 cov(k,l) -= mean[k]*mean[l];
110 Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
112 Eigen::Matrix3d U = svd.matrixU ();
114 for(
int k = 0; k < 3; k++) {
115 Eigen::Vector3d col = U.col(k);
119 cov+= v * col * col.transpose();
125 template <
typename Po
intSource,
typename Po
intTarget>
void
128 Eigen::Matrix3d dR_dPhi;
129 Eigen::Matrix3d dR_dTheta;
130 Eigen::Matrix3d dR_dPsi;
132 double phi = x[3], theta = x[4], psi = x[5];
134 double cphi = std::cos(phi), sphi = sin(phi);
135 double ctheta = std::cos(theta), stheta = sin(theta);
136 double cpsi = std::cos(psi), spsi = sin(psi);
142 dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta;
143 dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta;
144 dR_dPhi(2,1) = cphi*ctheta;
146 dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta;
147 dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta;
148 dR_dPhi(2,2) = -ctheta*sphi;
150 dR_dTheta(0,0) = -cpsi*stheta;
151 dR_dTheta(1,0) = -spsi*stheta;
152 dR_dTheta(2,0) = -ctheta;
154 dR_dTheta(0,1) = cpsi*ctheta*sphi;
155 dR_dTheta(1,1) = ctheta*sphi*spsi;
156 dR_dTheta(2,1) = -sphi*stheta;
158 dR_dTheta(0,2) = cphi*cpsi*ctheta;
159 dR_dTheta(1,2) = cphi*ctheta*spsi;
160 dR_dTheta(2,2) = -cphi*stheta;
162 dR_dPsi(0,0) = -ctheta*spsi;
163 dR_dPsi(1,0) = cpsi*ctheta;
166 dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta;
167 dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta;
170 dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta;
171 dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta;
174 g[3] = matricesInnerProd(dR_dPhi, R);
175 g[4] = matricesInnerProd(dR_dTheta, R);
176 g[5] = matricesInnerProd(dR_dPsi, R);
180 template <
typename Po
intSource,
typename Po
intTarget>
void
182 const std::vector<int> &indices_src,
184 const std::vector<int> &indices_tgt,
185 Eigen::Matrix4f &transformation_matrix)
187 if (indices_src.size () < 4)
190 "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () <<
" points!");
195 x[0] = transformation_matrix (0,3);
196 x[1] = transformation_matrix (1,3);
197 x[2] = transformation_matrix (2,3);
198 x[3] = std::atan2 (transformation_matrix (2,1), transformation_matrix (2,2));
199 x[4] = asin (-transformation_matrix (2,0));
200 x[5] = std::atan2 (transformation_matrix (1,0), transformation_matrix (0,0));
203 tmp_src_ = &cloud_src;
204 tmp_tgt_ = &cloud_tgt;
205 tmp_idx_src_ = &indices_src;
206 tmp_idx_tgt_ = &indices_tgt;
209 const double gradient_tol = 1e-2;
219 int inner_iterations_ = 0;
234 PCL_DEBUG (
"[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
235 PCL_DEBUG (
"BFGS solver finished with exit code %i \n", result);
236 transformation_matrix.setIdentity();
237 applyState(transformation_matrix, x);
241 "[pcl::" << getClassName () <<
"::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
245 template <
typename Po
intSource,
typename Po
intTarget>
inline double
252 for (
int i = 0; i < m; ++i)
258 Eigen::Vector4f pp (transformation_matrix * p_src);
261 Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
264 f+= double(res.transpose() * temp);
270 template <
typename Po
intSource,
typename Po
intTarget>
inline void
273 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
274 gicp_->applyState(transformation_matrix, x);
278 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
279 int m =
static_cast<int> (gicp_->tmp_idx_src_->size ());
280 for (
int i = 0; i < m; ++i)
283 Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
285 Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
287 Eigen::Vector4f pp (transformation_matrix * p_src);
289 Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
291 Eigen::Vector3d temp (gicp_->mahalanobis ((*gicp_->tmp_idx_src_)[i]) * res);
296 pp = gicp_->base_transformation_ * p_src;
297 Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
298 R+= p_src3 * temp.transpose();
300 g.head<3> ()*= 2.0/m;
302 gicp_->computeRDerivative(x, R, g);
306 template <
typename Po
intSource,
typename Po
intTarget>
inline void
309 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
310 gicp_->applyState(transformation_matrix, x);
313 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
314 const int m =
static_cast<int> (gicp_->tmp_idx_src_->size ());
315 for (
int i = 0; i < m; ++i)
318 Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
320 Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
321 Eigen::Vector4f pp (transformation_matrix * p_src);
323 Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
325 Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
327 f+= double(res.transpose() * temp);
331 pp = gicp_->base_transformation_ * p_src;
332 Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
334 R+= p_src3 * temp.transpose();
337 g.head<3> ()*=
double(2.0/m);
339 gicp_->computeRDerivative(x, R, g);
343 template <
typename Po
intSource,
typename Po
intTarget>
inline void
351 const std::size_t N =
indices_->size ();
371 std::vector<int> nn_indices (1);
372 std::vector<float> nn_dists (1);
379 std::vector<int> source_indices (
indices_->size ());
380 std::vector<int> target_indices (
indices_->size ());
383 Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
384 for(std::size_t i = 0; i < 4; i++)
385 for(std::size_t j = 0; j < 4; j++)
386 for(std::size_t k = 0; k < 4; k++)
389 Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
391 for (std::size_t i = 0; i < N; i++)
393 PointSource query = output[i];
398 PCL_ERROR (
"[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n",
getClassName ().c_str (), (*
indices_)[i]);
403 if (nn_dists[0] < dist_threshold)
405 Eigen::Matrix3d &C1 = (*input_covariances_)[i];
406 Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
411 Eigen::Matrix3d temp = M * R.transpose();
415 source_indices[cnt] =
static_cast<int> (i);
416 target_indices[cnt] = nn_indices[0];
421 source_indices.resize(cnt); target_indices.resize(cnt);
430 for(
int k = 0; k < 4; k++) {
431 for(
int l = 0; l < 4; l++) {
445 PCL_DEBUG (
"[pcl::%s::computeTransformation] Optimization issue %s\n",
getClassName ().c_str (), e.what ());
453 PCL_DEBUG (
"[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
458 PCL_DEBUG (
"[pcl::%s::computeTransformation] Convergence failed\n",
getClassName ().c_str ());
466 template <
typename Po
intSource,
typename Po
intTarget>
void
471 R = Eigen::AngleAxisf (
static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
472 * Eigen::AngleAxisf (
static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
473 * Eigen::AngleAxisf (
static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
474 t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
475 Eigen::Vector4f T (
static_cast<float> (x[0]),
static_cast<float> (x[1]),
static_cast<float> (x[2]), 0.0f);
479 #endif //PCL_REGISTRATION_IMPL_GICP_HPP_