Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fixed gicp guess handling #989

Merged
merged 3 commits into from
Aug 23, 2016
Merged

fixed gicp guess handling #989

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
59e6a79
Add test for guess matrix in GICP
VictorLamoine Oct 21, 2014
0b83f2c
transform the target pointcloud and adjust the expected FitnessScore …
saarnold Oct 21, 2014
fac54f0
fixed gicp guess handling
prclibo Nov 11, 2014
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
65 changes: 30 additions & 35 deletions registration/include/pcl/registration/impl/gicp.hpp
View file
Open in desktop
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,
std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >& 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 All @@ -367,13 +367,15 @@ pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransfor
if (input_covariances_.empty ())
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 @@ -392,15 +394,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 @@ -410,7 +411,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 @@ -441,7 +442,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 @@ -455,17 +456,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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<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 @@ -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
Expand Down Expand Up @@ -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++)
{
Expand All @@ -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 ()));
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -489,7 +489,6 @@ TEST (PCL, GeneralizedIterativeClosestPoint)
copyPointCloud (cloud_target, *tgt);
PointCloud<PointT> output;


GeneralizedIterativeClosestPoint<PointT, PointT> reg;
reg.setInputSource (src);
reg.setInputTarget (tgt);
Expand All @@ -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++)
Expand All @@ -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<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 @@ -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++)
{
Expand All @@ -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 ()));
Expand Down Expand Up @@ -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++)
Expand All @@ -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 ()));
Expand All @@ -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<float> (M_PI) / 2.0f;
Expand Down Expand Up @@ -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<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 @@ -775,12 +791,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 @@ -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);

Expand Down