diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp index 43079f4c9cb..7e5bf0f8c1b 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -52,9 +52,9 @@ pcl::GeneralizedIterativeClosestPoint::setInputCloud ( } //////////////////////////////////////////////////////////////////////////////////////// -template +template template void -pcl::GeneralizedIterativeClosestPoint::computeCovariances(typename pcl::PointCloud::ConstPtr cloud, +pcl::GeneralizedIterativeClosestPoint::computeCovariances(typename pcl::PointCloud::ConstPtr cloud, const typename pcl::search::KdTree::Ptr kdtree, std::vector >& cloud_covariances) { @@ -86,35 +86,35 @@ pcl::GeneralizedIterativeClosestPoint::computeCovarian // Search for the K nearest neighbours kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq); - + // Find the covariance matrix for(int j = 0; j < k_correspondences_; j++) { const PointT &pt = (*cloud)[nn_indecies[j]]; - + mean[0] += pt.x; mean[1] += pt.y; mean[2] += pt.z; - + cov(0,0) += pt.x*pt.x; - + cov(1,0) += pt.y*pt.x; cov(1,1) += pt.y*pt.y; - + cov(2,0) += pt.z*pt.x; cov(2,1) += pt.z*pt.y; - cov(2,2) += pt.z*pt.z; + cov(2,2) += pt.z*pt.z; } - + mean /= static_cast (k_correspondences_); // Get the actual covariance for (int k = 0; k < 3; k++) - for (int l = 0; l <= k; l++) + for (int l = 0; l <= k; l++) { cov(k,l) /= static_cast (k_correspondences_); cov(k,l) -= mean[k]*mean[l]; cov(l,k) = cov(k,l); } - + // Compute the SVD (covariance matrix is symmetric so U = V') Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU); cov.setZero (); @@ -125,7 +125,7 @@ pcl::GeneralizedIterativeClosestPoint::computeCovarian double v = 1.; // biggest 2 singular values replaced by 1 if(k == 2) // smallest singular value replaced by gicp_epsilon v = gicp_epsilon_; - cov+= v * col * col.transpose(); + cov+= v * col * col.transpose(); } } } @@ -139,11 +139,11 @@ pcl::GeneralizedIterativeClosestPoint::computeRDerivat Eigen::Matrix3d dR_dPsi; double phi = x[3], theta = x[4], psi = x[5]; - + double cphi = cos(phi), sphi = sin(phi); double ctheta = cos(theta), stheta = sin(theta); double cpsi = cos(psi), spsi = sin(psi); - + dR_dPhi(0,0) = 0.; dR_dPhi(1,0) = 0.; dR_dPhi(2,0) = 0.; @@ -179,7 +179,7 @@ pcl::GeneralizedIterativeClosestPoint::computeRDerivat dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta; dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta; dR_dPsi(2,2) = 0.; - + g[3] = matricesInnerProd(dR_dPhi, R); g[4] = matricesInnerProd(dR_dTheta, R); g[5] = matricesInnerProd(dR_dPsi, R); @@ -187,15 +187,15 @@ pcl::GeneralizedIterativeClosestPoint::computeRDerivat //////////////////////////////////////////////////////////////////////////////////////// template void -pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, - const std::vector &indices_src, - const PointCloudTarget &cloud_tgt, - const std::vector &indices_tgt, +pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, + const std::vector &indices_src, + const PointCloudTarget &cloud_tgt, + const std::vector &indices_tgt, Eigen::Matrix4f &transformation_matrix) { if (indices_src.size () < 4) // need at least 4 samples { - PCL_THROW_EXCEPTION (NotEnoughPointsException, + PCL_THROW_EXCEPTION (NotEnoughPointsException, "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!"); return; } @@ -246,7 +246,7 @@ pcl::GeneralizedIterativeClosestPoint::estimateRigidTr applyState(transformation_matrix, x); } else - PCL_THROW_EXCEPTION(SolverDidntConvergeException, + PCL_THROW_EXCEPTION(SolverDidntConvergeException, "[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!"); } @@ -340,7 +340,7 @@ pcl::GeneralizedIterativeClosestPoint::OptimizationFun pp = gicp_->base_transformation_ * p_src; Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]); // Increment rotation gradient - R+= p_src3 * temp.transpose(); + R+= p_src3 * temp.transpose(); } f/= double(m); g.head<3> ()*= double(2.0/m); @@ -367,13 +367,15 @@ pcl::GeneralizedIterativeClosestPoint::computeTransfor if (input_covariances_.empty ()) computeCovariances (input_, tree_reciprocal_, input_covariances_); - base_transformation_ = guess; + base_transformation_ = Eigen::Matrix4f::Identity(); nr_iterations_ = 0; converged_ = false; double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; std::vector nn_indices (1); std::vector nn_dists (1); + pcl::transformPointCloud(output, output, guess); + while(!converged_) { size_t cnt = 0; @@ -392,7 +394,6 @@ pcl::GeneralizedIterativeClosestPoint::computeTransfor for (size_t i = 0; i < N; i++) { PointSource query = output[i]; - query.getVector4fMap () = guess * query.getVector4fMap (); query.getVector4fMap () = transformation_ * query.getVector4fMap (); if (!searchForNeighbors (query, nn_indices, nn_dists)) @@ -400,7 +401,7 @@ pcl::GeneralizedIterativeClosestPoint::computeTransfor 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]); return; } - + // Check if the distance to the nearest neighbor is smaller than the user imposed threshold if (nn_dists[0] < dist_threshold) { @@ -410,7 +411,7 @@ pcl::GeneralizedIterativeClosestPoint::computeTransfor // M = R*C1 M = R * C1; // temp = M*R' + C2 = R*C1*R' + C2 - Eigen::Matrix3d temp = M * R.transpose(); + Eigen::Matrix3d temp = M * R.transpose(); temp+= C2; // M = temp^-1 M = temp.inverse (); @@ -441,7 +442,7 @@ pcl::GeneralizedIterativeClosestPoint::computeTransfor delta = c_delta; } } - } + } catch (PCLException &e) { PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ()); @@ -455,17 +456,11 @@ pcl::GeneralizedIterativeClosestPoint::computeTransfor previous_transformation_ = transformation_; PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n", getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ()); - } + } else PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ()); } - //for some reason the static equivalent methode raises an error - // final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * (guess.block<3,3> (0,0)); - // final_transformation_.block <3, 1> (0, 3) = transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3); - final_transformation_.topLeftCorner (3,3) = previous_transformation_.topLeftCorner (3,3) * guess.topLeftCorner (3,3); - final_transformation_(0,3) = previous_transformation_(0,3) + guess(0,3); - final_transformation_(1,3) = previous_transformation_(1,3) + guess(1,3); - final_transformation_(2,3) = previous_transformation_(2,3) + guess(2,3); + final_transformation_ = previous_transformation_ * guess; // Transform the point cloud pcl::transformPointCloud (*input_, output, final_transformation_); diff --git a/test/registration/test_registration.cpp b/test/registration/test_registration.cpp index c7e1dc69ed0..9d549de8928 100644 --- a/test/registration/test_registration.cpp +++ b/test/registration/test_registration.cpp @@ -235,19 +235,19 @@ TEST (PCL, IterativeClosestPointWithRejectors) Eigen::Affine3f delta_transform; sampleRandomTransform (delta_transform, 0., 0.05); // Sample random global transform for each pair, to make sure we aren't biased around the origin - Eigen::Affine3f net_transform; + Eigen::Affine3f net_transform; sampleRandomTransform (net_transform, 2*M_PI, 10.); - + PointCloud::ConstPtr source (cloud_source.makeShared ()); PointCloud::Ptr source_trans (new PointCloud); PointCloud::Ptr target_trans (new PointCloud); - + pcl::transformPointCloud (*source, *source_trans, delta_transform.inverse () * net_transform); pcl::transformPointCloud (*source, *target_trans, net_transform); reg.setInputSource (source_trans); reg.setInputTarget (target_trans); - + // Register reg.align (cloud_reg); Eigen::Matrix4f trans_final = reg.getFinalTransformation (); @@ -281,7 +281,7 @@ TEST (PCL, JointIterativeClosestPoint) size_t ntransforms = 10; for (size_t t = 0; t < ntransforms; t++) { - + // Sample a fixed offset between cloud pairs Eigen::Affine3f delta_transform; // No rotation, since at a random offset this could make it converge to a wrong (but still reasonable) result @@ -365,7 +365,7 @@ TEST (PCL, IterativeClosestPointNonLinear) EXPECT_EQ (transformation (3, 3), 1); */ EXPECT_LT (reg.getFitnessScore (), 0.001); - + // Check again, for all possible caching schemes for (int iter = 0; iter < 4; iter++) { @@ -381,7 +381,7 @@ TEST (PCL, IterativeClosestPointNonLinear) if (force_cache_reciprocal) tree_recip->setInputCloud (temp_src); reg.setSearchMethodSource (tree_recip, force_cache_reciprocal); - + // Register reg.align (output); EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); @@ -442,13 +442,13 @@ TEST (PCL, IterativeClosestPoint_PointToPlane) if (force_cache_reciprocal) tree_recip->setInputCloud (src); reg.setSearchMethodSource (tree_recip, force_cache_reciprocal); - + // Register reg.align (output); EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); EXPECT_LT (reg.getFitnessScore (), 0.005); } - + // We get different results on 32 vs 64-bit systems. To address this, we've removed the explicit output test @@ -489,7 +489,6 @@ TEST (PCL, GeneralizedIterativeClosestPoint) copyPointCloud (cloud_target, *tgt); PointCloud output; - GeneralizedIterativeClosestPoint reg; reg.setInputSource (src); reg.setInputTarget (tgt); @@ -499,7 +498,7 @@ TEST (PCL, GeneralizedIterativeClosestPoint) // Register reg.align (output); EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); - EXPECT_LT (reg.getFitnessScore (), 0.001); + EXPECT_LT (reg.getFitnessScore (), 0.0001); // Check again, for all possible caching schemes for (int iter = 0; iter < 4; iter++) @@ -516,12 +515,29 @@ TEST (PCL, GeneralizedIterativeClosestPoint) if (force_cache_reciprocal) tree_recip->setInputCloud (src); reg.setSearchMethodSource (tree_recip, force_cache_reciprocal); - + // Register reg.align (output); EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); EXPECT_LT (reg.getFitnessScore (), 0.001); } + + // Test guess matrix + Eigen::Isometry3f transform = Eigen::Isometry3f (Eigen::AngleAxisf (0.25 * M_PI, Eigen::Vector3f::UnitX ()) + * Eigen::AngleAxisf (0.50 * M_PI, Eigen::Vector3f::UnitY ()) + * Eigen::AngleAxisf (0.33 * M_PI, Eigen::Vector3f::UnitZ ())); + transform.translation () = Eigen::Vector3f (0.1, 0.2, 0.3); + PointCloud::Ptr transformed_tgt (new PointCloud); + pcl::transformPointCloud (*tgt, *transformed_tgt, transform.matrix ()); // transformed_tgt is now a copy of tgt with a transformation matrix applied + + GeneralizedIterativeClosestPoint reg_guess; + reg_guess.setInputSource (src); + reg_guess.setInputTarget (transformed_tgt); + reg_guess.setMaximumIterations (50); + reg_guess.setTransformationEpsilon (1e-8); + reg_guess.align (output, transform.matrix ()); + EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); + EXPECT_LT (reg.getFitnessScore (), 0.0001); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -601,7 +617,7 @@ TEST (PCL, NormalDistributionsTransform) reg.align (output); EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); EXPECT_LT (reg.getFitnessScore (), 0.001); - + // Check again, for all possible caching schemes for (int iter = 0; iter < 4; iter++) { @@ -617,7 +633,7 @@ TEST (PCL, NormalDistributionsTransform) if (force_cache_reciprocal) tree_recip->setInputCloud (src); reg.setSearchMethodSource (tree_recip, force_cache_reciprocal); - + // Register reg.align (output); EXPECT_EQ (int (output.points.size ()), int (cloud_source.points.size ())); @@ -683,7 +699,7 @@ TEST (PCL, SampleConsensusInitialAlignment) reg.align (cloud_reg); EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ())); EXPECT_LT (reg.getFitnessScore (), 0.0005); - + // Check again, for all possible caching schemes typedef pcl::PointXYZ PointT; for (int iter = 0; iter < 4; iter++) @@ -700,7 +716,7 @@ TEST (PCL, SampleConsensusInitialAlignment) if (force_cache_reciprocal) tree_recip->setInputCloud (cloud_source_ptr); reg.setSearchMethodSource(tree_recip, force_cache_reciprocal); - + // Register reg.align (cloud_reg); EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ())); @@ -718,7 +734,7 @@ TEST (PCL, SampleConsensusPrerejective) * 1) the number of iterations is increased 1000 --> 5000 * 2) the feature correspondence randomness (the number of kNNs) is decreased 10 --> 2 */ - + // Transform the source cloud by a large amount Eigen::Vector3f initial_offset (100, 0, 0); float angle = static_cast (M_PI) / 2.0f; @@ -760,13 +776,13 @@ TEST (PCL, SampleConsensusPrerejective) fpfh_est.setInputNormals (normals.makeShared ()); fpfh_est.compute (features_target); - // Initialize Sample Consensus Prerejective with 5x the number of iterations and 1/5 feature kNNs as SAC-IA + // Initialize Sample Consensus Prerejective with 5x the number of iterations and 1/5 feature kNNs as SAC-IA SampleConsensusPrerejective reg; reg.setMaxCorrespondenceDistance (0.1); reg.setMaximumIterations (5000); reg.setSimilarityThreshold (0.6f); reg.setCorrespondenceRandomness (2); - + // Set source and target cloud/features reg.setInputSource (cloud_source_ptr); reg.setInputTarget (cloud_target_ptr); @@ -775,12 +791,12 @@ TEST (PCL, SampleConsensusPrerejective) // Register reg.align (cloud_reg); - + // Check output consistency and quality of alignment EXPECT_EQ (static_cast (cloud_reg.points.size ()), static_cast (cloud_source.points.size ())); float inlier_fraction = static_cast (reg.getInliers ().size ()) / static_cast (cloud_source.points.size ()); EXPECT_GT (inlier_fraction, 0.95f); - + // Check again, for all possible caching schemes typedef pcl::PointXYZ PointT; for (int iter = 0; iter < 4; iter++) @@ -797,7 +813,7 @@ TEST (PCL, SampleConsensusPrerejective) if (force_cache_reciprocal) tree_recip->setInputCloud (cloud_source_ptr); reg.setSearchMethodSource(tree_recip, force_cache_reciprocal); - + // Register reg.align (cloud_reg);