Skip to content
Snippets Groups Projects
Commit 9aa03c72 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

wip in gtesticp

parent 56677d91
No related branches found
No related tags found
No related merge requests found
Pipeline #8941 failed
Hide whitespace changes
Inline Side-by-side
Showing
with 168 additions and 76 deletions
test/gtest_icp.cpp
+ 168
− 76
View file @ 9aa03c72
......@@ -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);
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment