little work on gtest icp

test/gtest/CMakeLists.txt

 include(FetchContent)
 
+SET(BUILD_GMOCK OFF)  # Disable gmock
+SET(INSTALL_GTEST OFF) # Disable installation of googletest
+
 FetchContent_Declare(
   googletest
   GIT_REPOSITORY https://github.com/google/googletest.git 
   GIT_TAG main)
 
-SET(INSTALL_GTEST OFF) # Disable installation of googletest
 FetchContent_MakeAvailable(googletest)
 
 function(add_gtest target)

test/gtest_icp.cpp

@@ -23,12 +23,6 @@
 #include "laser_scan_utils/laser_scan.h"
 #include "laser_scan_utils/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();
@@ -37,10 +31,8 @@ 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:
+/* Synthetic scans are created from this simple scenario with three vertical walls:
  *
  *    B  
  *    +
@@ -151,15 +143,6 @@ LaserScan simulateScan(const Eigen::Vector3d& laser_pose, const LaserScanParams&
     }
   }
 
-  // // 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;
 };
 
@@ -172,19 +155,14 @@ void generateRandomProblem(Eigen::Vector3d& pose_ref,
                            double perturbation = 1)
 {
   pose_ref = generateRandomPoseInsideTriangle();
-  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))
+  do
   {
-    pose_d = Eigen::Vector3d::Random();
+    pose_d = Eigen::Vector3d::Random() * perturbation;
+    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));
 
   scan_ref = simulateScan(pose_ref, scan_params);
   scan_tar = simulateScan(pose_tar, scan_params);
@@ -204,53 +182,9 @@ LaserScanParams generateLaserScanParams(double angle_min_deg, double angle_step_
   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 ////////////////////////////////////////////
@@ -310,8 +244,6 @@ TEST(TestIcp, TestGenerateRandomPose)
   // 0-2M_PI (5 degrees step)
   LaserScanParams scan_params = generateLaserScanParams(-180,1);
 
-  initPlot();
-
   // 100 random poses and scans
   for (auto i=0; i < 100; i++)
   {
@@ -319,11 +251,7 @@ TEST(TestIcp, TestGenerateRandomPose)
     ASSERT_TRUE(insideTriangle(laser_pose));
     
     auto scan = simulateScan(laser_pose, scan_params);
-
-    plotScan(scan, scan_params, laser_pose, true);
   }
-
-  showPlot();
 }
 
 TEST(TestIcp, TestIcpSame1)
@@ -445,11 +373,6 @@ 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();
   }
 }
 
@@ -482,11 +405,6 @@ TEST(TestIcp, TestIcp10)
 
     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();
   }
 }//*/