template <typename PointT, typename Scalar> void pcl::demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, const Eigen::Matrix<Scalar, 4, 1> ¢roid, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out, int npts) { // Calculate the number of points if not given if (npts == 0) { while (cloud_iterator.isValid ()) { ++npts; ++cloud_iterator; } cloud_iterator.reset (); } cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned int i = 0; while (cloud_iterator.isValid ()) { cloud_out (0, i) = cloud_iterator->x - centroid[0]; cloud_out (1, i) = cloud_iterator->y - centroid[1]; cloud_out (2, i) = cloud_iterator->z - centroid[2]; ++i; ++cloud_iterator; } }
TEST (PCL, compute3DCentroidCloudIterator) { pcl::PointIndices pindices; std::vector<int> indices; PointXYZ point; PointCloud<PointXYZ> cloud; Eigen::Vector4f centroid_f; for (point.x = -1; point.x < 2; point.x += 2) { for (point.y = -1; point.y < 2; point.y += 2) { for (point.z = -1; point.z < 2; point.z += 2) { cloud.push_back (point); } } } cloud.is_dense = true; indices.resize (4); // only positive y values indices [0] = 2; indices [1] = 3; indices [2] = 6; indices [3] = 7; // Test finite data { ConstCloudIterator<PointXYZ> it (cloud, indices); EXPECT_EQ (compute3DCentroid (it, centroid_f), 4); EXPECT_EQ (centroid_f[0], 0.0f); EXPECT_EQ (centroid_f[1], 1.0f); EXPECT_EQ (centroid_f[2], 0.0f); EXPECT_EQ (centroid_f[3], 1.0f); Eigen::Vector4d centroid_d; it.reset (); EXPECT_EQ (compute3DCentroid (it, centroid_d), 4); EXPECT_EQ (centroid_d[0], 0.0); EXPECT_EQ (centroid_d[1], 1.0); EXPECT_EQ (centroid_d[2], 0.0); EXPECT_EQ (centroid_d[3], 1.0); } // Test for non-finite data { point.getVector3fMap() << std::numeric_limits<float>::quiet_NaN (), std::numeric_limits<float>::quiet_NaN (), std::numeric_limits<float>::quiet_NaN (); cloud.push_back (point); cloud.is_dense = false; ConstCloudIterator<PointXYZ> it (cloud); EXPECT_EQ (8, compute3DCentroid (it, centroid_f)); EXPECT_EQ_VECTORS (Eigen::Vector4f (0.f, 0.f, 0.f, 1.f), centroid_f); } }
template <typename PointT, typename Scalar> void pcl::demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, const Eigen::Matrix<Scalar, 4, 1> ¢roid, pcl::PointCloud<PointT> &cloud_out, int npts) { // Calculate the number of points if not given if (npts == 0) { while (cloud_iterator.isValid ()) { ++npts; ++cloud_iterator; } cloud_iterator.reset (); } int i = 0; cloud_out.resize (npts); // Subtract the centroid from cloud_in while (cloud_iterator.isValid ()) { cloud_out[i].x = cloud_iterator->x - centroid[0]; cloud_out[i].y = cloud_iterator->y - centroid[1]; cloud_out[i].z = cloud_iterator->z - centroid[2]; ++i; ++cloud_iterator; } cloud_out.width = cloud_out.size (); cloud_out.height = 1; }
template <typename PointSource, typename PointTarget, typename Scalar> inline void pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation ( ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const { // Convert to Eigen format const int npts = static_cast <int> (source_it.size ()); Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts); Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts); for (int i = 0; i < npts; ++i) { cloud_src (0, i) = source_it->x; cloud_src (1, i) = source_it->y; cloud_src (2, i) = source_it->z; ++source_it; cloud_tgt (0, i) = target_it->x; cloud_tgt (1, i) = target_it->y; cloud_tgt (2, i) = target_it->z; ++target_it; } if (use_umeyama_) { // Call Umeyama directly from Eigen (PCL patched version until Eigen is released) transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false); } else { source_it.reset (); target_it.reset (); // <cloud_src,cloud_src> is the source dataset transformation_matrix.setIdentity (); Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt; // Estimate the centroids of source, target compute3DCentroid (source_it, centroid_src); compute3DCentroid (target_it, centroid_tgt); source_it.reset (); target_it.reset (); // Subtract the centroids from source, target Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean; demeanPointCloud (source_it, centroid_src, cloud_src_demean); demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean); getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); } }
template <typename PointT, typename Scalar> inline unsigned int pcl::compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator, Eigen::Matrix<Scalar, 4, 1> ¢roid) { // Initialize to 0 centroid.setZero (); unsigned cp = 0; // For each point in the cloud // If the data is dense, we don't need to check for NaN while (cloud_iterator.isValid ()) { // Check if the point is invalid if (!pcl_isfinite (cloud_iterator->x) || !pcl_isfinite (cloud_iterator->y) || !pcl_isfinite (cloud_iterator->z)) continue; centroid[0] += cloud_iterator->x; centroid[1] += cloud_iterator->y; centroid[2] += cloud_iterator->z; ++cp; ++cloud_iterator; } centroid[3] = 0; centroid /= static_cast<Scalar> (cp); return (cp); }
template <typename PointSource, typename PointTarget, typename Scalar> inline void pcl::registration::TransformationEstimationTranslation<PointSource, PointTarget, Scalar>::estimateRigidTransformation ( ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const { source_it.reset (); target_it.reset (); // <cloud_src,cloud_src> is the source dataset transformation_matrix.setIdentity (); Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt; // Estimate the centroids of source, target compute3DCentroid (source_it, centroid_src); compute3DCentroid (target_it, centroid_tgt); source_it.reset (); target_it.reset (); getTransformationFromCorrelation (centroid_src, centroid_tgt, transformation_matrix); }
template <typename PointSource, typename PointTarget, typename Scalar> inline void pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation ( ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const { source_it.reset (); target_it.reset (); Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt; // Estimate the centroids of source, target compute3DCentroid (source_it, centroid_src); compute3DCentroid (target_it, centroid_tgt); source_it.reset (); target_it.reset (); // ignore z component centroid_src[2] = 0.0f; centroid_tgt[2] = 0.0f; // Subtract the centroids from source, target Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean; demeanPointCloud (source_it, centroid_src, cloud_src_demean); demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean); getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); }
template <typename PointSource, typename PointTarget, typename Scalar> inline void pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>:: estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, typename std::vector<Scalar>::const_iterator& weights_it, Matrix4 &transformation_matrix) const { typedef Eigen::Matrix<double, 6, 1> Vector6d; typedef Eigen::Matrix<double, 6, 6> Matrix6d; Matrix6d ATA; Vector6d ATb; ATA.setZero (); ATb.setZero (); while (source_it.isValid () && target_it.isValid ()) { if (!pcl_isfinite (source_it->x) || !pcl_isfinite (source_it->y) || !pcl_isfinite (source_it->z) || !pcl_isfinite (target_it->x) || !pcl_isfinite (target_it->y) || !pcl_isfinite (target_it->z) || !pcl_isfinite (target_it->normal_x) || !pcl_isfinite (target_it->normal_y) || !pcl_isfinite (target_it->normal_z)) { ++ source_it; ++ target_it; ++ weights_it; continue; } const float & sx = source_it->x; const float & sy = source_it->y; const float & sz = source_it->z; const float & dx = target_it->x; const float & dy = target_it->y; const float & dz = target_it->z; const float & nx = target_it->normal[0] * (*weights_it); const float & ny = target_it->normal[1] * (*weights_it); const float & nz = target_it->normal[2] * (*weights_it); double a = nz*sy - ny*sz; double b = nx*sz - nz*sx; double c = ny*sx - nx*sy; // 0 1 2 3 4 5 // 6 7 8 9 10 11 // 12 13 14 15 16 17 // 18 19 20 21 22 23 // 24 25 26 27 28 29 // 30 31 32 33 34 35 ATA.coeffRef (0) += a * a; ATA.coeffRef (1) += a * b; ATA.coeffRef (2) += a * c; ATA.coeffRef (3) += a * nx; ATA.coeffRef (4) += a * ny; ATA.coeffRef (5) += a * nz; ATA.coeffRef (7) += b * b; ATA.coeffRef (8) += b * c; ATA.coeffRef (9) += b * nx; ATA.coeffRef (10) += b * ny; ATA.coeffRef (11) += b * nz; ATA.coeffRef (14) += c * c; ATA.coeffRef (15) += c * nx; ATA.coeffRef (16) += c * ny; ATA.coeffRef (17) += c * nz; ATA.coeffRef (21) += nx * nx; ATA.coeffRef (22) += nx * ny; ATA.coeffRef (23) += nx * nz; ATA.coeffRef (28) += ny * ny; ATA.coeffRef (29) += ny * nz; ATA.coeffRef (35) += nz * nz; double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz; ATb.coeffRef (0) += a * d; ATb.coeffRef (1) += b * d; ATb.coeffRef (2) += c * d; ATb.coeffRef (3) += nx * d; ATb.coeffRef (4) += ny * d; ATb.coeffRef (5) += nz * d; ++ source_it; ++ target_it; ++ weights_it; } ATA.coeffRef (6) = ATA.coeff (1); ATA.coeffRef (12) = ATA.coeff (2); ATA.coeffRef (13) = ATA.coeff (8); ATA.coeffRef (18) = ATA.coeff (3); ATA.coeffRef (19) = ATA.coeff (9); ATA.coeffRef (20) = ATA.coeff (15); ATA.coeffRef (24) = ATA.coeff (4); ATA.coeffRef (25) = ATA.coeff (10); ATA.coeffRef (26) = ATA.coeff (16); ATA.coeffRef (27) = ATA.coeff (22); ATA.coeffRef (30) = ATA.coeff (5); ATA.coeffRef (31) = ATA.coeff (11); ATA.coeffRef (32) = ATA.coeff (17); ATA.coeffRef (33) = ATA.coeff (23); ATA.coeffRef (34) = ATA.coeff (29); // Solve A*x = b Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb); // Construct the transformation matrix from x constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix); }