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>
void 51 setInputSource (cloud);
55 template <
typename Po
intSource,
typename Po
intTarget>
56 template<
typename Po
intT>
void 61 if (k_correspondences_ >
int (cloud->
size ()))
63 PCL_ERROR (
"[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->
size (), k_correspondences_);
68 std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
69 std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);
72 if(cloud_covariances.size () < cloud->
size ())
73 cloud_covariances.resize (cloud->
size ());
76 MatricesVector::iterator matrices_iterator = cloud_covariances.begin ();
78 points_iterator != cloud->
end ();
79 ++points_iterator, ++matrices_iterator)
81 const PointT &query_point = *points_iterator;
82 Eigen::Matrix3d &cov = *matrices_iterator;
88 kdtree->
nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
91 for(
int j = 0; j < k_correspondences_; j++) {
92 const PointT &pt = (*cloud)[nn_indecies[j]];
98 cov(0,0) += pt.x*pt.x;
100 cov(1,0) += pt.y*pt.x;
101 cov(1,1) += pt.y*pt.y;
103 cov(2,0) += pt.z*pt.x;
104 cov(2,1) += pt.z*pt.y;
105 cov(2,2) += pt.z*pt.z;
108 mean /= static_cast<double> (k_correspondences_);
110 for (
int k = 0; k < 3; k++)
111 for (
int l = 0; l <= k; l++)
113 cov(k,l) /= static_cast<double> (k_correspondences_);
114 cov(k,l) -= mean[k]*mean[l];
119 Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
121 Eigen::Matrix3d U = svd.matrixU ();
123 for(
int k = 0; k < 3; k++) {
124 Eigen::Vector3d col = U.col(k);
128 cov+= v * col * col.transpose();
134 template <
typename Po
intSource,
typename Po
intTarget>
void 137 Eigen::Matrix3d dR_dPhi;
138 Eigen::Matrix3d dR_dTheta;
139 Eigen::Matrix3d dR_dPsi;
141 double phi = x[3], theta = x[4], psi = x[5];
143 double cphi = cos(phi), sphi = sin(phi);
144 double ctheta = cos(theta), stheta = sin(theta);
145 double cpsi = cos(psi), spsi = sin(psi);
151 dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta;
152 dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta;
153 dR_dPhi(2,1) = cphi*ctheta;
155 dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta;
156 dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta;
157 dR_dPhi(2,2) = -ctheta*sphi;
159 dR_dTheta(0,0) = -cpsi*stheta;
160 dR_dTheta(1,0) = -spsi*stheta;
161 dR_dTheta(2,0) = -ctheta;
163 dR_dTheta(0,1) = cpsi*ctheta*sphi;
164 dR_dTheta(1,1) = ctheta*sphi*spsi;
165 dR_dTheta(2,1) = -sphi*stheta;
167 dR_dTheta(0,2) = cphi*cpsi*ctheta;
168 dR_dTheta(1,2) = cphi*ctheta*spsi;
169 dR_dTheta(2,2) = -cphi*stheta;
171 dR_dPsi(0,0) = -ctheta*spsi;
172 dR_dPsi(1,0) = cpsi*ctheta;
175 dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta;
176 dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta;
179 dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta;
180 dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta;
183 g[3] = matricesInnerProd(dR_dPhi, R);
184 g[4] = matricesInnerProd(dR_dTheta, R);
185 g[5] = matricesInnerProd(dR_dPsi, R);
189 template <
typename Po
intSource,
typename Po
intTarget>
void 191 const std::vector<int> &indices_src,
193 const std::vector<int> &indices_tgt,
194 Eigen::Matrix4f &transformation_matrix)
196 if (indices_src.size () < 4)
199 "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () <<
" points!");
204 x[0] = transformation_matrix (0,3);
205 x[1] = transformation_matrix (1,3);
206 x[2] = transformation_matrix (2,3);
207 x[3] = atan2 (transformation_matrix (2,1), transformation_matrix (2,2));
208 x[4] = asin (-transformation_matrix (2,0));
209 x[5] = atan2 (transformation_matrix (1,0), transformation_matrix (0,0));
212 tmp_src_ = &cloud_src;
213 tmp_tgt_ = &cloud_tgt;
214 tmp_idx_src_ = &indices_src;
215 tmp_idx_tgt_ = &indices_tgt;
218 const double gradient_tol = 1e-2;
228 int inner_iterations_ = 0;
243 PCL_DEBUG (
"[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
244 PCL_DEBUG (
"BFGS solver finished with exit code %i \n", result);
245 transformation_matrix.setIdentity();
246 applyState(transformation_matrix, x);
250 "[pcl::" << getClassName () <<
"::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
254 template <
typename Po
intSource,
typename Po
intTarget>
inline double 261 for (
int i = 0; i < m; ++i)
267 Eigen::Vector4f pp (transformation_matrix * p_src);
270 Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
273 f+= double(res.transpose() * temp);
279 template <
typename Po
intSource,
typename Po
intTarget>
inline void 282 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
283 gicp_->applyState(transformation_matrix, x);
287 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
288 int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
289 for (
int i = 0; i < m; ++i)
292 Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
294 Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
296 Eigen::Vector4f pp (transformation_matrix * p_src);
298 Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
300 Eigen::Vector3d temp (gicp_->mahalanobis ((*gicp_->tmp_idx_src_)[i]) * res);
305 pp = gicp_->base_transformation_ * p_src;
306 Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
307 R+= p_src3 * temp.transpose();
309 g.head<3> ()*= 2.0/m;
311 gicp_->computeRDerivative(x, R, g);
315 template <
typename Po
intSource,
typename Po
intTarget>
inline void 318 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
319 gicp_->applyState(transformation_matrix, x);
322 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
323 const int m = static_cast<const int> (gicp_->tmp_idx_src_->size ());
324 for (
int i = 0; i < m; ++i)
327 Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
329 Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
330 Eigen::Vector4f pp (transformation_matrix * p_src);
332 Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
334 Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
336 f+= double(res.transpose() * temp);
340 pp = gicp_->base_transformation_ * p_src;
341 Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
343 R+= p_src3 * temp.transpose();
346 g.head<3> ()*=
double(2.0/m);
348 gicp_->computeRDerivative(x, R, g);
352 template <
typename Po
intSource,
typename Po
intTarget>
inline void 380 std::vector<int> nn_indices (1);
381 std::vector<float> nn_dists (1);
388 std::vector<int> source_indices (
indices_->size ());
389 std::vector<int> target_indices (
indices_->size ());
392 Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
393 for(
size_t i = 0; i < 4; i++)
394 for(
size_t j = 0; j < 4; j++)
395 for(
size_t k = 0; k < 4; k++)
398 Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
400 for (
size_t i = 0; i < N; i++)
402 PointSource query = output[i];
407 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]);
412 if (nn_dists[0] < dist_threshold)
414 Eigen::Matrix3d &C1 = (*input_covariances_)[i];
415 Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
420 Eigen::Matrix3d temp = M * R.transpose();
424 source_indices[cnt] = static_cast<int> (i);
425 target_indices[cnt] = nn_indices[0];
430 source_indices.resize(cnt); target_indices.resize(cnt);
439 for(
int k = 0; k < 4; k++) {
440 for(
int l = 0; l < 4; l++) {
454 PCL_DEBUG (
"[pcl::%s::computeTransformation] Optimization issue %s\n",
getClassName ().c_str (), e.what ());
463 PCL_DEBUG (
"[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
467 PCL_DEBUG (
"[pcl::%s::computeTransformation] Convergence failed\n",
getClassName ().c_str ());
475 template <
typename Po
intSource,
typename Po
intTarget>
void 480 R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
481 * Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
482 * Eigen::AngleAxisf (static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
483 t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
484 Eigen::Vector4f T (static_cast<float> (x[0]), static_cast<float> (x[1]), static_cast<float> (x[2]), 0.0f);
488 #endif //PCL_REGISTRATION_IMPL_GICP_HPP_ KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object of the source.
Eigen::Matrix4f base_transformation_
base transformation
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
const std::string & getClassName() const
Abstract class get name method.
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const std::vector< int > &indices_src, const PointCloudTarget &cloud_tgt, const std::vector< int > &indices_tgt, Eigen::Matrix4f &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
boost::function< void(const pcl::PointCloud< PointSource > &cloud_src, const std::vector< int > &src_indices, const pcl::PointCloud< PointTarget > &cloud_tgt, const std::vector< int > &tgt_indices, Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
A base class for all pcl exceptions which inherits from std::runtime_error.
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)
Rigid transformation computation method with initial guess.
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
bool searchForNeighbors(const PointSource &query, std::vector< int > &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
const GeneralizedIterativeClosestPoint * gicp_
double operator()(const Vector6d &x)
IndicesPtr indices_
A pointer to the vector of point indices to use.
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for the k-nearest neighbors for the given query point.
void fdf(const Vector6d &x, double &f, Vector6d &df)
void computeCovariances(typename pcl::PointCloud< PointT >::ConstPtr cloud, const typename pcl::search::KdTree< PointT >::Ptr tree, MatricesVector &cloud_covariances)
compute points covariances matrices according to the K nearest neighbors.
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
void df(const Vector6d &x, Vector6d &df)
void setInputCloud(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input dataset.
KdTreePtr tree_
A pointer to the spatial search object.
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
Matrix4 transformation_
The transformation matrix estimated by the registration method.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points.
An exception that is thrown when the number of correspondants is not equal to the minimum required.
double rotation_epsilon_
The epsilon constant for rotation error.
BFGSSpace::Status testGradient(Scalar epsilon)
An exception that is thrown when the non linear solver didn't converge.
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
MatricesVectorPtr input_covariances_
Input cloud points covariances.
bool converged_
Holds internal convergence state, given user parameters.
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
PointCloudConstPtr input_
The input point cloud dataset.
const Eigen::Matrix3d & mahalanobis(size_t index) const
Eigen::Matrix< double, 6, 1 > Vector6d
A point structure representing Euclidean xyz coordinates, and the RGB color.
BFGSSpace::Status minimizeOneStep(FVectorType &x)
BFGSSpace::Status minimizeInit(FVectorType &x)
optimization functor structure
PointCloudSource::ConstPtr PointCloudSourceConstPtr
MatricesVectorPtr target_covariances_
Target cloud points covariances.
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear op...
VectorType::const_iterator const_iterator