Skip to content

Commit

Permalink
Return correct ICP transformation
Browse files Browse the repository at this point in the history 
* Remove the centering step, which is unnecessary and can cause problems
  when applying the same transformation after the fact

* Add test to recover rotation (5 degrees about Y axis)
  • Loading branch information
@chambbj
chambbj committed Mar 6, 2020
1 parent a5db153 commit df6c50b
Show file tree
Hide file tree
Showing 2 changed files with 57 additions and 42 deletions.
69 changes: 27 additions & 42 deletions filters/IterativeClosestPoint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,23 +105,13 @@ void IterativeClosestPoint::done(PointTableRef _)
PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed,
PointViewPtr moving) const
{
// Compute centroid of fixed PointView such that both the fixed an moving
// PointViews can be centered.
PointIdList ids(fixed->size());
std::iota(ids.begin(), ids.end(), 0);
auto centroid = computeCentroid(*fixed, ids);

// Demean the fixed and moving PointViews.
PointViewPtr tempFixed = demeanPointView(*fixed, centroid.data());
PointViewPtr tempMoving = demeanPointView(*moving, centroid.data());

// Initialize the final_transformation to identity. In the future, it would
// be reasonable to alternately accept an initial guess.
Eigen::Matrix4d final_transformation = Eigen::Matrix4d::Identity();

// Construct 3D KD-tree of the centered, fixed PointView to facilitate

This comment was marked as resolved.

Sign in to view

Copy link
@jomey

jomey Mar 6, 2020

Contributor

The comments starting here should be updated to remove the reference to the initial 'centered' approach.
// nearest neighbor searches in each iteration.
KD3Index& kd_fixed = tempFixed->build3dIndex();
KD3Index& kd_fixed = fixed->build3dIndex();

// Iterate to the max number of iterations or until converged.
bool converged(false);
Expand All @@ -132,7 +122,7 @@ PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed,
// At the beginning of each iteration, transform our centered, moving
// PointView by the current final_transformation.
PointViewPtr tempMovingTransformed =
transform(*tempMoving, final_transformation.data());
transform(*moving, final_transformation.data());

// Create empty lists to hold point correspondences, and initialize MSE
// to zero.
Expand All @@ -144,20 +134,19 @@ PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed,
// For every point in the centered, moving PointView, find the nearest
// neighbor in the centered fixed PointView. Record the indices of each
// and update the MSE.
for (PointId i = 0; i < tempMovingTransformed->size(); ++i)
for (PointRef&& point : *tempMovingTransformed)
{
// Find the index of the nearest neighbor, and the square distance
// between each point.
PointRef p = tempMovingTransformed->point(i);
PointIdList indices(1);
std::vector<double> sqr_dists(1);
kd_fixed.knnSearch(p, 1, &indices, &sqr_dists);
kd_fixed.knnSearch(point, 1, &indices, &sqr_dists);

// In the PCL code, there would've been a check that the square
// distance did not exceed a threshold value.

// Store the indices of the correspondence and update the MSE.
moving_idx.push_back(i);
moving_idx.push_back(point.pointId());
fixed_idx.push_back(indices[0]);
mse += std::sqrt(sqr_dists[0]);
}
Expand All @@ -168,7 +157,7 @@ PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed,

// Estimate rigid transformation using Umeyama method, logging the
// current translation in X and Y.
auto A = pointViewToEigen(*tempFixed, fixed_idx);
auto A = pointViewToEigen(*fixed, fixed_idx);
auto B = pointViewToEigen(*tempMovingTransformed, moving_idx);
auto T = Eigen::umeyama(B.transpose(), A.transpose(), false);
log()->get(LogLevel::Debug2) << "Current dx: " << T.coeff(0, 3) << ", "
Expand Down Expand Up @@ -229,41 +218,37 @@ PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed,
}

// Apply the final_transformation to the moving PointView.
for (PointId i = 0; i < moving->size(); ++i)
for (PointRef&& point : *moving)
{
double x =
moving->getFieldAs<double>(Dimension::Id::X, i) - centroid.x();
double y =
moving->getFieldAs<double>(Dimension::Id::Y, i) - centroid.y();
double z =
moving->getFieldAs<double>(Dimension::Id::Z, i) - centroid.z();
moving->setField(Dimension::Id::X, i,
x * final_transformation.coeff(0, 0) +
y * final_transformation.coeff(0, 1) +
z * final_transformation.coeff(0, 2) +
final_transformation.coeff(0, 3) + centroid.x());
moving->setField(Dimension::Id::Y, i,
x * final_transformation.coeff(1, 0) +
y * final_transformation.coeff(1, 1) +
z * final_transformation.coeff(1, 2) +
final_transformation.coeff(1, 3) + centroid.y());
moving->setField(Dimension::Id::Z, i,
x * final_transformation.coeff(2, 0) +
y * final_transformation.coeff(2, 1) +
z * final_transformation.coeff(2, 2) +
final_transformation.coeff(2, 3) + centroid.z());
double x = point.getFieldAs<double>(Dimension::Id::X);
double y = point.getFieldAs<double>(Dimension::Id::Y);
double z = point.getFieldAs<double>(Dimension::Id::Z);
point.setField(Dimension::Id::X,
x * final_transformation.coeff(0, 0) +
y * final_transformation.coeff(0, 1) +
z * final_transformation.coeff(0, 2) +
final_transformation.coeff(0, 3));
point.setField(Dimension::Id::Y,
x * final_transformation.coeff(1, 0) +
y * final_transformation.coeff(1, 1) +
z * final_transformation.coeff(1, 2) +
final_transformation.coeff(1, 3));
point.setField(Dimension::Id::Z,
x * final_transformation.coeff(2, 0) +
y * final_transformation.coeff(2, 1) +
z * final_transformation.coeff(2, 2) +
final_transformation.coeff(2, 3));
}

// Compute the MSE one last time, using the unaltered, fixed PointView and
// the transformed, moving PointView.
double mse(0.0);
KD3Index& kd_fixed_orig = fixed->build3dIndex();
for (PointId i = 0; i < moving->size(); ++i)
for (PointRef&& point : *moving)
{
PointRef p = moving->point(i);
PointIdList indices(1);
std::vector<double> sqr_dists(1);
kd_fixed_orig.knnSearch(p, 1, &indices, &sqr_dists);
kd_fixed_orig.knnSearch(point, 1, &indices, &sqr_dists);
mse += std::sqrt(sqr_dists[0]);
}
mse /= moving->size();
Expand Down
30 changes: 30 additions & 0 deletions test/unit/filters/IcpFilterTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,6 +136,36 @@ TEST(IcpFilterTest, RecoverTranslation)
checkPointsEqualReader(pointViewSet, tolerance);
}

TEST(IcpFilterTest, RecoverRotation)
{
auto reader1 = newReader();
auto reader2 = newReader();
TransformationFilter transformationFilter;
Options transformationOptions;
transformationOptions.add("matrix", "0.996 0 0.087 0\n0 1 0 0\n-0.087 0 0.996 0\n0 0 0 1");
transformationFilter.setOptions(transformationOptions);
transformationFilter.setInput(*reader2);

auto filter = newFilter();
filter->setInput(*reader1);
filter->setInput(transformationFilter);

PointTable table;
filter->prepare(table);
PointViewSet pointViewSet = filter->execute(table);

MetadataNode root = filter->getMetadata();
Eigen::MatrixXd transform =
root.findChild("transform").value<Eigen::MatrixXd>();
double tolerance = 0.001;
EXPECT_NEAR(0.996, transform(0, 0), tolerance);
EXPECT_NEAR(-0.087, transform(0, 2), tolerance);
EXPECT_NEAR(0.087, transform(2, 0), tolerance);
EXPECT_NEAR(0.996, transform(2, 2), tolerance);
double tolerance2 = 0.5;
checkPointsEqualReader(pointViewSet, tolerance2);
}

TEST(IcpFilterTest, TooFewInputs)
{
auto reader = newReader();
Expand Down

0 comments on commit df6c50b

Please sign in to comment.