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 279 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
281 for (
int i = 0; i < m; ++i)
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]);
298 Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
299 R+= p_src3 * temp.transpose();
301 g.head<3> ()*= 2.0/m;
307 template <
typename Po
intSource,
typename Po
intTarget>
inline void 314 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
316 for (
int i = 0; i < m; ++i)
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]);
328 f+= double(res.transpose() * temp);
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);
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_ 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)
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 correspondents 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
MatricesVectorPtr target_covariances_
Target cloud points covariances.
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear...
VectorType::const_iterator const_iterator