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
59 std::vector<Eigen::Matrix3d>& cloud_covariances)
61 if (k_correspondences_ >
int (cloud->
size ()))
63 PCL_ERROR (
"[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%zu) is less than k_correspondences_ (%zu)!\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 std::vector<Eigen::Matrix3d>::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;
242 PCL_DEBUG (
"[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
243 PCL_DEBUG (
"BFGS solver finished with exit code %i \n", result);
244 transformation_matrix.setIdentity();
245 applyState(transformation_matrix, x);
249 "[pcl::" << getClassName () <<
"::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
253 template <
typename Po
intSource,
typename Po
intTarget>
inline double
260 for (
int i = 0; i < m; ++i)
266 Eigen::Vector4f pp (transformation_matrix * p_src);
269 Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
272 f+= double(res.transpose() * temp);
278 template <
typename Po
intSource,
typename Po
intTarget>
inline void
281 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
282 gicp_->applyState(transformation_matrix, x);
286 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
287 int m =
static_cast<int> (gicp_->tmp_idx_src_->size ());
288 for (
int i = 0; i < m; ++i)
291 Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
293 Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
295 Eigen::Vector4f pp (transformation_matrix * p_src);
297 Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
299 Eigen::Vector3d temp (gicp_->mahalanobis ((*gicp_->tmp_idx_src_)[i]) * res);
304 pp = gicp_->base_transformation_ * p_src;
305 Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
306 R+= p_src3 * temp.transpose();
308 g.head<3> ()*= 2.0/m;
310 gicp_->computeRDerivative(x, R, g);
314 template <
typename Po
intSource,
typename Po
intTarget>
inline void
317 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
318 gicp_->applyState(transformation_matrix, x);
321 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
322 const int m =
static_cast<const int> (gicp_->tmp_idx_src_->size ());
323 for (
int i = 0; i < m; ++i)
326 Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
328 Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
329 Eigen::Vector4f pp (transformation_matrix * p_src);
331 Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
333 Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
335 f+= double(res.transpose() * temp);
339 pp = gicp_->base_transformation_ * p_src;
340 Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
342 R+= p_src3 * temp.transpose();
345 g.head<3> ()*=
double(2.0/m);
347 gicp_->computeRDerivative(x, R, g);
351 template <
typename Po
intSource,
typename Po
intTarget>
inline void
373 std::vector<int> nn_indices (1);
374 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(
size_t i = 0; i < 4; i++)
385 for(
size_t j = 0; j < 4; j++)
386 for(
size_t k = 0; k < 4; k++)
389 Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
391 for (
size_t i = 0; i < N; i++)
393 PointSource query = output[i];
394 query.getVector4fMap () = guess * query.getVector4fMap ();
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)
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 ());
470 template <
typename Po
intSource,
typename Po
intTarget>
void
475 R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
476 * Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
477 * Eigen::AngleAxisf (static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
478 t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
479 Eigen::Vector4f T (static_cast<float> (x[0]), static_cast<float> (x[1]), static_cast<float> (x[2]), 0.0f);
483 #endif //PCL_REGISTRATION_IMPL_GICP_HPP_
boost::shared_ptr< KdTree< PointT > > Ptr
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
An exception that is thrown when the number of correspondants is not equal to the minimum required...
Eigen::Matrix4f base_transformation_
base transformation
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
void computeCovariances(typename pcl::PointCloud< PointT >::ConstPtr cloud, const typename pcl::search::KdTree< PointT >::Ptr tree, std::vector< Eigen::Matrix3d > &cloud_covariances)
compute points covariances matrices according to the K nearest neighbors.
bool searchForNeighbors(const PointSource &query, std::vector< int > &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
BFGSSpace::Status testGradient(Scalar epsilon)
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
const std::string & getClassName() const
Abstract class get name method.
optimization functor structure
IndicesPtr indices_
A pointer to the vector of point indices to use.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
An exception that is thrown when the non linear solver didn't converge.
std::vector< Eigen::Matrix3d > target_covariances_
Target cloud points covariances.
double operator()(const Vector6d &x)
PointCloudConstPtr input_
The input point cloud dataset.
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
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_
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
double rotation_epsilon_
The epsilon constant for rotation error.
VectorType::const_iterator const_iterator
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
Matrix4 transformation_
The transformation matrix estimated by the registration method.
void df(const Vector6d &x, Vector6d &df)
const Eigen::Matrix3d & mahalanobis(size_t index) const
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear op...
PointCloudSource::ConstPtr PointCloudSourceConstPtr
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
bool converged_
Holds internal convergence state, given user parameters.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
KdTreePtr tree_
A pointer to the spatial search object.
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.
char const * what() const
BFGSSpace::Status minimizeOneStep(FVectorType &x)
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)
Rigid transformation computation method with initial guess.
std::vector< Eigen::Matrix3d > input_covariances_
Input cloud points covariances.
A base class for all pcl exceptions which inherits from std::runtime_error.
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
const GeneralizedIterativeClosestPoint * gicp_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Eigen::Matrix< double, 6, 1 > Vector6d
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
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...
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
PointCloud represents the base class in PCL for storing collections of 3D points. ...
BFGSSpace::Status minimizeInit(FVectorType &x)
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
void fdf(const Vector6d &x, double &f, Vector6d &df)
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object of the source.