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

gtest icp working

parent 334ba084
No related branches found
No related tags found
No related merge requests found
Pipeline #9286 failed
Expand all Hide whitespace changes
Inline Side-by-side
src/icp.h
+ 4
− 4
View file @ 6fff2113
......@@ -156,13 +156,13 @@ struct icpParams
const icpParams icp_params_default = {
false, //bool verbose; // prints debug messages
true, 180.0, 10, // bool use_point_to_line_distance; double max_angular_correction_deg; double max_linear_correction;
0.5, true, false, // double max_correspondence_dist; bool use_corr_tricks; bool debug_verify_tricks;
true, 5.0, 1, // bool use_point_to_line_distance; double max_angular_correction_deg; double max_linear_correction;
0.5, false, false, // double max_correspondence_dist; bool use_corr_tricks; bool debug_verify_tricks;
50, 1e-4, 1e-3, // int max_iterations; double epsilon_xy; double epsilon_theta;
false, 0, 0, 0, // bool restart; double restart_threshold_mean_error; double restart_dt; double restart_dtheta;
0.023, 60, // double min_reading, max_reading;
0.9, 0.7, 1.5, false, // double outliers_maxPerc; double outliers_adaptive_order; double outliers_adaptive_mult;
false, false, 10, // bool outliers_remove_doubles; bool do_visibility_test; bool do_alpha_test; double do_alpha_test_thresholdDeg;
1, 0.8, 2, // double outliers_maxPerc; double outliers_adaptive_order; double outliers_adaptive_mult;
false, false, false, 10, // bool outliers_remove_doubles; bool do_visibility_test; bool do_alpha_test; double do_alpha_test_thresholdDeg;
0.5, 4, // double clustering_threshold; int orientation_neighbourhood;
false, false, 0.2, // bool use_ml_weights; bool use_sigma_weights; double sigma;
true, 5 // bool do_compute_covariance; double cov_factor;
......
test/CMakeLists.txt
+ 2
− 1
View file @ 6fff2113
......@@ -8,6 +8,7 @@ include_directories(${GTEST_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(../src)
INCLUDE_DIRECTORIES(/data)
FIND_PACKAGE(Eigen3 3.3 REQUIRED)
FIND_PACKAGE(PythonLibs 3)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
############# USE THIS TEST AS AN EXAMPLE ####################
......@@ -24,7 +25,7 @@ laser_scan_utils_add_gtest(gtest_example gtest_example.cpp)
# gtest icp
IF(csm_FOUND)
laser_scan_utils_add_gtest(gtest_icp gtest_icp.cpp)
target_link_libraries(gtest_icp ${PROJECT_NAME})
target_link_libraries(gtest_icp ${PROJECT_NAME} ${PYTHON_LIBRARIES})
ENDIF(csm_FOUND)
IF(falkolib_FOUND)
......
test/gtest_icp.cpp
+ 234
− 40
View file @ 6fff2113
......@@ -20,10 +20,15 @@
//
//--------LICENSE_END--------
#include "gtest/utils_gtest.h"
#include "laser_scan.h"
#include "icp.h"
#define TEST_PLOTS false
#if TEST_PLOTS
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;
#endif
using namespace laserscanutils;
const Eigen::Vector2d A = Eigen::Vector2d::Zero();
......@@ -31,6 +36,9 @@ 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());
double dist_min = 2;
int color_id=0;
const std::vector<std::string> colors({"b","r","g","c","m","y"});
/* Synthetic scans are created from this simple scenario with three orthogonal walls:
*
......@@ -67,7 +75,7 @@ double pi2pi(const double& _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;
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()
......@@ -79,10 +87,7 @@ Eigen::Vector3d generateRandomPoseInsideTriangle()
pose(2) *= M_PI;
if (not insideTriangle(pose))
{
pose(0) = C(0) - pose(0);
pose(1) = B(1) - pose(1);
}
pose = generateRandomPoseInsideTriangle();
return pose;
}
......@@ -135,6 +140,9 @@ LaserScan simulateScan(const Eigen::Vector3d& laser_pose, const LaserScanParams&
else
throw std::runtime_error("bad theta angle..!");
// assert dist min
assert(scan.ranges_raw_[i] >= dist_min);
// max range
if (scan.ranges_raw_[i] > params.range_max_ or scan.ranges_raw_[i] < params.range_min_)
{
......@@ -160,16 +168,21 @@ void generateRandomProblem(Eigen::Vector3d& pose_ref,
Eigen::Vector3d& pose_d,
const LaserScanParams& scan_params,
LaserScan& scan_ref,
LaserScan& scan_tar)
LaserScan& scan_tar,
double perturbation = 1)
{
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_d = Eigen::Vector3d::Random() * perturbation;
while (pose_d(2) > M_PI)
pose_d(2) -= 2*M_PI;
while (pose_d(2) <= -M_PI)
pose_d(2) += 2*M_PI;
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))
{
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_d = Eigen::Vector3d::Random();
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);
}
......@@ -177,19 +190,77 @@ void generateRandomProblem(Eigen::Vector3d& pose_ref,
scan_tar = simulateScan(pose_tar, scan_params);
}
TEST(TestIcp, TestSimulateScan)
LaserScanParams generateLaserScanParams(double angle_min_deg, double angle_step_deg)
{
// 4 beams: 0º, 90º, 180ª, 270ª
// 360º field of view
LaserScanParams scan_params;
scan_params.angle_min_ = 0;
scan_params.angle_step_ = M_PI / 2;
scan_params.angle_max_ = 3*M_PI/2;
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_;
scan_params.scan_time_ = 0;
scan_params.range_min_ = 0;
scan_params.range_max_ = 1e3;
scan_params.range_max_ = 1e2;
scan_params.range_std_dev_ = 0;
scan_params.angle_std_dev_ = 0;
//scan_params.print();
return scan_params;
}
void initPlot()
{
#if TEST_PLOTS
plt::figure();
#endif
}
void showPlot()
{
#if TEST_PLOTS
plt::show();
#endif
}
void plotScan(const LaserScan& scan, const LaserScanParams& scan_params, const Eigen::Vector3d pose, bool fig_created = false)
{
#if TEST_PLOTS
// Create figure
if (not fig_created)
plt::figure();
plt::axis("scaled");
std::vector<double> x(scan.ranges_raw_.size());
std::vector<double> y(scan.ranges_raw_.size());
for (auto i = 0; i < scan.ranges_raw_.size(); i++)
{
x.at(i) = pose(0) + cos(scan_params.angle_min_ + i*scan_params.angle_step_ + pose(2)) * scan.ranges_raw_.at(i);
y.at(i) = pose(1) + sin(scan_params.angle_min_ + i*scan_params.angle_step_ + pose(2)) * scan.ranges_raw_.at(i);
}
plt::plot(x, y,"."+colors.at(color_id));
std::vector<double> pose_x{pose(0)-sin(pose(2)), pose(0), pose(0)+cos(pose(2))};
std::vector<double> pose_y{pose(1)+cos(pose(2)), pose(1), pose(1)+sin(pose(2))};
plt::plot(pose_x, pose_y,colors.at(color_id)+"-");
if (not fig_created)
plt::show();
color_id++;
if (color_id >= colors.size())
color_id = 0;
#endif
}
///////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////// TESTS ////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
TEST(TestIcp, TestSimulateScan)
{
// 4 beams: 0º, 90º, 180ª, 270ª
LaserScanParams scan_params = generateLaserScanParams(0,90);
Eigen::Vector3d laser_pose;
laser_pose << C(0) / 4, B(1) / 4, 0;
......@@ -237,38 +308,118 @@ TEST(TestIcp, TestSimulateScan)
TEST(TestIcp, TestGenerateRandomPose)
{
// 0-2M_PI (5 degrees step)
LaserScanParams scan_params;
scan_params.angle_min_ = 0;
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;
scan_params.range_max_ = 1e3;
scan_params.range_std_dev_ = 0;
scan_params.angle_std_dev_ = 0;
LaserScanParams scan_params = generateLaserScanParams(-180,1);
initPlot();
// 100 random poses and scans
for (auto i=0; i < 100; i++)
{
auto laser_pose = generateRandomPoseInsideTriangle();
ASSERT_TRUE(insideTriangle(laser_pose));
auto scan = simulateScan(laser_pose, scan_params);
plotScan(scan, scan_params, laser_pose, true);
}
showPlot();
}
TEST(TestIcp, TestIcpSame1)
{
// -180,180 (1 degrees step)
LaserScanParams scan_params = generateLaserScanParams(-180,1);
for (auto i = 0; i<10; i++)
{
auto pose = generateRandomPoseInsideTriangle();
auto scan = simulateScan(pose, scan_params);
// icp
auto icp_params = icp_params_default;
// 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,
scan,
scan_params,
icp_params,
Eigen::Vector3d::Zero());
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
// perturbation
std::cout << "//////////// WITH perturbation" << std::endl;
icp_output = ICP::align(scan,
scan,
scan_params,
icp_params,
Eigen::Vector3d::Random()*0.1);
if (not icp_output.valid)
icp_output = ICP::align(scan,
scan,
scan_params,
icp_params,
Eigen::Vector3d::Random()*0.1);
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
}
}
TEST(TestIcp, TestIcpSame2)
{
// 0,360 (1 degrees step)
LaserScanParams scan_params = generateLaserScanParams(0,1);
for (auto i = 0; i<10; i++)
{
auto pose = generateRandomPoseInsideTriangle();
auto scan = simulateScan(pose, scan_params);
// icp
auto icp_params = icp_params_default;
// 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,
scan,
scan_params,
icp_params,
Eigen::Vector3d::Zero());
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
// perturbation
std::cout << "//////////// WITH perturbation" << std::endl;
icp_output = ICP::align(scan,
scan,
scan_params,
icp_params,
Eigen::Vector3d::Random()*0.1);
if (not icp_output.valid)
icp_output = ICP::align(scan,
scan,
scan_params,
icp_params,
Eigen::Vector3d::Random()*0.1);
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, Eigen::Vector3d::Zero(), 1e-1);
}
}
TEST(TestIcp, TestIcp1)
{
// Scan params
// 0-2M_PI (5 degrees step)
LaserScanParams scan_params;
scan_params.angle_min_ = 0;
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;
scan_params.range_max_ = 1e2;
scan_params.range_std_dev_ = 0;
scan_params.angle_std_dev_ = 0;
// 0,360 (1 degrees step)
LaserScanParams scan_params = generateLaserScanParams(0,1);
Eigen::Vector3d pose_ref, pose_tar, pose_d;
LaserScan scan_ref, scan_tar;
......@@ -278,12 +429,13 @@ TEST(TestIcp, TestIcp1)
// 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;
// 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,
......@@ -293,10 +445,52 @@ TEST(TestIcp, TestIcp1)
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, pose_d, 1e-1);
initPlot();
plotScan(scan_ref,scan_params,pose_ref,true);
plotScan(scan_tar,scan_params,pose_tar,true);
showPlot();
}
}
TEST(TestIcp, TestIcp10)
{
// -180,180 (1 degrees step)
LaserScanParams scan_params = generateLaserScanParams(-180,1);
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, 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;
// icp
auto icp_params = icp_params_default;
auto icp_output = ICP::align(scan_tar,
scan_ref,
scan_params,
icp_params,
pose_d);
ASSERT_TRUE(icp_output.valid);
EXPECT_MATRIX_APPROX(icp_output.res_transf, pose_d, 1e-1);
initPlot();
plotScan(scan_ref,scan_params,pose_ref,true);
plotScan(scan_tar,scan_params,pose_tar,true);
showPlot();
}
}//*/
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
......
test/matplotlibcpp.h 0 → 100644
+ 2986
− 0
View file @ 6fff2113
This diff is collapsed.
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