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 ());
68 MatricesVector::iterator matrices_iterator = cloud_covariances.begin ();
70 points_iterator != cloud->
end ();
71 ++points_iterator, ++matrices_iterator)
73 const PointT &query_point = *points_iterator;
74 Eigen::Matrix3d &cov = *matrices_iterator;
80 kdtree->
nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
83 for(
int j = 0; j < k_correspondences_; j++) {
84 const PointT &pt = (*cloud)[nn_indecies[j]];
90 cov(0,0) += pt.x*pt.x;
92 cov(1,0) += pt.y*pt.x;
93 cov(1,1) += pt.y*pt.y;
95 cov(2,0) += pt.z*pt.x;
96 cov(2,1) += pt.z*pt.y;
97 cov(2,2) += pt.z*pt.z;
100 mean /=
static_cast<double> (k_correspondences_);
102 for (
int k = 0; k < 3; k++)
103 for (
int l = 0; l <= k; l++)
105 cov(k,l) /=
static_cast<double> (k_correspondences_);
106 cov(k,l) -= mean[k]*mean[l];
111 Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
113 Eigen::Matrix3d U = svd.matrixU ();
115 for(
int k = 0; k < 3; k++) {
116 Eigen::Vector3d col = U.col(k);
120 cov+= v * col * col.transpose();
126 template <
typename Po
intSource,
typename Po
intTarget>
void
129 Eigen::Matrix3d dR_dPhi;
130 Eigen::Matrix3d dR_dTheta;
131 Eigen::Matrix3d dR_dPsi;
133 double phi = x[3], theta = x[4], psi = x[5];
135 double cphi = cos(phi), sphi = sin(phi);
136 double ctheta = cos(theta), stheta = sin(theta);
137 double cpsi = cos(psi), spsi = sin(psi);
143 dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta;
144 dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta;
145 dR_dPhi(2,1) = cphi*ctheta;
147 dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta;
148 dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta;
149 dR_dPhi(2,2) = -ctheta*sphi;
151 dR_dTheta(0,0) = -cpsi*stheta;
152 dR_dTheta(1,0) = -spsi*stheta;
153 dR_dTheta(2,0) = -ctheta;
155 dR_dTheta(0,1) = cpsi*ctheta*sphi;
156 dR_dTheta(1,1) = ctheta*sphi*spsi;
157 dR_dTheta(2,1) = -sphi*stheta;
159 dR_dTheta(0,2) = cphi*cpsi*ctheta;
160 dR_dTheta(1,2) = cphi*ctheta*spsi;
161 dR_dTheta(2,2) = -cphi*stheta;
163 dR_dPsi(0,0) = -ctheta*spsi;
164 dR_dPsi(1,0) = cpsi*ctheta;
167 dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta;
168 dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta;
171 dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta;
172 dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta;
175 g[3] = matricesInnerProd(dR_dPhi, R);
176 g[4] = matricesInnerProd(dR_dTheta, R);
177 g[5] = matricesInnerProd(dR_dPsi, R);
181 template <
typename Po
intSource,
typename Po
intTarget>
void
183 const std::vector<int> &indices_src,
185 const std::vector<int> &indices_tgt,
186 Eigen::Matrix4f &transformation_matrix)
188 if (indices_src.size () < 4)
191 "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () <<
" points!");
196 x[0] = transformation_matrix (0,3);
197 x[1] = transformation_matrix (1,3);
198 x[2] = transformation_matrix (2,3);
199 x[3] = atan2 (transformation_matrix (2,1), transformation_matrix (2,2));
200 x[4] = asin (-transformation_matrix (2,0));
201 x[5] = atan2 (transformation_matrix (1,0), transformation_matrix (0,0));
204 tmp_src_ = &cloud_src;
205 tmp_tgt_ = &cloud_tgt;
206 tmp_idx_src_ = &indices_src;
207 tmp_idx_tgt_ = &indices_tgt;
210 const double gradient_tol = 1e-2;
220 int inner_iterations_ = 0;
235 PCL_DEBUG (
"[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
236 PCL_DEBUG (
"BFGS solver finished with exit code %i \n", result);
237 transformation_matrix.setIdentity();
238 applyState(transformation_matrix, x);
242 "[pcl::" << getClassName () <<
"::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
246 template <
typename Po
intSource,
typename Po
intTarget>
inline double
253 for (
int i = 0; i < m; ++i)
259 Eigen::Vector4f pp (transformation_matrix * p_src);
262 Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
265 f+= double(res.transpose() * temp);
271 template <
typename Po
intSource,
typename Po
intTarget>
inline void
274 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
275 gicp_->applyState(transformation_matrix, x);
279 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
280 int m =
static_cast<int> (gicp_->tmp_idx_src_->size ());
281 for (
int i = 0; i < m; ++i)
284 Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
286 Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
288 Eigen::Vector4f pp (transformation_matrix * p_src);
290 Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
292 Eigen::Vector3d temp (gicp_->mahalanobis ((*gicp_->tmp_idx_src_)[i]) * res);
297 pp = gicp_->base_transformation_ * p_src;
298 Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
299 R+= p_src3 * temp.transpose();
301 g.head<3> ()*= 2.0/m;
303 gicp_->computeRDerivative(x, R, g);
307 template <
typename Po
intSource,
typename Po
intTarget>
inline void
310 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
311 gicp_->applyState(transformation_matrix, x);
314 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
315 const int m =
static_cast<const int> (gicp_->tmp_idx_src_->size ());
316 for (
int i = 0; i < m; ++i)
319 Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
321 Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
322 Eigen::Vector4f pp (transformation_matrix * p_src);
324 Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
326 Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
328 f+= double(res.transpose() * temp);
332 pp = gicp_->base_transformation_ * p_src;
333 Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
335 R+= p_src3 * temp.transpose();
338 g.head<3> ()*=
double(2.0/m);
340 gicp_->computeRDerivative(x, R, g);
344 template <
typename Po
intSource,
typename Po
intTarget>
inline void
372 std::vector<int> nn_indices (1);
373 std::vector<float> nn_dists (1);
380 std::vector<int> source_indices (
indices_->size ());
381 std::vector<int> target_indices (
indices_->size ());
384 Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
385 for(
size_t i = 0; i < 4; i++)
386 for(
size_t j = 0; j < 4; j++)
387 for(
size_t k = 0; k < 4; k++)
390 Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
392 for (
size_t i = 0; i < N; i++)
394 PointSource query = output[i];
399 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]);
404 if (nn_dists[0] < dist_threshold)
406 Eigen::Matrix3d &C1 = (*input_covariances_)[i];
407 Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
412 Eigen::Matrix3d temp = M * R.transpose();
416 source_indices[cnt] =
static_cast<int> (i);
417 target_indices[cnt] = nn_indices[0];
422 source_indices.resize(cnt); target_indices.resize(cnt);
431 for(
int k = 0; k < 4; k++) {
432 for(
int l = 0; l < 4; l++) {
446 PCL_DEBUG (
"[pcl::%s::computeTransformation] Optimization issue %s\n",
getClassName ().c_str (), e.what ());
455 PCL_DEBUG (
"[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
459 PCL_DEBUG (
"[pcl::%s::computeTransformation] Convergence failed\n",
getClassName ().c_str ());
467 template <
typename Po
intSource,
typename Po
intTarget>
void
472 R = Eigen::AngleAxisf (
static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
473 * Eigen::AngleAxisf (
static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
474 * Eigen::AngleAxisf (
static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
475 t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
476 Eigen::Vector4f T (
static_cast<float> (x[0]),
static_cast<float> (x[1]),
static_cast<float> (x[2]), 0.0f);
480 #endif //PCL_REGISTRATION_IMPL_GICP_HPP_