56 if (k_correspondences_ >
int(cloud->size())) {
57 PCL_ERROR(
"[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or "
58 "points in cloud (%lu) is less than k_correspondences_ (%lu)!\n",
66 nn_indecies.reserve(k_correspondences_);
67 std::vector<float> nn_dist_sq;
68 nn_dist_sq.reserve(k_correspondences_);
71 if (cloud_covariances.size() < cloud->size())
72 cloud_covariances.resize(cloud->size());
74 MatricesVector::iterator matrices_iterator = cloud_covariances.begin();
75 for (
auto points_iterator = cloud->begin(); points_iterator != cloud->end();
76 ++points_iterator, ++matrices_iterator) {
77 const PointT& query_point = *points_iterator;
78 Eigen::Matrix3d& cov = *matrices_iterator;
84 kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
87 for (
int j = 0; j < k_correspondences_; j++) {
88 const PointT& pt = (*cloud)[nn_indecies[j]];
94 cov(0, 0) += pt.x * pt.x;
96 cov(1, 0) += pt.y * pt.x;
97 cov(1, 1) += pt.y * pt.y;
99 cov(2, 0) += pt.z * pt.x;
100 cov(2, 1) += pt.z * pt.y;
101 cov(2, 2) += pt.z * pt.z;
104 mean /=
static_cast<double>(k_correspondences_);
106 for (
int k = 0; k < 3; k++)
107 for (
int l = 0; l <= k; l++) {
108 cov(k, l) /=
static_cast<double>(k_correspondences_);
109 cov(k, l) -= mean[k] * mean[l];
110 cov(l, k) = cov(k, l);
114 Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
116 Eigen::Matrix3d U = svd.matrixU();
119 for (
int k = 0; k < 3; k++) {
120 Eigen::Vector3d col = U.col(k);
124 cov += v * col * col.transpose();
134 Eigen::Matrix3d dR_dPhi;
135 Eigen::Matrix3d dR_dTheta;
136 Eigen::Matrix3d dR_dPsi;
138 double phi = x[3], theta = x[4], psi = x[5];
140 double cphi = std::cos(phi), sphi = sin(phi);
141 double ctheta = std::cos(theta), stheta = sin(theta);
142 double cpsi = std::cos(psi), spsi = sin(psi);
148 dR_dPhi(0, 1) = sphi * spsi + cphi * cpsi * stheta;
149 dR_dPhi(1, 1) = -cpsi * sphi + cphi * spsi * stheta;
150 dR_dPhi(2, 1) = cphi * ctheta;
152 dR_dPhi(0, 2) = cphi * spsi - cpsi * sphi * stheta;
153 dR_dPhi(1, 2) = -cphi * cpsi - sphi * spsi * stheta;
154 dR_dPhi(2, 2) = -ctheta * sphi;
156 dR_dTheta(0, 0) = -cpsi * stheta;
157 dR_dTheta(1, 0) = -spsi * stheta;
158 dR_dTheta(2, 0) = -ctheta;
160 dR_dTheta(0, 1) = cpsi * ctheta * sphi;
161 dR_dTheta(1, 1) = ctheta * sphi * spsi;
162 dR_dTheta(2, 1) = -sphi * stheta;
164 dR_dTheta(0, 2) = cphi * cpsi * ctheta;
165 dR_dTheta(1, 2) = cphi * ctheta * spsi;
166 dR_dTheta(2, 2) = -cphi * stheta;
168 dR_dPsi(0, 0) = -ctheta * spsi;
169 dR_dPsi(1, 0) = cpsi * ctheta;
172 dR_dPsi(0, 1) = -cphi * cpsi - sphi * spsi * stheta;
173 dR_dPsi(1, 1) = -cphi * spsi + cpsi * sphi * stheta;
176 dR_dPsi(0, 2) = cpsi * sphi - cphi * spsi * stheta;
177 dR_dPsi(1, 2) = sphi * spsi + cphi * cpsi * stheta;
180 g[3] = matricesInnerProd(dR_dPhi, R);
181 g[4] = matricesInnerProd(dR_dTheta, R);
182 g[5] = matricesInnerProd(dR_dPsi, R);
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.
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const pcl::Indices &indices_src, const PointCloudTarget &cloud_tgt, const pcl::Indices &indices_tgt, Eigen::Matrix4f &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.