Create tests for Optimiser

include/cam_lidar_calibration/optimiser.h

 #ifndef optimiser_h_
 #define optimiser_h_
 
+#include <opencv/cv.hpp>
+
+#include <ros/ros.h>
+
+#include "cam_lidar_calibration/load_params.h"
 #include "cam_lidar_calibration/openga.h"
 
 namespace cam_lidar_calibration
@@ -15,6 +20,21 @@ struct Rotation
     return std::string("{") + "roll:" + std::to_string(roll) + ", pitch:" + std::to_string(pitch) +
            ", yaw:" + std::to_string(yaw) + "}";
   }
+  cv::Mat toMat() const
+  {
+    using cv::Mat_;
+    using std::cos;
+    using std::sin;
+
+    cv::Mat R_x = (Mat_<double>(3, 3) << 1, 0, 0, 0, cos(roll), -sin(roll), 0, sin(roll), cos(roll));
+    // Calculate rotation about y axis
+    cv::Mat R_y = (Mat_<double>(3, 3) << cos(pitch), 0, sin(pitch), 0, 1, 0, -sin(pitch), 0, cos(pitch));
+
+    // Calculate rotation about z axis
+    cv::Mat R_z = (Mat_<double>(3, 3) << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1);
+
+    return R_z * R_y * R_x;
+  }
 };
 
 cv::Mat operator*(const Rotation& lhs, const cv::Point3d& rhs);
@@ -33,6 +53,8 @@ struct RotationTranslation
   }
 };
 
+cv::Mat operator*(const RotationTranslation& lhs, const cv::Point3d& rhs);
+
 struct RotationCost  // equivalent to y in matlab
 {
   double objective1;  // This is where the results of simulation is stored but not yet finalized.
@@ -59,7 +81,7 @@ typedef EA::Genetic<RotationTranslation, RotationTranslationCost> GA_Type2;
 class Optimiser
 {
 public:
-  Optimiser();
+  Optimiser(const initial_parameters_t& params);
   ~Optimiser() = default;
 
   bool optimise();
@@ -89,6 +111,7 @@ private:
 
   double perpendicularCost(const Rotation& rot);
   double alignmentCost(const Rotation& rot);
+  double reprojectionCost(const RotationTranslation& rot_trans);
 
   cv::Mat camera_normals_;
   cv::Mat camera_centres_;

src/optimiser.cpp

-#include "cam_lidar_calibration/point_xyzir.h"
-
-#include <opencv/cv.hpp>
+#include "cam_lidar_calibration/optimiser.h"
 
-#include <ros/ros.h>
+#include "cam_lidar_calibration/point_xyzir.h"
 
 #include <tf/transform_datatypes.h>
 
-#include "cam_lidar_calibration/calibration_data.h"
-#include "cam_lidar_calibration/load_params.h"
-#include "cam_lidar_calibration/optimiser.h"
-
 namespace cam_lidar_calibration
 {
 double colmap[50][3] = { { 0, 0, 0.5385 },
@@ -67,20 +61,18 @@ void imageProjection(RotationTranslation rot_trans);
 cv::Mat operator*(const Rotation& lhs, const cv::Point3d& rhs)
 {
   using cv::Mat_;
-  using std::cos;
-  using std::sin;
 
   cv::Mat mat = cv::Mat(rhs).reshape(1);
-  cv::Mat R_x = (Mat_<double>(3, 3) << 1, 0, 0, 0, cos(lhs.roll), -sin(lhs.roll), 0, sin(lhs.roll), cos(lhs.roll));
-  // Calculate rotation about y axis
-  cv::Mat R_y = (Mat_<double>(3, 3) << cos(lhs.pitch), 0, sin(lhs.pitch), 0, 1, 0, -sin(lhs.pitch), 0, cos(lhs.pitch));
-
-  // Calculate rotation about z axis
-  cv::Mat R_z = (Mat_<double>(3, 3) << cos(lhs.yaw), -sin(lhs.yaw), 0, sin(lhs.yaw), cos(lhs.yaw), 0, 0, 0, 1);
-
   // Combined rotation matrix
-  cv::Mat retval = R_z * R_y * R_x * mat;
-  return retval;
+  return lhs.toMat() * mat;
+}
+cv::Mat operator*(const RotationTranslation& lhs, const cv::Point3d& rhs)
+{
+  auto rotated = cv::Point3d(lhs.rot * rhs);
+  rotated.x += lhs.x;
+  rotated.y += lhs.y;
+  rotated.z += lhs.z;
+  return cv::Mat(rotated).reshape(1);
 }
 
 void Optimiser::init_genes2(RotationTranslation& p, const std::function<double(void)>& rnd01)
@@ -138,6 +130,46 @@ double Optimiser::alignmentCost(const Rotation& rot)
   return cost;
 }
 
+double Optimiser::reprojectionCost(const RotationTranslation& rot_trans)
+{
+  // TODO - this is currently operating in the lidar frame
+  // The paper uses the camera frame
+  cv::Mat rvec = cv::Mat_<double>::zeros(3, 1);
+  cv::Mat tvec = cv::Mat_<double>::zeros(3, 1);
+
+  double cost = 0;
+  for (auto& sample : samples_)
+  {
+    std::vector<cv::Point3d> cam_centre_3d;
+    std::vector<cv::Point3d> lidar_centre_3d;
+
+    /*cam_centre_3d.push_back(sample.camera_centre);
+    auto lidar_centre_camera_frame = cv::Point3d(rot_trans * sample.lidar_centre);
+    lidar_centre_3d.push_back(lidar_centre_camera_frame); */
+    auto camera_centre_lidar_frame = cv::Point3d(rot_trans * sample.camera_centre);
+    cam_centre_3d.push_back(camera_centre_lidar_frame);
+    lidar_centre_3d.push_back(sample.lidar_centre);
+
+    std::vector<cv::Point2d> cam, lidar;
+    if (i_params.fisheye_model)
+    {
+      cv::fisheye::projectPoints(cam_centre_3d, cam, rvec, tvec, i_params.cameramat, i_params.distcoeff);
+      cv::fisheye::projectPoints(lidar_centre_3d, lidar, rvec, tvec, i_params.cameramat, i_params.distcoeff);
+    }
+    else
+    {
+      cv::projectPoints(cam_centre_3d, rvec, tvec, i_params.cameramat, i_params.distcoeff, cam);
+      cv::projectPoints(lidar_centre_3d, rvec, tvec, i_params.cameramat, i_params.distcoeff, lidar);
+    }
+    double error = cv::norm(cam[0] - lidar[0]);
+    if (error > cost)
+    {
+      cost = error;
+    }
+  }
+  return cost;
+}
+
 bool Optimiser::eval_solution2(const RotationTranslation& p, RotationTranslationCost& c)
 {
   const double& e1 = p.rot.roll;
@@ -191,8 +223,11 @@ bool Optimiser::eval_solution2(const RotationTranslation& p, RotationTranslation
   }
 
   double error_pix = *std::max_element(pixel_error.begin(), pixel_error.end());
+  double repro_cost = reprojectionCost(p);
+  // ROS_INFO_STREAM("Reprojection error - old: " << error_pix << ", new: " << repro_cost);
 
-  c.objective2 = rot_error + var + error_trans + error_pix;
+  // c.objective2 = rot_error + var + error_trans + error_pix;
+  c.objective2 = rot_error + var + error_trans + repro_cost;
 
   if (output2)
   {
@@ -368,6 +403,22 @@ bool Optimiser::optimise()
 {
   auto n = samples_.size();
   ROS_INFO_STREAM("Optimising " << n << " collected samples");
+  for (auto& sample : samples_)
+  {
+    ROS_INFO_STREAM("Sample");
+    ROS_INFO_STREAM("Camera normal:" << sample.camera_normal);
+    ROS_INFO_STREAM("Camera centre:" << sample.camera_centre);
+    for (auto& c : sample.camera_corners)
+    {
+      ROS_INFO_STREAM("Camera corner:" << c);
+    }
+    ROS_INFO_STREAM("Lidar normal:" << sample.lidar_normal);
+    ROS_INFO_STREAM("Lidar centre:" << sample.lidar_centre);
+    for (auto& c : sample.lidar_corners)
+    {
+      ROS_INFO_STREAM("Lidar corner:" << c);
+    }
+  }
   camera_normals_ = cv::Mat(n, 3, CV_64F);
   camera_centres_ = cv::Mat(n, 3, CV_64F);
   lidar_centres_ = cv::Mat(n, 3, CV_64F);
@@ -605,10 +656,7 @@ pcl::PointCloud<pcl::PointXYZIR> organizedPointcloud(pcl::PointCloud<pcl::PointX
   return (organized_pc);
 }
 */
-Optimiser::Optimiser()
+Optimiser::Optimiser(const initial_parameters_t& params) : i_params(params)
 {
-  ros::NodeHandle n;
-
-  loadParams(n, i_params);
 }
 }  // namespace cam_lidar_calibration

test/test_optimiser.cpp

+#include <gtest/gtest.h>
+
+#include "cam_lidar_calibration/load_params.h"
+#include "cam_lidar_calibration/optimiser.h"
+
+using namespace cam_lidar_calibration;
+using namespace cv;
+
+auto createSamples = [](int count) {
+  std::vector<OptimisationSample> samples;
+
+  OptimisationSample s;
+  s.camera_normal = Point3d(-0.553537, -0.0230432, -0.832505);
+  s.camera_centre = Point3d(862.356, 83.6577, 2358.1);
+  s.camera_corners.push_back(Point3d(884.857, -458.79, 2358.16));
+  s.camera_corners.push_back(Point3d(437.634, 277.619, 2635.13));
+  s.camera_corners.push_back(Point3d(1287.08, -110.304, 2081.07));
+  s.camera_corners.push_back(Point3d(839.854, 626.106, 2358.05));
+  s.lidar_normal = Point3d(-0.34838, 0.936304, -0.0443391);
+  s.lidar_centre = Point3d(1502.56, -2137.06, -57.7846);
+  s.lidar_corners.push_back(Point3d(998.856, -2318.25, 73.655));
+  s.lidar_corners.push_back(Point3d(1398.51, -2149.77, 491.443));
+  s.lidar_corners.push_back(Point3d(1998.63, -1957.96, -173.489));
+  s.lidar_corners.push_back(Point3d(1614.26, -2122.26, -622.747));
+  samples.push_back(s);
+
+  if (count < 2)
+    return samples;
+  s.camera_normal = Point3d(-0.351556, -0.029757, -0.935694);
+  s.camera_centre = Point3d(80.5651, 93.5535, 3419.82);
+  s.camera_corners.push_back(Point3d(107.726, -448.636, 3426.86));
+  s.camera_corners.push_back(Point3d(-397.497, 283.512, 3593.4));
+  s.camera_corners.push_back(Point3d(558.628, -96.4048, 3246.25));
+  s.camera_corners.push_back(Point3d(53.4043, 635.743, 3412.79));
+  s.lidar_normal = Point3d(-0.546148, 0.835692, -0.0577988);
+  s.lidar_centre = Point3d(2811.41, -2125.12, 105.91);
+  s.lidar_corners.push_back(Point3d(2372.3, -2401.89, 253.491));
+  s.lidar_corners.push_back(Point3d(2694.58, -2165.51, 625.87));
+  s.lidar_corners.push_back(Point3d(3286.11, -1822.92, -10.0884));
+  s.lidar_corners.push_back(Point3d(2892.65, -2110.17, -445.608));
+  samples.push_back(s);
+
+  if (count < 3)
+    return samples;
+  s.camera_normal = Point3d(0.77516, -0.0494489, -0.629827);
+  s.camera_centre = Point3d(-2287.59, 137.276, 3153.72);
+  s.camera_corners.push_back(Point3d(-2286.98, -403.913, 3196.96));
+  s.camera_corners.push_back(Point3d(-2603.76, 316.749, 2750.51));
+  s.camera_corners.push_back(Point3d(-1971.42, -42.1969, 3556.94));
+  s.camera_corners.push_back(Point3d(-2288.2, 678.466, 3110.49));
+  s.lidar_normal = Point3d(-0.968748, -0.24062, -0.0602387);
+  s.lidar_centre = Point3d(3966.33, -27.9274, 254.86);
+  s.lidar_corners.push_back(Point3d(4079.01, -531.781, 455.466));
+  s.lidar_corners.push_back(Point3d(3938.84, -55.7691, 808.135));
+  s.lidar_corners.push_back(Point3d(3854.27, 473.949, 52.3765));
+  s.lidar_corners.push_back(Point3d(3993.21, 1.8917, -296.538));
+  samples.push_back(s);
+
+  if (count < 4)
+    return samples;
+  s.camera_normal = Point3d(0.257226, -0.0508405, -0.965013);
+  s.camera_centre = Point3d(-1258.54, 117.407, 3462.74);
+  s.camera_corners.push_back(Point3d(-1237.73, -424.038, 3496.82));
+  s.camera_corners.push_back(Point3d(-1749.51, 301.638, 3322.17));
+  s.camera_corners.push_back(Point3d(-767.567, -66.8243, 3603.32));
+  s.camera_corners.push_back(Point3d(-1279.35, 658.852, 3428.67));
+  s.lidar_normal = Point3d(-0.926893, 0.372328, -0.0473505);
+  s.lidar_centre = Point3d(3632.97, -1055.55, 216.876);
+  s.lidar_corners.push_back(Point3d(3430.47, -1539.84, 372.701));
+  s.lidar_corners.push_back(Point3d(3585.3, -1103.64, 771.808));
+  s.lidar_corners.push_back(Point3d(3833.92, -575.185, 60.4742));
+  s.lidar_corners.push_back(Point3d(3682.18, -1003.55, -337.478));
+  samples.push_back(s);
+  /*
+    s.camera_normal = Point3d();
+    s.camera_centre = Point3d();
+    s.camera_corners.push_back(Point3d());
+    s.camera_corners.push_back(Point3d());
+    s.camera_corners.push_back(Point3d());
+    s.camera_corners.push_back(Point3d());
+    s.lidar_normal = Point3d();
+    s.lidar_centre = Point3d();
+    s.lidar_corners.push_back(Point3d());
+    s.lidar_corners.push_back(Point3d());
+    s.lidar_corners.push_back(Point3d());
+    s.lidar_corners.push_back(Point3d());
+    samples.push_back(s);
+    */
+
+  return samples;
+};
+
+auto createParams = []() {
+  initial_parameters_t params;
+  params.fisheye_model = true;
+  params.cameramat = (Mat_<double>(3, 3) << 1178.654264230649, 0.0, 960.769818463969, 0.0, 1182.0760126922614,
+                      603.9601849872713, 0.0, 0.0, 1.0);
+  params.distcoeff =
+      (Mat_<double>(1, 4) << -0.09405418270319424, 0.08610743090764036, -0.17081566190501285, 0.09818990340541457);
+  return params;
+};
+
+TEST(OptimiserTest, fullRunTest)
+{
+  Optimiser o(createParams());
+  o.samples_ = createSamples(99);
+  o.optimise();
+  EXPECT_TRUE(true);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}