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

removed bug from kfpcs test and extended it to be successfull in 99.99% #1229

Merged
merged 1 commit into from
May 7, 2015
Merged

removed bug from kfpcs test and extended it to be successfull in 99.99% #1229

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
44 changes: 26 additions & 18 deletions test/registration/test_kfpcs_ia.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,33 +59,41 @@ TEST (PCL, KFPCSInitialAlignment)
cloud_target_ptr = cloud_target.makeShared ();

// initialize k-fpcs
PointCloud <PointXYZI> source_aligned;
PointCloud <PointXYZI> cloud_source_aligned;
KFPCSInitialAlignment <PointXYZI, PointXYZI> kfpcs_ia;
kfpcs_ia.setInputSource (cloud_source_ptr);
kfpcs_ia.setInputTarget (cloud_target_ptr);

kfpcs_ia.setNumberOfThreads (nr_threads);
//kfpcs_ia.setNumberOfThreads (nr_threads);
kfpcs_ia.setApproxOverlap (approx_overlap);
kfpcs_ia.setDelta (voxel_size, false);
kfpcs_ia.setScoreThreshold (abort_score);

// align
kfpcs_ia.align (source_aligned);
EXPECT_EQ (static_cast <int> (source_aligned.points.size ()), static_cast <int> (cloud_source.points.size ()));

// copy initial matrix
Eigen::Matrix4f transform_groundtruth;
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
transform_groundtruth (i, j) = transformation_office1_office2[i][j];

// check for correct transformation
Eigen::Matrix4f transform_rest = kfpcs_ia.getFinalTransformation ().colPivHouseholderQr ().solve (transform_groundtruth);
const float angle3d = Eigen::AngleAxisf (transform_rest.block <3, 3> (0, 0)).angle ();
const float translation3d = transform_rest.block <3, 1> (0, 3).norm ();
// repeat alignment 2 times to increase probability to ~99.99%
const float max_angle3d = 0.1745f, max_translation3d = 1.f;
float angle3d = FLT_MAX, translation3d = FLT_MAX;
for (int i = 0; i < 2; i++)
{
kfpcs_ia.align (cloud_source_aligned);

// copy initial matrix
Eigen::Matrix4f transform_groundtruth;
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
transform_groundtruth (i, j) = transformation_office1_office2[i][j];

// check for correct transformation
Eigen::Matrix4f transform_rest = kfpcs_ia.getFinalTransformation ().colPivHouseholderQr ().solve (transform_groundtruth);
angle3d = std::min (angle3d, Eigen::AngleAxisf (transform_rest.block <3, 3> (0, 0)).angle ());
translation3d = std::min (translation3d, transform_rest.block <3, 1> (0, 3).norm ());

if (angle3d < max_angle3d && translation3d < max_translation3d)
break;
}

EXPECT_NEAR (angle3d, 0.f, 0.1745f); // 10�
EXPECT_NEAR (translation3d, 0.f, 1.f); // 1m
EXPECT_EQ (static_cast <int> (cloud_source_aligned.points.size ()), static_cast <int> (cloud_source.points.size ()));
EXPECT_NEAR (angle3d, 0.f, max_angle3d);
EXPECT_NEAR (translation3d, 0.f, max_translation3d);
}


Expand Down
4 changes: 2 additions & 2 deletions test/registration/test_kfpcs_ia_data.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,12 @@
const int nr_threads = 1;
const float voxel_size = 0.1f;
const float approx_overlap = 0.9f;
const float abort_score = 1.0f;
const float abort_score = 0.0f;

const float transformation_office1_office2 [4][4] = {
{ -0.6946f, -0.7194f, -0.0051f, -3.6352f },
{ 0.7194f, -0.6945f, -0.0100f, -2.3865f },
{ 0.0037f, -0.0106f, -0.9999f, 0.7778f },
{ 0.0037f, -0.0106f, 0.9999f, 0.7778f },
{ 0.0000f, 0.0000f, 0.0000f, 1.0000f }
};

Expand Down