Commit b7b75352 authored by James Ward's avatar James Ward
Browse files

Create tests for Optimiser

parent 317573bd
#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_;
......
#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
#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();
}
Supports Markdown
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