Skip to content

Commit

Permalink
Merge pull request #989 from prclibo/master
Browse files Browse the repository at this point in the history 
fixed gicp guess handling
  • Loading branch information
@SergioRAgostinho
SergioRAgostinho authored Aug 23, 2016
2 parents c874d28 + fac54f0 commit 0c4ae84
Show file tree
Hide file tree
Showing 2 changed files with 68 additions and 57 deletions.
65 changes: 30 additions & 35 deletions registration/include/pcl/registration/impl/gicp.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,9 +52,9 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::setInputCloud (
}

////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget>
template <typename PointSource, typename PointTarget>
template<typename PointT> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
const typename pcl::search::KdTree<PointT>::Ptr kdtree,
MatricesVector& cloud_covariances)
{
Expand Down Expand Up @@ -86,35 +86,35 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::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<double> (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<double> (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<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
cov.setZero ();
Expand All @@ -125,7 +125,7 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::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();
}
}
}
Expand All @@ -139,11 +139,11 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::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.;
Expand Down Expand Up @@ -179,23 +179,23 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::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);
}

////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src,
const std::vector<int> &indices_src,
const PointCloudTarget &cloud_tgt,
const std::vector<int> &indices_tgt,
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::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)
{
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;
}
Expand Down Expand Up @@ -246,7 +246,7 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTr
applyState(transformation_matrix, x);
}
else
PCL_THROW_EXCEPTION(SolverDidntConvergeException,
PCL_THROW_EXCEPTION(SolverDidntConvergeException,
"[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
}

Expand Down Expand Up @@ -340,7 +340,7 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::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);
Expand Down Expand Up @@ -373,13 +373,15 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransfor
computeCovariances<PointSource> (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<int> nn_indices (1);
std::vector<float> nn_dists (1);

pcl::transformPointCloud(output, output, guess);

while(!converged_)
{
size_t cnt = 0;
Expand All @@ -398,15 +400,14 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::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))
{
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)
{
Expand All @@ -416,7 +417,7 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::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 ();
Expand Down Expand Up @@ -447,7 +448,7 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransfor
delta = c_delta;
}
}
}
}
catch (PCLException &e)
{
PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ());
Expand All @@ -461,17 +462,11 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::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_);
Expand Down
60 changes: 38 additions & 22 deletions test/registration/test_registration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,19 +236,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<PointXYZ>::ConstPtr source (cloud_source.makeShared ());
PointCloud<PointXYZ>::Ptr source_trans (new PointCloud<PointXYZ>);
PointCloud<PointXYZ>::Ptr target_trans (new PointCloud<PointXYZ>);

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 ();
Expand Down Expand Up @@ -282,7 +282,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
Expand Down Expand Up @@ -366,7 +366,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++)
{
Expand All @@ -382,7 +382,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 ()));
Expand Down Expand Up @@ -443,13 +443,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
Expand Down Expand Up @@ -490,7 +490,6 @@ TEST (PCL, GeneralizedIterativeClosestPoint)
copyPointCloud (cloud_target, *tgt);
PointCloud<PointT> output;


GeneralizedIterativeClosestPoint<PointT, PointT> reg;
reg.setInputSource (src);
reg.setInputTarget (tgt);
Expand All @@ -500,7 +499,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++)
Expand All @@ -517,12 +516,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<PointT>::Ptr transformed_tgt (new PointCloud<PointT>);
pcl::transformPointCloud (*tgt, *transformed_tgt, transform.matrix ()); // transformed_tgt is now a copy of tgt with a transformation matrix applied

GeneralizedIterativeClosestPoint<PointT, PointT> 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);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -602,7 +618,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++)
{
Expand All @@ -618,7 +634,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 ()));
Expand Down Expand Up @@ -684,7 +700,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++)
Expand All @@ -701,7 +717,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 ()));
Expand All @@ -719,7 +735,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<float> (M_PI) / 2.0f;
Expand Down Expand Up @@ -761,13 +777,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<PointXYZ, PointXYZ, FPFHSignature33> 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);
Expand All @@ -776,12 +792,12 @@ TEST (PCL, SampleConsensusPrerejective)

// Register
reg.align (cloud_reg);

// Check output consistency and quality of alignment
EXPECT_EQ (static_cast<int> (cloud_reg.points.size ()), static_cast<int> (cloud_source.points.size ()));
float inlier_fraction = static_cast<float> (reg.getInliers ().size ()) / static_cast<float> (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++)
Expand All @@ -798,7 +814,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);

Expand Down

0 comments on commit 0c4ae84

Please sign in to comment.