diff --git a/test/gtest_icp.cpp b/test/gtest_icp.cpp
index 53be96da36d22c6d9cbce3ab53f5525ad22d3c79..de1fc3b93f2a1f8c38b78cfc8d562c58632a0baa 100644
--- a/test/gtest_icp.cpp
+++ b/test/gtest_icp.cpp
@@ -26,72 +26,157 @@
using namespace laserscanutils;
-const double By = 20;
-
-// TODO: change to a triangle. Change Inf by C in (Cx, 0)?
+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());
/* Synthetic scans are created from this simple scenario with three orthogonal walls:
*
- * B: (0, By)
- * +------------------------------------------------------------------->> (to infinite)
- * |
- * |
- * | ( the laser can be in this area
- * | with whichever orientation )
- * |
- * |
- * +------------------------------------------------------------------->> (to infinite)
- * A: (0, 0)
+ * B
+ * +
+ * |\
+ * | \
+ * | \
+ * | \
+ * | \
+ * | \
+ * | the \
+ * | laser \
+ * | can be \
+ * | inside \
+ * | this \
+ * | area \
+ * | \
+ * +-------------+
+ * A C
*
*/
+
+double pi2pi(const double& _angle)
+{
+ double angle = _angle;
+ while (angle > M_PI )
+ angle -= 2 * M_PI;
+ while (angle <= -M_PI)
+ angle += 2 * M_PI ;
+
+ return angle;
+}
+
+bool insideTriangle(const Eigen::Vector3d& laser_pose)
+{
+ return laser_pose(0) > 0 and laser_pose(1) > 0 and laser_pose(0) / C(0) + laser_pose(1) / B(1) < 1;
+}
+
+Eigen::Vector3d generateRandomPoseInsideTriangle()
+{
+ Eigen::Vector3d pose;
+ pose = Eigen::Vector3d::Random();
+ pose(0) = (pose(0) + 1.0) * C(0) / 2;
+ pose(1) = (pose(1) + 1.0) * B(1) / 2;
+ pose(2) *= M_PI;
+
+ if (not insideTriangle(pose))
+ {
+ pose(0) = C(0) - pose(0);
+ pose(1) = B(1) - pose(1);
+ }
+
+ return pose;
+}
+
LaserScan simulateScan(const Eigen::Vector3d& laser_pose, const LaserScanParams& params)
{
- assert(laser_pose(0) > 0);
- assert(laser_pose(1) > 0 and laser_pose(1) < By);
+ // assert inside triangle
+ assert(insideTriangle(laser_pose));
// create scan
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_inf=0;
- double theta_A = atan2(-laser_pose(1), -laser_pose(0));
- while (theta_A < 0)
- theta_A += 2*M_PI;
- double theta_B = atan2(By-laser_pose(1), -laser_pose(0));
- while (theta_B < 0)
- theta_B += 2*M_PI;
-
+ 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 = (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_)
{
- // Ensure theta in [0, 2*M_PI)
- while (theta < 0)
- theta += 2*M_PI;
- while (theta >= 2*M_PI)
- theta -= 2*M_PI;
-
- // B-Inf segment
- if (theta <= theta_B)
- //scan.ranges_raw_[i] = 0.0;
- scan.ranges_raw_[i] = (By-laser_pose(1)) / cos(theta - M_PI/2);
+ // Ensure theta in (-M_PI, M_PI]
+ theta = pi2pi(theta);
+ // std::cout << "id " << i << " theta: " << theta << std::endl;
+
+ // B-C segment
+ if (theta <= theta_B and theta >= theta_C)
+ {
+ // std::cout << "B-C segment" << std::endl;
+ scan.ranges_raw_[i] = d_p_BC / sin(theta + AB_ang);
+ }
// A-B segment
- else if (theta > theta_B and theta < theta_A)
+ else if (theta > theta_B or theta <= theta_A)
+ {
+ // std::cout << "A-B segment" << std::endl;
scan.ranges_raw_[i] = laser_pose(0) / cos(theta - M_PI);
- // A-Inf segment
- else if (theta >= theta_A)
- scan.ranges_raw_[i] = laser_pose(1) / cos(theta - 3*M_PI/2);
+ }
+ // A-C segment
+ 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);
+ }
else
throw std::runtime_error("bad theta angle..!");
// max range
if (scan.ranges_raw_[i] > params.range_max_ or scan.ranges_raw_[i] < params.range_min_)
+ {
+ std::cout << "range max reached! " << i << " range: " << scan.ranges_raw_[i] << std::endl;
scan.ranges_raw_[i] = 0.0;
+ }
}
+ // // print ranges
+ // std::cout << "Scan:" << std::endl;
+ // std::cout << " angle_min: " << params.angle_min_ << std::endl;
+ // std::cout << " angle_step_: " << params.angle_step_ << std::endl;
+ // std::cout << " ranges: ";
+ // for (auto range : scan.ranges_raw_)
+ // std::cout << range << ", ";
+ // std::cout << std::endl;
+
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)
+{
+ pose_ref = generateRandomPoseInsideTriangle();
+ pose_d = Eigen::Vector3d::Random() * 0.1;
+ pose_tar.head<2>() = pose_ref.head<2>() + Eigen::Rotation2Dd(pose_d(2)) * pose_d.head<2>();
+ pose_tar(2) = pose_ref(2) + pose_d(2);
+ while (not insideTriangle(pose_tar))
+ {
+ pose_d = Eigen::Vector3d::Random() * 0.1;
+ pose_tar.head<2>() = pose_ref.head<2>() + Eigen::Rotation2Dd(pose_d(2)) * pose_d.head<2>();
+ pose_tar(2) = pose_ref(2) + pose_d(2);
+ }
+
+ scan_ref = simulateScan(pose_ref, scan_params);
+ scan_tar = simulateScan(pose_tar, scan_params);
+}
+
TEST(TestIcp, TestSimulateScan)
{
// 4 beams: 0º, 90º, 180ª, 270ª
@@ -106,15 +191,15 @@ TEST(TestIcp, TestSimulateScan)
scan_params.angle_std_dev_ = 0;
Eigen::Vector3d laser_pose;
- laser_pose << 1, 6, 0;
+ laser_pose << C(0) / 4, B(1) / 4, 0;
// theta = 0
laser_pose(2) = 0;
std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
auto scan1 = simulateScan(laser_pose, scan_params);
- ASSERT_NEAR(scan1.ranges_raw_[0], 0, 1e-9);
- //ASSERT_NEAR(scan1.ranges_raw_[1], By-laser_pose(1), 1e-9);
+ ASSERT_NEAR(scan1.ranges_raw_[0], C(0) / 2, 1e-9);
+ ASSERT_NEAR(scan1.ranges_raw_[1], B(1) / 2, 1e-9);
ASSERT_NEAR(scan1.ranges_raw_[2], laser_pose(0), 1e-9);
ASSERT_NEAR(scan1.ranges_raw_[3], laser_pose(1), 1e-9);
@@ -123,10 +208,10 @@ TEST(TestIcp, TestSimulateScan)
std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
auto scan2 = simulateScan(laser_pose, scan_params);
- //ASSERT_NEAR(scan2.ranges_raw_[0], By-laser_pose(1), 1e-9);
+ ASSERT_NEAR(scan2.ranges_raw_[0], B(1) / 2, 1e-9);
ASSERT_NEAR(scan2.ranges_raw_[1], laser_pose(0), 1e-9);
ASSERT_NEAR(scan2.ranges_raw_[2], laser_pose(1), 1e-9);
- ASSERT_NEAR(scan2.ranges_raw_[3], 0, 1e-9);
+ ASSERT_NEAR(scan2.ranges_raw_[3], C(0) / 2, 1e-9);
// theta = 180ª
laser_pose(2) = M_PI;
@@ -135,8 +220,8 @@ TEST(TestIcp, TestSimulateScan)
ASSERT_NEAR(scan3.ranges_raw_[0], laser_pose(0), 1e-9);
ASSERT_NEAR(scan3.ranges_raw_[1], laser_pose(1), 1e-9);
- ASSERT_NEAR(scan3.ranges_raw_[2], 0, 1e-9);
- //ASSERT_NEAR(scan3.ranges_raw_[3], By-laser_pose(1), 1e-9);
+ ASSERT_NEAR(scan3.ranges_raw_[2], C(0) / 2, 1e-9);
+ ASSERT_NEAR(scan3.ranges_raw_[3], B(1) / 2, 1e-9);
// theta = 270ª
laser_pose(2) = 3*M_PI/2;
@@ -144,14 +229,17 @@ TEST(TestIcp, TestSimulateScan)
auto scan4 = simulateScan(laser_pose, scan_params);
ASSERT_NEAR(scan4.ranges_raw_[0], laser_pose(1), 1e-9);
- ASSERT_NEAR(scan4.ranges_raw_[1], 0, 1e-9);
- //ASSERT_NEAR(scan4.ranges_raw_[2], By-laser_pose(1), 1e-9);
+ ASSERT_NEAR(scan4.ranges_raw_[1], C(0) / 2, 1e-9);
+ ASSERT_NEAR(scan4.ranges_raw_[2], B(1) / 2, 1e-9);
ASSERT_NEAR(scan4.ranges_raw_[3], laser_pose(0), 1e-9);
-
+}
- // 0-2M_PI (10 degrees step)
+TEST(TestIcp, TestGenerateRandomPose)
+{
+ // 0-2M_PI (5 degrees step)
+ LaserScanParams scan_params;
scan_params.angle_min_ = 0;
- scan_params.angle_step_ = 10 * M_PI / 180;
+ scan_params.angle_step_ = 5 * M_PI / 180;
scan_params.angle_max_ = 2*M_PI-scan_params.angle_step_;
scan_params.scan_time_ = 0;
scan_params.range_min_ = 0;
@@ -159,10 +247,13 @@ TEST(TestIcp, TestSimulateScan)
scan_params.range_std_dev_ = 0;
scan_params.angle_std_dev_ = 0;
- laser_pose << 26, 3, 0;
-
- std::cout << "Creating simulated scan with laser pose: " << laser_pose.transpose() << "\n";
- auto scan = simulateScan(laser_pose, scan_params);
+ for (auto i=0; i < 100; i++)
+ {
+ auto laser_pose = generateRandomPoseInsideTriangle();
+ ASSERT_TRUE(insideTriangle(laser_pose));
+
+ auto scan = simulateScan(laser_pose, scan_params);
+ }
}
TEST(TestIcp, TestIcp1)
@@ -179,29 +270,30 @@ TEST(TestIcp, TestIcp1)
scan_params.range_std_dev_ = 0;
scan_params.angle_std_dev_ = 0;
- Eigen::Vector3d pose_ref, pose_tar;
- pose_ref << 2.6, 2, 0;
- pose_tar << 13.0, 12.5, 0;
-
- std::cout << "Creating simulated scan_ref with laser pose: " << pose_ref.transpose() << "\n";
- auto scan_ref = simulateScan(pose_ref, scan_params);
- std::cout << "Creating simulated scan_tar with laser pose: " << pose_tar.transpose() << "\n";
- auto scan_tar = simulateScan(pose_tar, scan_params);
-
- // icp
- auto icp_params = icp_params_default;
- icp_params.max_linear_correction = 20;
- icp_params.print();
-
- icp_params.verbose = true;
- auto icp_output = ICP::align(scan_tar,
- scan_ref,
- scan_params,
- icp_params,
- (Eigen::Vector3d() << 10.4, 10.5, 0).finished());
-
- std::cout << "resulting transformation: " << icp_output.res_transf.transpose() << "\n";
-
+ Eigen::Vector3d pose_ref, pose_tar, pose_d;
+ LaserScan scan_ref, scan_tar;
+
+ for (auto i=0; i < 10; i++)
+ {
+ // Random problem
+ generateRandomProblem(pose_ref, pose_tar, pose_d, scan_params, scan_ref, scan_tar);
+
+ // icp
+ auto icp_params = icp_params_default;
+ //icp_params.max_linear_correction = 20;
+ //icp_params.print();
+
+ icp_params.verbose = true;
+ auto icp_output = ICP::align(scan_tar,
+ scan_ref,
+ scan_params,
+ icp_params,
+ //Eigen::Vector3d::Zero());
+ pose_d);
+
+ ASSERT_TRUE(icp_output.valid);
+ EXPECT_MATRIX_APPROX(icp_output.res_transf, pose_d, 1e-1);
+ }
}