diff --git a/test/gtest_icp.cpp b/test/gtest_icp.cpp
index e398677ed23f666d594d9b64ffe0ed7ef47ca8c2..b79c7a07bd4563f5e7388e2f5dd49165e5a13119 100644
--- a/test/gtest_icp.cpp
+++ b/test/gtest_icp.cpp
@@ -28,20 +28,20 @@ using namespace laserscanutils;
const Eigen::Vector2d A = Eigen::Vector2d::Zero();
const Eigen::Vector2d B = (Eigen::Vector2d() << 0, 40).finished();
const Eigen::Vector2d C = (Eigen::Vector2d() << 30, 0).finished();
-const Eigen::Vector2d CB = B-C;
-double AB_ang = atan2((A-B).norm(),(A-C).norm());
+const Eigen::Vector2d CB = B - C;
+double AB_ang = atan2((A - B).norm(), (A - C).norm());
double dist_min = 2;
/* Synthetic scans are created from this simple scenario with three vertical walls:
*
- * B
+ * B
* +
* |\
- * | \
- * | \
- * | \
- * | \
- * | \
+ * | \
+ * | \
+ * | \
+ * | \
+ * | \
* | the \
* | laser \
* | can be \
@@ -51,23 +51,23 @@ double dist_min = 2;
* | \
* +-------------+
* A C
- *
-*/
+ *
+ */
-double pi2pi(const double& _angle)
+double pi2pi(const double &_angle)
{
- double angle = _angle;
- while (angle > M_PI )
- angle -= 2 * M_PI;
- while (angle <= -M_PI)
- angle += 2 * M_PI ;
+ double angle = _angle;
+ while (angle > M_PI)
+ angle -= 2 * M_PI;
+ while (angle <= -M_PI)
+ angle += 2 * M_PI;
- return angle;
+ return angle;
}
-bool insideTriangle(const Eigen::Vector3d& laser_pose)
+bool insideTriangle(const Eigen::Vector3d &laser_pose)
{
- return laser_pose(0) > 0 + dist_min and laser_pose(1) > 0 + dist_min and laser_pose(0) / (C(0)-dist_min/sin(AB_ang)) + laser_pose(1) / (B(1)-dist_min/cos(AB_ang)) < 1;
+ return laser_pose(0) > 0 + dist_min and laser_pose(1) > 0 + dist_min and laser_pose(0) / (C(0) - dist_min / sin(AB_ang)) + laser_pose(1) / (B(1) - dist_min / cos(AB_ang)) < 1;
}
Eigen::Vector3d generateRandomPoseInsideTriangle()
@@ -84,28 +84,28 @@ Eigen::Vector3d generateRandomPoseInsideTriangle()
return pose;
}
-LaserScan simulateScan(const Eigen::Vector3d& laser_pose, const LaserScanParams& params)
+LaserScan simulateScan(const Eigen::Vector3d &laser_pose, const LaserScanParams ¶ms)
{
// assert inside triangle
assert(insideTriangle(laser_pose));
// create scan
- int n_beams = int((params.angle_max_-params.angle_min_)/params.angle_step_)+1;
+ int n_beams = int((params.angle_max_ - params.angle_min_) / params.angle_step_) + 1;
LaserScan scan = LaserScan(std::vector<float>(n_beams, 0));
// compute angle limits for the three segments (in non rotated reference)
- double theta_A = pi2pi(atan2(A(1)-laser_pose(1), A(0)-laser_pose(0)));
- double theta_B = pi2pi(atan2(B(1)-laser_pose(1), B(0)-laser_pose(0)));
- double theta_C = pi2pi(atan2(C(1)-laser_pose(1), C(0)-laser_pose(0)));
+ double theta_A = pi2pi(atan2(A(1) - laser_pose(1), A(0) - laser_pose(0)));
+ double theta_B = pi2pi(atan2(B(1) - laser_pose(1), B(0) - laser_pose(0)));
+ double theta_C = pi2pi(atan2(C(1) - laser_pose(1), C(0) - laser_pose(0)));
// std::cout << "theta_A: " << theta_A << std::endl;
// std::cout << "theta_B: " << theta_B << std::endl;
// std::cout << "theta_C: " << theta_C << std::endl;
// compute distance to diagonal via area of parallelogram
- double d_p_BC = (Eigen::Matrix2d() << CB(0), laser_pose(0)-C(0), CB(1), laser_pose(1)-C(1)).finished().determinant() / CB.norm();
+ double d_p_BC = (Eigen::Matrix2d() << CB(0), laser_pose(0) - C(0), CB(1), laser_pose(1) - C(1)).finished().determinant() / CB.norm();
// double d_p_BC = (CB.cross(Eigen::Vector2d(laser_pose.head<2>()-C))).norm() / CB.norm();
double theta = params.angle_min_ + laser_pose(2);
- for (auto i = 0; i < n_beams; i++, theta+=params.angle_step_)
+ for (auto i = 0; i < n_beams; i++, theta += params.angle_step_)
{
// Ensure theta in (-M_PI, M_PI]
theta = pi2pi(theta);
@@ -127,7 +127,7 @@ LaserScan simulateScan(const Eigen::Vector3d& laser_pose, const LaserScanParams&
else if (theta > theta_A and theta < theta_C)
{
// std::cout << "A-C segment" << std::endl;
- scan.ranges_raw_[i] = laser_pose(1) / cos(theta + M_PI/2);
+ scan.ranges_raw_[i] = laser_pose(1) / cos(theta + M_PI / 2);
}
else
throw std::runtime_error("bad theta angle..!");
@@ -146,23 +146,22 @@ LaserScan simulateScan(const Eigen::Vector3d& laser_pose, const LaserScanParams&
return scan;
};
-void generateRandomProblem(Eigen::Vector3d& pose_ref,
- Eigen::Vector3d& pose_tar,
- Eigen::Vector3d& pose_d,
- const LaserScanParams& scan_params,
- LaserScan& scan_ref,
- LaserScan& scan_tar,
- double perturbation = 1)
+void generateRandomProblem(Eigen::Vector3d &pose_ref,
+ Eigen::Vector3d &pose_tar,
+ Eigen::Vector3d &pose_d,
+ const LaserScanParams &scan_params,
+ LaserScan &scan_ref,
+ LaserScan &scan_tar,
+ double magnitude = 1)
{
pose_ref = generateRandomPoseInsideTriangle();
do
{
- pose_d = Eigen::Vector3d::Random() * perturbation;
+ pose_d = Eigen::Vector3d::Random() * magnitude;
pose_d(2) = pi2pi(pose_d(2));
pose_tar.head<2>() = pose_ref.head<2>() + Eigen::Rotation2Dd(pose_ref(2)) * pose_d.head<2>();
pose_tar(2) = pose_ref(2) + pose_d(2);
- }
- while (not insideTriangle(pose_tar));
+ } while (not insideTriangle(pose_tar));
scan_ref = simulateScan(pose_ref, scan_params);
scan_tar = simulateScan(pose_tar, scan_params);
@@ -174,8 +173,8 @@ LaserScanParams generateLaserScanParams(double angle_min_deg, double angle_step_
LaserScanParams scan_params;
scan_params.angle_min_ = angle_min_deg * M_PI / 180;
scan_params.angle_step_ = angle_step_deg * M_PI / 180;
- auto n_ranges = int (2*M_PI / scan_params.angle_step_);
- scan_params.angle_max_ = scan_params.angle_min_ + (n_ranges-1) * scan_params.angle_step_;
+ auto n_ranges = int(2 * M_PI / scan_params.angle_step_);
+ scan_params.angle_max_ = scan_params.angle_min_ + (n_ranges - 1) * scan_params.angle_step_;
scan_params.scan_time_ = 0;
scan_params.range_min_ = 0;
scan_params.range_max_ = 1e2;
@@ -185,7 +184,6 @@ LaserScanParams generateLaserScanParams(double angle_min_deg, double angle_step_
return scan_params;
}
-
///////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////// TESTS ////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
@@ -193,7 +191,7 @@ LaserScanParams generateLaserScanParams(double angle_min_deg, double angle_step_
TEST(TestIcp, TestSimulateScan)
{
// 4 beams: 0º, 90º, 180ª, 270ª
- LaserScanParams scan_params = generateLaserScanParams(0,90);
+ LaserScanParams scan_params = generateLaserScanParams(0, 90);
Eigen::Vector3d laser_pose;
laser_pose << C(0) / 4, B(1) / 4, 0;
@@ -209,7 +207,7 @@ TEST(TestIcp, TestSimulateScan)
ASSERT_NEAR(scan1.ranges_raw_[3], laser_pose(1), 1e-9);
// theta = 90ª
- laser_pose(2) = M_PI/2;
+ laser_pose(2) = M_PI / 2;
std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
auto scan2 = simulateScan(laser_pose, scan_params);
@@ -229,7 +227,7 @@ TEST(TestIcp, TestSimulateScan)
ASSERT_NEAR(scan3.ranges_raw_[3], B(1) / 2, 1e-9);
// theta = 270ª
- laser_pose(2) = 3*M_PI/2;
+ laser_pose(2) = 3 * M_PI / 2;
std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
auto scan4 = simulateScan(laser_pose, scan_params);
@@ -242,14 +240,14 @@ TEST(TestIcp, TestSimulateScan)
TEST(TestIcp, TestGenerateRandomPose)
{
// 0-2M_PI (5 degrees step)
- LaserScanParams scan_params = generateLaserScanParams(-180,1);
+ LaserScanParams scan_params = generateLaserScanParams(-180, 1);
// 100 random poses and scans
- for (auto i=0; i < 100; i++)
+ for (auto i = 0; i < 100; i++)
{
auto laser_pose = generateRandomPoseInsideTriangle();
ASSERT_TRUE(insideTriangle(laser_pose));
-
+
auto scan = simulateScan(laser_pose, scan_params);
}
}
@@ -257,9 +255,9 @@ TEST(TestIcp, TestGenerateRandomPose)
TEST(TestIcp, TestIcpSame1)
{
// -180,180 (1 degrees step)
- LaserScanParams scan_params = generateLaserScanParams(-180,1);
+ LaserScanParams scan_params = generateLaserScanParams(-180, 1);
- for (auto i = 0; i<10; i++)
+ for (auto i = 0; i < 10; i++)
{
auto pose = generateRandomPoseInsideTriangle();
auto scan = simulateScan(pose, scan_params);
@@ -270,9 +268,9 @@ TEST(TestIcp, TestIcpSame1)
// no perturbation
std::cout << "//////////// TestIcpSame1: random problem " << i << " pose: " << pose.transpose() << std::endl;
std::cout << "//////////// NO perturbation" << std::endl;
- auto icp_output = ICP::align(scan,
+ auto icp_output = ICP::align(scan,
scan,
- scan_params,
+ scan_params,
icp_params,
Eigen::Vector3d::Zero());
@@ -281,18 +279,18 @@ TEST(TestIcp, TestIcpSame1)
// perturbation
std::cout << "//////////// WITH perturbation" << std::endl;
- icp_output = ICP::align(scan,
+ icp_output = ICP::align(scan,
scan,
- scan_params,
+ scan_params,
icp_params,
- Eigen::Vector3d::Random()*0.1);
+ Eigen::Vector3d::Random() * 0.1);
if (not icp_output.valid)
- icp_output = ICP::align(scan,
+ icp_output = ICP::align(scan,
scan,
- scan_params,
+ scan_params,
icp_params,
- Eigen::Vector3d::Random()*0.1);
+ Eigen::Vector3d::Random() * 0.1);
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
@@ -302,9 +300,9 @@ TEST(TestIcp, TestIcpSame1)
TEST(TestIcp, TestIcpSame2)
{
// 0,360 (1 degrees step)
- LaserScanParams scan_params = generateLaserScanParams(0,1);
+ LaserScanParams scan_params = generateLaserScanParams(0, 1);
- for (auto i = 0; i<10; i++)
+ for (auto i = 0; i < 10; i++)
{
auto pose = generateRandomPoseInsideTriangle();
auto scan = simulateScan(pose, scan_params);
@@ -315,9 +313,9 @@ TEST(TestIcp, TestIcpSame2)
// no perturbation
std::cout << "//////////// TestIcpSame1: random problem " << i << " pose: " << pose.transpose() << std::endl;
std::cout << "//////////// NO perturbation" << std::endl;
- auto icp_output = ICP::align(scan,
+ auto icp_output = ICP::align(scan,
scan,
- scan_params,
+ scan_params,
icp_params,
Eigen::Vector3d::Zero());
@@ -326,18 +324,18 @@ TEST(TestIcp, TestIcpSame2)
// perturbation
std::cout << "//////////// WITH perturbation" << std::endl;
- icp_output = ICP::align(scan,
+ icp_output = ICP::align(scan,
scan,
- scan_params,
+ scan_params,
icp_params,
- Eigen::Vector3d::Random()*0.1);
+ Eigen::Vector3d::Random() * 0.1);
if (not icp_output.valid)
- icp_output = ICP::align(scan,
+ icp_output = ICP::align(scan,
scan,
- scan_params,
+ scan_params,
icp_params,
- Eigen::Vector3d::Random()*0.1);
+ Eigen::Vector3d::Random() * 0.1);
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
@@ -347,67 +345,78 @@ TEST(TestIcp, TestIcpSame2)
TEST(TestIcp, TestIcp1)
{
// 0,360 (1 degrees step)
- LaserScanParams scan_params = generateLaserScanParams(0,1);
+ LaserScanParams scan_params = generateLaserScanParams(0, 1);
- Eigen::Vector3d pose_ref, pose_tar, pose_d;
+ Eigen::Vector3d pose_ref, pose_tar, pose_d, initial_guess;
LaserScan scan_ref, scan_tar;
- for (auto i=0; i < 10; i++)
- {
+ for (auto i = 0; i < 10; i++)
+ {
// Random problem
generateRandomProblem(pose_ref, pose_tar, pose_d, scan_params, scan_ref, scan_tar);
- std::cout << "//////////// TestIcp1: random problem " << i << std::endl;
- std::cout << "\tpose_ref: " << pose_ref.transpose() << std::endl;
- std::cout << "\tpose_tar: " << pose_tar.transpose() << std::endl;
+ initial_guess = pose_d + 0.2 * Eigen::Vector3d::Random();
// icp
auto icp_params = icp_params_default;
- auto icp_output = ICP::align(scan_tar,
+ auto icp_output = ICP::align(scan_tar,
scan_ref,
- scan_params,
+ scan_params,
icp_params,
- //Eigen::Vector3d::Zero());
- pose_d);
-
+ initial_guess);
+
+ std::cout << "\n//////////// TestIcp1: random problem " << i << std::endl;
+ std::cout << " pose_ref: " << pose_ref.transpose() << std::endl;
+ std::cout << " pose_tar: " << pose_tar.transpose() << std::endl;
+ std::cout << " groundtruth: " << pose_d.transpose() << std::endl;
+ std::cout << " initial_guess: " << initial_guess.transpose() << std::endl;
+ std::cout << " icp_output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " icp error: " << icp_output.error << std::endl;
+ std::cout << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
+
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, pose_d, 1e-1);
}
}
-
TEST(TestIcp, TestIcp10)
{
// -180,180 (1 degrees step)
- LaserScanParams scan_params = generateLaserScanParams(-180,1);
+ LaserScanParams scan_params = generateLaserScanParams(-180, 1);
- Eigen::Vector3d pose_ref, pose_tar, pose_d;
+ Eigen::Vector3d pose_ref, pose_tar, pose_d, initial_guess;
LaserScan scan_ref, scan_tar;
- for (auto i=0; i < 10; i++)
- {
+ for (auto i = 0; i < 10; i++)
+ {
// Random problem
generateRandomProblem(pose_ref, pose_tar, pose_d, scan_params, scan_ref, scan_tar, 10);
- std::cout << "//////////// TestIcp1: random problem " << i << std::endl;
- std::cout << "\tpose_ref: " << pose_ref.transpose() << std::endl;
- std::cout << "\tpose_tar: " << pose_tar.transpose() << std::endl;
+ initial_guess = pose_d + 0.2 * Eigen::Vector3d::Random();
// icp
auto icp_params = icp_params_default;
- auto icp_output = ICP::align(scan_tar,
+ auto icp_output = ICP::align(scan_tar,
scan_ref,
- scan_params,
+ scan_params,
icp_params,
- pose_d);
-
+ initial_guess);
+
+ std::cout << "\n//////////// TestIcp1: random problem " << i << std::endl;
+ std::cout << " pose_ref: " << pose_ref.transpose() << std::endl;
+ std::cout << " pose_tar: " << pose_tar.transpose() << std::endl;
+ std::cout << " groundtruth: " << pose_d.transpose() << std::endl;
+ std::cout << " initial_guess: " << initial_guess.transpose() << std::endl;
+ std::cout << " icp_output: " << icp_output.res_transf.transpose() << std::endl;
+ std::cout << " icp error: " << icp_output.error << std::endl;
+ std::cout << " d error: " << (pose_d - icp_output.res_transf).transpose() << std::endl;
+
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, pose_d, 1e-1);
}
-}//*/
-
+} //*/
int main(int argc, char **argv)
{