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

better gtest

parent a114aebe
No related branches found
No related tags found
No related merge requests found
Pipeline #18416 passed
Hide whitespace changes
Inline Side-by-side
Showing
with 101 additions and 92 deletions
test/gtest_icp.cpp
+ 101
− 92
View file @ 3fbd35e3
......@@ -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 &params)
{
// 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)
{
......
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