diff --git a/include/kimera-vio/frontend/CameraParams.h b/include/kimera-vio/frontend/CameraParams.h index 70c2e0a63..61cdcd5be 100644 --- a/include/kimera-vio/frontend/CameraParams.h +++ b/include/kimera-vio/frontend/CameraParams.h @@ -57,7 +57,6 @@ class CameraParams : public PipelineParams { body_Pose_cam_(), frame_rate_(), image_size_(), - calibration_(), distortion_model_(), distortion_coeff_(), distortion_coeff_mat_(), @@ -93,7 +92,8 @@ class CameraParams : public PipelineParams { Intrinsics intrinsics_; // OpenCV structures: needed to compute the undistortion map. // 3x3 camera matrix K (last row is {0,0,1}) - cv::Mat K_; + cv::Mat + ; // Sensor extrinsics wrt body-frame gtsam::Pose3 body_Pose_cam_; @@ -102,10 +102,6 @@ class CameraParams : public PipelineParams { double frame_rate_; cv::Size image_size_; - // GTSAM structures to calibrate points: - // Contains intrinsics and distortion parameters. - gtsam::Cal3DS2 calibration_; - // Distortion parameters DistortionModel distortion_model_; std::vector distortion_coeff_; @@ -132,7 +128,7 @@ class CameraParams : public PipelineParams { cv::Mat* distortion_coeffs_mat); static void convertIntrinsicsVectorToMatrix(const Intrinsics& intrinsics, cv::Mat* camera_matrix); - static void createGtsamCalibration(const std::vector& distortion, + static void createGtsamCalibration(const cv::Mat& distortion, const Intrinsics& intrinsics, gtsam::Cal3DS2* calibration); diff --git a/src/frontend/CameraParams.cpp b/src/frontend/CameraParams.cpp index 47a6d4f87..1ae73b0f4 100644 --- a/src/frontend/CameraParams.cpp +++ b/src/frontend/CameraParams.cpp @@ -21,7 +21,6 @@ namespace VIO { -/* -------------------------------------------------------------------------- */ // Parse YAML file describing camera parameters. bool CameraParams::parseYAML(const std::string& filepath) { YamlParser yaml_parser(filepath); @@ -48,25 +47,20 @@ bool CameraParams::parseYAML(const std::string& filepath) { // Convert intrinsics to cv::Mat format. convertIntrinsicsVectorToMatrix(intrinsics_, &K_); - // Create gtsam calibration object. - // Calibration of a camera with radial distortion that also supports - createGtsamCalibration(distortion_coeff_mat_, intrinsics_, &calibration_); - // P_ = R_rectify_ * camera_matrix_; return true; } -/* -------------------------------------------------------------------------- */ void CameraParams::parseDistortion(const YamlParser& yaml_parser) { std::string distortion_model; yaml_parser.getYamlParam("distortion_model", &distortion_model); yaml_parser.getYamlParam("camera_model", &camera_model_); distortion_model_ = stringToDistortion(distortion_model, camera_model_); - // 4 parameters (read from file) CHECK(distortion_model_ == DistortionModel::RADTAN || distortion_model_ == DistortionModel::EQUIDISTANT) << "Unsupported distortion model. Expected: radtan or equidistant."; yaml_parser.getYamlParam("distortion_coefficients", &distortion_coeff_); + CHECK_GE(distortion_coeff_.size(), 4u); convertDistortionVectorToMatrix(distortion_coeff_, &distortion_coeff_mat_); } @@ -107,20 +101,18 @@ const DistortionModel CameraParams::stringToDistortion( } } -/* -------------------------------------------------------------------------- */ // Convert distortion coefficients to OpenCV Format void CameraParams::convertDistortionVectorToMatrix( const std::vector& distortion_coeffs, cv::Mat* distortion_coeffs_mat) { CHECK_NOTNULL(distortion_coeffs_mat); - CHECK_EQ(distortion_coeffs.size(), 4); + CHECK_GE(distortion_coeffs.size(), 4u); *distortion_coeffs_mat = cv::Mat::zeros(1, distortion_coeffs.size(), CV_64F); for (int k = 0; k < distortion_coeffs_mat->cols; k++) { distortion_coeffs_mat->at(0, k) = distortion_coeffs[k]; } } -/* -------------------------------------------------------------------------- */ void CameraParams::parseImgSize(const YamlParser& yaml_parser, cv::Size* image_size) { CHECK_NOTNULL(image_size); @@ -130,7 +122,6 @@ void CameraParams::parseImgSize(const YamlParser& yaml_parser, *image_size = cv::Size(resolution[0], resolution[1]); } -/* -------------------------------------------------------------------------- */ void CameraParams::parseFrameRate(const YamlParser& yaml_parser, double* frame_rate) { CHECK_NOTNULL(frame_rate); @@ -140,22 +131,14 @@ void CameraParams::parseFrameRate(const YamlParser& yaml_parser, *frame_rate = 1 / static_cast(rate); } -/* -------------------------------------------------------------------------- */ void CameraParams::parseBodyPoseCam(const YamlParser& yaml_parser, gtsam::Pose3* body_Pose_cam) { CHECK_NOTNULL(body_Pose_cam); - // int n_rows = 0; - // yaml_parser.getNestedYamlParam("T_BS", "rows", &n_rows); - // CHECK_EQ(n_rows, 4); - // int n_cols = 0; - // yaml_parser.getNestedYamlParam("T_BS", "cols", &n_cols); - // CHECK_EQ(n_cols, 4); std::vector vector_pose; yaml_parser.getNestedYamlParam("T_BS", "data", &vector_pose); *body_Pose_cam = UtilsOpenCV::poseVectorToGtsamPose3(vector_pose); } -/* -------------------------------------------------------------------------- */ void CameraParams::parseCameraIntrinsics(const YamlParser& yaml_parser, Intrinsics* intrinsics_) { CHECK_NOTNULL(intrinsics_); @@ -168,7 +151,6 @@ void CameraParams::parseCameraIntrinsics(const YamlParser& yaml_parser, intrinsics_->begin()); } -/* -------------------------------------------------------------------------- */ void CameraParams::convertIntrinsicsVectorToMatrix(const Intrinsics& intrinsics, cv::Mat* camera_matrix) { CHECK_NOTNULL(camera_matrix); @@ -180,26 +162,27 @@ void CameraParams::convertIntrinsicsVectorToMatrix(const Intrinsics& intrinsics, camera_matrix->at(1, 2) = intrinsics[3]; } -/* -------------------------------------------------------------------------- */ -// TODO(Toni) : Check if equidistant distortion is supported as well in gtsam. -void CameraParams::createGtsamCalibration(const std::vector& distortion, +// TODO(Toni): Check if equidistant distortion is supported as well in gtsam. +// TODO(Toni): rather remove this function as it is only used in tests for +// uncalibrating the keypoints.. Use instead opencv. +void CameraParams::createGtsamCalibration(const cv::Mat& distortion, const Intrinsics& intrinsics, gtsam::Cal3DS2* calibration) { CHECK_NOTNULL(calibration); CHECK_GE(intrinsics.size(), 4); - CHECK_GE(distortion.size(), 4); - *calibration = gtsam::Cal3DS2(intrinsics[0], // fx - intrinsics[1], // fy - 0.0, // skew - intrinsics[2], // u0 - intrinsics[3], // v0 - distortion[0], // k1 - distortion[1], // k2 - distortion[2], // p1 (k3) - distortion[3]); // p2 (k4) + CHECK_GE(distortion.cols, 4); + CHECK_EQ(distortion.rows, 1); + *calibration = gtsam::Cal3DS2(intrinsics[0], // fx + intrinsics[1], // fy + 0.0, // skew + intrinsics[2], // u0 + intrinsics[3], // v0 + distortion.at(0, 0), // k1 + distortion.at(0, 1), // k2 + distortion.at(0, 2), // p1 (k3) + distortion.at(0, 3)); // p2 (k4) } -/* -------------------------------------------------------------------------- */ // Display all params. void CameraParams::print() const { std::stringstream out; @@ -228,27 +211,17 @@ void CameraParams::print() const { << "- distortion_coeff: " << distortion_coeff_mat_ << '\n' << "- R_rectify: " << R_rectify_ << '\n' << "- P: " << P_; - - if (FLAGS_minloglevel < 1) calibration_.print("\n gtsam calibration:\n"); } -/* -------------------------------------------------------------------------- */ // Assert equality up to a tolerance. bool CameraParams::equals(const CameraParams& cam_par, const double& tol) const { - bool areIntrinsicEqual = true; - for (size_t i = 0; i < intrinsics_.size(); i++) { - if (std::fabs(intrinsics_[i] - cam_par.intrinsics_[i]) > tol) { - areIntrinsicEqual = false; - break; - } - } - return camera_id_ == cam_par.camera_id_ && areIntrinsicEqual && + return camera_id_ == cam_par.camera_id_ && + intrinsics_ == cam_par.intrinsics_ && body_Pose_cam_.equals(cam_par.body_Pose_cam_, tol) && - (std::fabs(frame_rate_ - cam_par.frame_rate_) < tol) && - (image_size_.width == cam_par.image_size_.width) && - (image_size_.height == cam_par.image_size_.height) && - calibration_.equals(cam_par.calibration_, tol) && + frame_rate_ == cam_par.frame_rate_ && + image_size_.width == cam_par.image_size_.width && + image_size_.height == cam_par.image_size_.height && UtilsOpenCV::compareCvMatsUpToTol(K_, cam_par.K_) && UtilsOpenCV::compareCvMatsUpToTol(distortion_coeff_mat_, cam_par.distortion_coeff_mat_) && diff --git a/tests/data/ForStereoFrame/sensorLeft5thDistNeg.yaml b/tests/data/ForStereoFrame/sensorLeft5thDistNeg.yaml new file mode 100644 index 000000000..05c37b36b --- /dev/null +++ b/tests/data/ForStereoFrame/sensorLeft5thDistNeg.yaml @@ -0,0 +1,21 @@ +%YAML:1.0 +# General sensor definitions. +camera_id: camera + +# Sensor extrinsics wrt. the body-frame. +T_BS: + cols: 4 + rows: 4 + data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, + 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, + -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, + 0.0, 0.0, 0.0, 1.0] + +# Camera specific definitions. +rate_hz: 20 +resolution: [752, 480] +camera_model: pinhole +intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv +distortion_model: radial-tangential +distortion_coefficients: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, -0.04] + diff --git a/tests/data/ForStereoFrame/sensorLeft5thDistPos.yaml b/tests/data/ForStereoFrame/sensorLeft5thDistPos.yaml new file mode 100644 index 000000000..25162b27b --- /dev/null +++ b/tests/data/ForStereoFrame/sensorLeft5thDistPos.yaml @@ -0,0 +1,21 @@ +%YAML:1.0 +# General sensor definitions. +camera_id: camera + +# Sensor extrinsics wrt. the body-frame. +T_BS: + cols: 4 + rows: 4 + data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, + 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, + -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, + 0.0, 0.0, 0.0, 1.0] + +# Camera specific definitions. +rate_hz: 20 +resolution: [752, 480] +camera_model: pinhole +intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv +distortion_model: radial-tangential +distortion_coefficients: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.4] + diff --git a/tests/data/ForStereoFrame/sensorLeft5thDistZero.yaml b/tests/data/ForStereoFrame/sensorLeft5thDistZero.yaml new file mode 100644 index 000000000..ad2fc3aa2 --- /dev/null +++ b/tests/data/ForStereoFrame/sensorLeft5thDistZero.yaml @@ -0,0 +1,21 @@ +%YAML:1.0 +# General sensor definitions. +camera_id: camera + +# Sensor extrinsics wrt. the body-frame. +T_BS: + cols: 4 + rows: 4 + data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, + 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, + -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, + 0.0, 0.0, 0.0, 1.0] + +# Camera specific definitions. +rate_hz: 20 +resolution: [752, 480] +camera_model: pinhole +intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv +distortion_model: radial-tangential +distortion_coefficients: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] + diff --git a/tests/data/ForStereoFrame/sensorRight5thDistNeg.yaml b/tests/data/ForStereoFrame/sensorRight5thDistNeg.yaml new file mode 100644 index 000000000..e68ade95c --- /dev/null +++ b/tests/data/ForStereoFrame/sensorRight5thDistNeg.yaml @@ -0,0 +1,20 @@ +%YAML:1.0 +# General sensor definitions. +camera_id: camera + +# Sensor extrinsics wrt. the body-frame. +T_BS: + cols: 4 + rows: 4 + data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556, + 0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024, + -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038, + 0.0, 0.0, 0.0, 1.0] + +# Camera specific definitions. +rate_hz: 20 +resolution: [752, 480] +camera_model: pinhole +intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv +distortion_model: radial-tangential +distortion_coefficients: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05, -0.04] diff --git a/tests/data/ForStereoFrame/sensorRight5thDistPos.yaml b/tests/data/ForStereoFrame/sensorRight5thDistPos.yaml new file mode 100644 index 000000000..1bc6cb895 --- /dev/null +++ b/tests/data/ForStereoFrame/sensorRight5thDistPos.yaml @@ -0,0 +1,20 @@ +%YAML:1.0 +# General sensor definitions. +camera_id: camera + +# Sensor extrinsics wrt. the body-frame. +T_BS: + cols: 4 + rows: 4 + data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556, + 0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024, + -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038, + 0.0, 0.0, 0.0, 1.0] + +# Camera specific definitions. +rate_hz: 20 +resolution: [752, 480] +camera_model: pinhole +intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv +distortion_model: radial-tangential +distortion_coefficients: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05, 0.4] diff --git a/tests/data/ForStereoFrame/sensorRight5thDistZero.yaml b/tests/data/ForStereoFrame/sensorRight5thDistZero.yaml new file mode 100644 index 000000000..58eee567c --- /dev/null +++ b/tests/data/ForStereoFrame/sensorRight5thDistZero.yaml @@ -0,0 +1,20 @@ +%YAML:1.0 +# General sensor definitions. +camera_id: camera + +# Sensor extrinsics wrt. the body-frame. +T_BS: + cols: 4 + rows: 4 + data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556, + 0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024, + -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038, + 0.0, 0.0, 0.0, 1.0] + +# Camera specific definitions. +rate_hz: 20 +resolution: [752, 480] +camera_model: pinhole +intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv +distortion_model: radial-tangential +distortion_coefficients: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05, 0.0] diff --git a/tests/testCameraParams.cpp b/tests/testCameraParams.cpp index 4e0015e49..368a4d57b 100644 --- a/tests/testCameraParams.cpp +++ b/tests/testCameraParams.cpp @@ -14,27 +14,23 @@ */ #include -#include -#include #include - -#include +#include #include #include #include +#include +#include +#include + #include "kimera-vio/frontend/CameraParams.h" -#include "kimera-vio/utils/UtilsOpenCV.h" DECLARE_string(test_data_path); -using namespace std; -using namespace gtsam; -using namespace VIO; -using namespace cv; +namespace VIO { -/* ************************************************************************** */ TEST(testCameraParams, parseYAML) { CameraParams cam_params; cam_params.parseYAML(FLAGS_test_data_path + "/sensor.yaml"); @@ -44,55 +40,98 @@ TEST(testCameraParams, parseYAML) { EXPECT_DOUBLE_EQ(frame_rate_expected, cam_params.frame_rate_); // Image size. - const Size size_expected(752, 480); + const cv::Size size_expected(752, 480); EXPECT_EQ(size_expected.width, cam_params.image_size_.width); EXPECT_EQ(size_expected.height, cam_params.image_size_.height); // Intrinsics. - const std::vector intrinsics_expected = {458.654, 457.296, 367.215, - 248.375}; + const std::vector intrinsics_expected = { + 458.654, 457.296, 367.215, 248.375}; for (int c = 0u; c < 4u; c++) { EXPECT_DOUBLE_EQ(intrinsics_expected[c], cam_params.intrinsics_[c]); } - EXPECT_DOUBLE_EQ(intrinsics_expected[0], - cam_params.K_.at(0, 0)); - EXPECT_DOUBLE_EQ(intrinsics_expected[1], - cam_params.K_.at(1, 1)); - EXPECT_DOUBLE_EQ(intrinsics_expected[2], - cam_params.K_.at(0, 2)); - EXPECT_DOUBLE_EQ(intrinsics_expected[3], - cam_params.K_.at(1, 2)); + EXPECT_DOUBLE_EQ(intrinsics_expected[0], cam_params.K_.at(0, 0)); + EXPECT_DOUBLE_EQ(intrinsics_expected[1], cam_params.K_.at(1, 1)); + EXPECT_DOUBLE_EQ(intrinsics_expected[2], cam_params.K_.at(0, 2)); + EXPECT_DOUBLE_EQ(intrinsics_expected[3], cam_params.K_.at(1, 2)); EXPECT_EQ(cam_params.intrinsics_.size(), 4u); - EXPECT_DOUBLE_EQ(intrinsics_expected[0], cam_params.calibration_.fx()); - EXPECT_DOUBLE_EQ(intrinsics_expected[1], cam_params.calibration_.fy()); - EXPECT_DOUBLE_EQ(0u, cam_params.calibration_.skew()); - EXPECT_DOUBLE_EQ(intrinsics_expected[2], cam_params.calibration_.px()); - EXPECT_DOUBLE_EQ(intrinsics_expected[3], cam_params.calibration_.py()); + gtsam::Cal3DS2 gtsam_calib; + CameraParams::createGtsamCalibration(cam_params.distortion_coeff_mat_, + cam_params.intrinsics_, + >sam_calib); + EXPECT_DOUBLE_EQ(intrinsics_expected[0], gtsam_calib.fx()); + EXPECT_DOUBLE_EQ(intrinsics_expected[1], gtsam_calib.fy()); + EXPECT_DOUBLE_EQ(0u, gtsam_calib.skew()); + EXPECT_DOUBLE_EQ(intrinsics_expected[2], gtsam_calib.px()); + EXPECT_DOUBLE_EQ(intrinsics_expected[3], gtsam_calib.py()); // Sensor extrinsics wrt. the body-frame. - Rot3 R_expected(0.0148655429818, -0.999880929698, 0.00414029679422, - 0.999557249008, 0.0149672133247, 0.025715529948, - -0.0257744366974, 0.00375618835797, 0.999660727178); - Point3 T_expected(-0.0216401454975, -0.064676986768, 0.00981073058949); - Pose3 pose_expected(R_expected, T_expected); + gtsam::Rot3 R_expected(0.0148655429818, + -0.999880929698, + 0.00414029679422, + 0.999557249008, + 0.0149672133247, + 0.025715529948, + -0.0257744366974, + 0.00375618835797, + 0.999660727178); + gtsam::Point3 T_expected(-0.0216401454975, -0.064676986768, 0.00981073058949); + gtsam::Pose3 pose_expected(R_expected, T_expected); EXPECT_TRUE(assert_equal(pose_expected, cam_params.body_Pose_cam_)); // Distortion coefficients. - const std::vector distortion_expected = {-0.28340811, 0.07395907, - 0.00019359, 1.76187114e-05}; - for (int c = 0u; c < 4u; c++) { + const std::vector distortion_expected = { + -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05}; + ASSERT_EQ(cam_params.distortion_coeff_mat_.rows, 1u); + ASSERT_EQ(cam_params.distortion_coeff_mat_.cols, 4u); + ASSERT_EQ(cam_params.distortion_coeff_.size(), 4u); + for (int c = 0u; c < distortion_expected.size(); c++) { EXPECT_DOUBLE_EQ(distortion_expected[c], cam_params.distortion_coeff_mat_.at(c)); + EXPECT_DOUBLE_EQ(distortion_expected[c], + cam_params.distortion_coeff_.at(c)); + } + EXPECT_DOUBLE_EQ(distortion_expected[0], gtsam_calib.k1()); + EXPECT_DOUBLE_EQ(distortion_expected[1], gtsam_calib.k2()); + EXPECT_DOUBLE_EQ(distortion_expected[2], gtsam_calib.p1()); + EXPECT_DOUBLE_EQ(distortion_expected[3], gtsam_calib.p2()); +} + +TEST(testCameraParams, convertDistortionVectorToMatrix) { + std::vector distortion_coeffs; + + // 4 distortion params + distortion_coeffs = {1.0, -2.0, 1.3, 10}; + cv::Mat distortion_coeffs_mat; + CameraParams::convertDistortionVectorToMatrix(distortion_coeffs, + &distortion_coeffs_mat); + EXPECT_EQ(distortion_coeffs_mat.cols, distortion_coeffs.size()); + EXPECT_EQ(distortion_coeffs_mat.rows, 1u); + for (size_t i = 0u; i < distortion_coeffs.size(); i++) { + EXPECT_EQ(distortion_coeffs_mat.at(0, i), distortion_coeffs.at(i)); + } + + // 5 distortion params + distortion_coeffs = {1, 1.2f, 3u, 4l, 5.34}; //! randomize types as well + CameraParams::convertDistortionVectorToMatrix(distortion_coeffs, + &distortion_coeffs_mat); + EXPECT_EQ(distortion_coeffs_mat.cols, distortion_coeffs.size()); + EXPECT_EQ(distortion_coeffs_mat.rows, 1u); + for (size_t i = 0u; i < distortion_coeffs.size(); i++) { + EXPECT_EQ(distortion_coeffs_mat.at(0u, i), distortion_coeffs.at(i)); + } + + // n distortion params + distortion_coeffs = {1.0, 1.2, 3.2, 4.3, 5.34, 10203, 1818.9, 1.9}; + CameraParams::convertDistortionVectorToMatrix(distortion_coeffs, + &distortion_coeffs_mat); + EXPECT_EQ(distortion_coeffs_mat.cols, distortion_coeffs.size()); + EXPECT_EQ(distortion_coeffs_mat.rows, 1u); + for (size_t i = 0u; i < distortion_coeffs.size(); i++) { + EXPECT_EQ(distortion_coeffs_mat.at(0u, i), distortion_coeffs.at(i)); } - EXPECT_EQ(cam_params.distortion_coeff_mat_.rows, 1u); - EXPECT_EQ(cam_params.distortion_coeff_mat_.cols, 4u); - EXPECT_DOUBLE_EQ(distortion_expected[0], cam_params.calibration_.k1()); - EXPECT_DOUBLE_EQ(distortion_expected[1], cam_params.calibration_.k2()); - EXPECT_DOUBLE_EQ(distortion_expected[2], cam_params.calibration_.p1()); - EXPECT_DOUBLE_EQ(distortion_expected[3], cam_params.calibration_.p2()); } -/* ************************************************************************** */ TEST(testCameraParams, equals) { CameraParams camParams; camParams.parseYAML(FLAGS_test_data_path + "/sensor.yaml"); @@ -107,56 +146,4 @@ TEST(testCameraParams, equals) { EXPECT_TRUE(camParams.equals(camParams2, 1e-7)); } -/* ************************************************************************** */ -TEST(testCameraParams, Cal3_S2ToCvmat) { - Cal3_S2 K(500, 500, 0.0, 640 / 2, 480 / 2); -} - -/* ************************************************************************** */ -TEST(testCameraParams, DISABLED_parseKITTICalib) { - CameraParams camParams; - // camParams.parseKITTICalib( - // FLAGS_test_data_path + "/ForKittiData/calib_cam_to_cam.txt", - // cv::Mat::eye(3, 3, CV_64F), cv::Mat::zeros(3, 1, CV_64F), "00"); - - // Frame rate - const double frame_rate_expected = 1.0 / 10.0; - EXPECT_DOUBLE_EQ(frame_rate_expected, camParams.frame_rate_); - - // image size - const Size size_expected(1392, 512); - EXPECT_EQ(size_expected.width, camParams.image_size_.width); - EXPECT_EQ(size_expected.height, camParams.image_size_.height); - - // intrinsics - const double intrinsics_expected[] = {984.2439, 980.8141, 690.0, 233.1966}; - for (int c = 0; c < 4; c++) { - EXPECT_DOUBLE_EQ(intrinsics_expected[c], camParams.intrinsics_[c]); - } - - EXPECT_DOUBLE_EQ(intrinsics_expected[0], camParams.calibration_.fx()); - EXPECT_DOUBLE_EQ(intrinsics_expected[1], camParams.calibration_.fy()); - EXPECT_DOUBLE_EQ(0, camParams.calibration_.skew()); - EXPECT_DOUBLE_EQ(intrinsics_expected[2], camParams.calibration_.px()); - EXPECT_DOUBLE_EQ(intrinsics_expected[3], camParams.calibration_.py()); - - // Sensor extrinsics wrt. the body-frame - Rot3 R_expected(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); - Point3 T_expected(2.573699e-16, -1.059758e-16, 1.614870e-16); - Pose3 pose_expected(R_expected, T_expected); - EXPECT_TRUE(assert_equal(pose_expected, camParams.body_Pose_cam_)); - - // distortion coefficients - const double distortion_expected[] = { - -3.728755e-01, 2.037299e-01, 2.219027e-03, 1.383707e-03, -7.233722e-02}; - for (int c = 0; c < 5; c++) { - EXPECT_DOUBLE_EQ(distortion_expected[c], - camParams.distortion_coeff_mat_.at(c)); - } - EXPECT_DOUBLE_EQ(distortion_expected[2], - camParams.distortion_coeff_mat_.at(2)); - EXPECT_DOUBLE_EQ(distortion_expected[0], camParams.calibration_.k1()); - EXPECT_DOUBLE_EQ(distortion_expected[1], camParams.calibration_.k2()); - EXPECT_DOUBLE_EQ(distortion_expected[3], camParams.calibration_.p1()); - EXPECT_DOUBLE_EQ(distortion_expected[4], camParams.calibration_.p2()); -} +} // namespace VIO diff --git a/tests/testFrame.cpp b/tests/testFrame.cpp index 0172e1339..327f2c625 100644 --- a/tests/testFrame.cpp +++ b/tests/testFrame.cpp @@ -57,8 +57,7 @@ TEST(testFrame, ExtractCornersChessboard) { 0, CameraParams(), UtilsOpenCV::ReadAndConvertToGrayScale(chessboardImgName)); - UtilsOpenCV::ExtractCorners(f.img_, - &f.keypoints_); + UtilsOpenCV::ExtractCorners(f.img_, &f.keypoints_); int numCorners_expected = 7 * 9; int numCorners_actual = f.keypoints_.size(); // Assert that there are right number of corners! @@ -67,10 +66,11 @@ TEST(testFrame, ExtractCornersChessboard) { /* ************************************************************************* */ TEST(testFrame, ExtractCornersWhiteBoard) { - Frame f(0, 0, CameraParams(), + Frame f(0, + 0, + CameraParams(), UtilsOpenCV::ReadAndConvertToGrayScale(whitewallImgName)); - UtilsOpenCV::ExtractCorners(f.img_, - &f.keypoints_); + UtilsOpenCV::ExtractCorners(f.img_, &f.keypoints_); int numCorners_expected = 0; int numCorners_actual = f.keypoints_.size(); // Assert that there are no corners! @@ -79,7 +79,9 @@ TEST(testFrame, ExtractCornersWhiteBoard) { /* ------------------------------------------------------------------------- */ TEST(testFrame, getNrValidKeypoints) { - Frame f(0, 0, CameraParams(), + Frame f(0, + 0, + CameraParams(), UtilsOpenCV::ReadAndConvertToGrayScale(chessboardImgName)); const int nrValidExpected = 200; const int outlier_rate = 5; // Insert one outlier every 5 valid landmark ids. @@ -101,8 +103,8 @@ TEST(testFrame, CalibratePixel) { const int numTestCols = 8; // Get the camera parameters - CameraParams camParams; - camParams.parseYAML(sensorPath); + CameraParams cam_params; + cam_params.parseYAML(sensorPath); // Generate the pixels KeypointsCV testPointsCV; @@ -116,16 +118,21 @@ TEST(testFrame, CalibratePixel) { } // Calibrate, and uncalibrate the point, verify that we get the same point for (KeypointsCV::iterator iter = testPointsCV.begin(); - iter != testPointsCV.end(); iter++) { - Vector3 versor = Frame::calibratePixel(*iter, camParams); + iter != testPointsCV.end(); + iter++) { + Vector3 versor = Frame::calibratePixel(*iter, cam_params); ASSERT_DOUBLE_EQ(versor.norm(), 1); // distort the pixel again versor = versor / versor(2); - Point2 uncalibrated_px_actual = - camParams.calibration_.uncalibrate(Point2(versor(0), versor(1))); - Point2 uncalibrated_px_expected = Point2(iter->x, iter->y); - Point2 px_mismatch = uncalibrated_px_actual - uncalibrated_px_expected; + gtsam::Cal3DS2 gtsam_calib; + CameraParams::createGtsamCalibration( + cam_params.distortion_coeff_mat_, cam_params.intrinsics_, >sam_calib); + gtsam::Point2 uncalibrated_px_actual = + gtsam_calib.uncalibrate(gtsam::Point2(versor(0), versor(1))); + gtsam::Point2 uncalibrated_px_expected = gtsam::Point2(iter->x, iter->y); + gtsam::Point2 px_mismatch = + uncalibrated_px_actual - uncalibrated_px_expected; ASSERT_TRUE(px_mismatch.vector().norm() < 0.5); } } @@ -138,8 +145,12 @@ TEST(testFrame, DISABLED_CalibratePixel) { const int numTestCols = 8; // Get the camera parameters - CameraParams camParams; - camParams.parseYAML(sensorPath); + CameraParams cam_params; + cam_params.parseYAML(sensorPath); + + gtsam::Cal3DS2 gtsam_calib; + CameraParams::createGtsamCalibration( + cam_params.distortion_coeff_mat_, cam_params.intrinsics_, >sam_calib); // Generate the pixels KeypointsCV testPointsCV; @@ -148,22 +159,22 @@ TEST(testFrame, DISABLED_CalibratePixel) { for (int r = 0; r < numTestRows; r++) { for (int c = 0; c < numTestCols; c++) { testPointsCV.push_back(KeypointCV(c * imgWidth / (numTestCols - 1), - r * imgHeight / (numTestRows - 1))); + r * imgHeight / (numTestRows - 1))); } } // Calibrate, and uncalibrate the point, verify that we get the same point for (KeypointsCV::iterator iter = testPointsCV.begin(); iter != testPointsCV.end(); iter++) { - Vector3 versor = Frame::calibratePixel(*iter, camParams); + Vector3 versor = Frame::calibratePixel(*iter, cam_params); ASSERT_DOUBLE_EQ(versor.norm(), 1); // distort the pixel again versor = versor / versor(2); Point2 uncalibrated_px_actual = -camParams.calibration_.uncalibrate(Point2(versor(0), versor(1))); Point2 -uncalibrated_px_expected = Point2(iter->x, iter->y); Point2 px_mismatch = -uncalibrated_px_actual - uncalibrated_px_expected; + gtsam_calib.uncalibrate(Point2(versor(0), versor(1))); + Point2 uncalibrated_px_expected = Point2(iter->x, iter->y); + Point2 px_mismatch = uncalibrated_px_actual - uncalibrated_px_expected; ASSERT_TRUE(px_mismatch.vector().norm() < 0.5); } } @@ -172,7 +183,9 @@ uncalibrated_px_actual - uncalibrated_px_expected; /* ************************************************************************* */ TEST(testFrame, findLmkIdFromPixel) { - Frame f(0, 0, CameraParams(), + Frame f(0, + 0, + CameraParams(), UtilsOpenCV::ReadAndConvertToGrayScale(chessboardImgName)); UtilsOpenCV::ExtractCorners(f.img_, &f.keypoints_); for (int i = 0; i < f.keypoints_.size(); i++) { diff --git a/tests/testOpticalFlowPredictor.cpp b/tests/testOpticalFlowPredictor.cpp index 2d8a5f1a4..c6ed7b0c8 100644 --- a/tests/testOpticalFlowPredictor.cpp +++ b/tests/testOpticalFlowPredictor.cpp @@ -49,18 +49,15 @@ class OpticalFlowPredictorFixture : public ::testing::Test { // for large rotations of cam2 wrt cam1 buildSceneLandmarks(&lmks_, 1.0); - // Create cameras calibration from Euroc dataset. - // Euroc dataset with distortion and skewness. - euroc_calib_ = camera_params_.calibration_; - // You could use euroc_calib_ as well, but you'll have similar results // but you need to account for distortion, since the projectSafe function // of GTSAM also uncalibrates the pixels using the distortion params. // Simulated calibration without distortion or skewness, and with the // same fx and fy focal length. - simulated_calib_ = gtsam::Cal3DS2(euroc_calib_.fx(), - euroc_calib_.fx(), + const auto& fx = camera_params_.intrinsics_.at(0); + simulated_calib_ = gtsam::Cal3DS2(fx, + fx, 0.0, camera_params_.image_size_.width / 2u, camera_params_.image_size_.height / 2u, @@ -503,7 +500,7 @@ TEST_F(OpticalFlowPredictorFixture, RotationalOpticalFlowPredictionOutOfImage) { // Create Right Camera //! Cam2 is at 45 degree rotation wrt x axis wrt Cam1: so that landmarks //! remain in front of camera, but outside of image of cam2. - gtsam::Rot3 rot_45_x( 0.924, 0.383, 0.0, 0.0); + gtsam::Rot3 rot_45_x(0.924, 0.383, 0.0, 0.0); gtsam::Pose3 cam_1_P_cam_2(rot_45_x, gtsam::Vector3::Zero()); generateCam2(cam_1_P_cam_2); diff --git a/tests/testStereoFrame.cpp b/tests/testStereoFrame.cpp index 0e67f81e6..11d1db990 100644 --- a/tests/testStereoFrame.cpp +++ b/tests/testStereoFrame.cpp @@ -119,10 +119,70 @@ class StereoFrameFixture : public ::testing::Test { sfnew->sparseStereoMatching(); } + void GenerateStereoFromParams(const string left_param_name, + const string right_param_name) { + cam_params_left.parseYAML(stereo_FLAGS_test_data_path + left_param_name); + cam_params_right.parseYAML(stereo_FLAGS_test_data_path + right_param_name); + + // construct stereo camera + FrontendParams tp; + sf_custom_ = std::make_shared( + id, + timestamp, + UtilsOpenCV::ReadAndConvertToGrayScale( + stereo_FLAGS_test_data_path + left_image_name, + tp.stereo_matching_params_.equalize_image_), + cam_params_left, + UtilsOpenCV::ReadAndConvertToGrayScale( + stereo_FLAGS_test_data_path + right_image_name, + tp.stereo_matching_params_.equalize_image_), + cam_params_right, + tp.stereo_matching_params_); + } + + void GenerateStereoFromParamsAndImgs(const string left_param_name, + const string right_param_name, + const string left_img_name, + const string right_img_name) { + cam_params_left.parseYAML(stereo_FLAGS_test_data_path + left_param_name); + cam_params_right.parseYAML(stereo_FLAGS_test_data_path + right_param_name); + + // construct stereo camera + FrontendParams tp; + sf_custom_ = std::make_shared( + id, + timestamp, + UtilsOpenCV::ReadAndConvertToGrayScale( + stereo_FLAGS_test_data_path + left_image_name, + tp.stereo_matching_params_.equalize_image_), + cam_params_left, + UtilsOpenCV::ReadAndConvertToGrayScale( + stereo_FLAGS_test_data_path + right_image_name, + tp.stereo_matching_params_.equalize_image_), + cam_params_right, + tp.stereo_matching_params_); + } + + // generate keypoints regularly spaced along a grid of size numRows x numRows + void GeneratePointGrid(int num_rows, + int num_cols, + int image_height, + int image_width, + KeypointsCV* keypoints) { + for (int r = 0; r < num_rows; r++) { + for (int c = 0; c < num_cols; c++) { + int y = image_height / (num_rows - 1) * r; + int x = image_width / (num_cols - 1) * c; + keypoints->push_back(cv::Point2f(x, y)); + } + } + } + CameraParams cam_params_left; CameraParams cam_params_right; std::shared_ptr sf; std::shared_ptr sfnew; + std::shared_ptr sf_custom_; cv::Mat left_image_rectified, right_image_rectified; cv::Mat P1, P2; }; @@ -418,6 +478,13 @@ TEST_F(StereoFrameFixture, matchTemplate) { /* ************************************************************************* */ TEST_F(StereoFrameFixture, DistortUnrectifyPoints) { + CHECK(sf); + gtsam::Cal3DS2 gtsam_calib; + CameraParams::createGtsamCalibration( + sf->getLeftFrame().cam_param_.distortion_coeff_mat_, + sf->getLeftFrame().cam_param_.intrinsics_, + >sam_calib); + // Prepare the input data, on a grid! const int numRows = 8; const int numCols = 10; @@ -470,8 +537,7 @@ TEST_F(StereoFrameFixture, DistortUnrectifyPoints) { xn.at(1, 0) / xn.at( 2, 0)); // convert back to pixel (non-homogeneous coordinates) - Point2 pt_expected = - sf->getLeftFrame().cam_param_.calibration_.uncalibrate(pt_in); + Point2 pt_expected = gtsam_calib.uncalibrate(pt_in); // actual value Point2 pt_actual = @@ -488,17 +554,8 @@ TEST_F(StereoFrameFixture, undistortRectifyPoints) { const int img_rows = sf->getLeftFrame().img_.rows; const int img_cols = sf->getLeftFrame().img_.cols; - // generate keypoints regularly spaced along a grid of size numRows x numRows KeypointsCV keypoints_unrectified_gt; - const int numRows = 8; - const int numCols = 10; - for (int r = 0; r < numRows; r++) { - for (int c = 0; c < numCols; c++) { - int y = img_rows / (numRows - 1) * r; - int x = img_cols / (numCols - 1) * c; - keypoints_unrectified_gt.push_back(cv::Point2f(x, y)); - } - } + GeneratePointGrid(8, 10, img_rows, img_cols, &keypoints_unrectified_gt); // Actual value StatusKeypointsCV keypoints_rectified; @@ -527,6 +584,123 @@ TEST_F(StereoFrameFixture, undistortRectifyPoints) { } } +/* ************************************************************************* */ +TEST_F(StereoFrameFixture, undistortRectifyPoints5thDistPos) { + GenerateStereoFromParams("/sensorLeft5thDistPos.yaml", + "/sensorRight5thDistPos.yaml"); + // get image rows and cols + const int img_rows = sf_custom_->getLeftFrame().img_.rows; + const int img_cols = sf_custom_->getLeftFrame().img_.cols; + + // generate keypoints regularly spaced along a grid of size numRows x numRows + KeypointsCV keypoints_unrectified_gt; + GeneratePointGrid(8, 10, img_rows, img_cols, &keypoints_unrectified_gt); + + // Actual value + StatusKeypointsCV keypoints_rectified; + sf_custom_->undistortRectifyPoints(keypoints_unrectified_gt, + sf_custom_->getLeftFrame().cam_param_, + sf_custom_->getLeftUndistRectCamMat(), + &keypoints_rectified); + + // Map back! + KeypointsCV keypoints_unrectified_actual; + std::vector status_unrectified; + tie(keypoints_unrectified_actual, status_unrectified) = + StereoFrame::distortUnrectifyPoints( + keypoints_rectified, + sf_custom_->getLeftFrame().cam_param_.undistort_rectify_map_x_, + sf_custom_->getLeftFrame().cam_param_.undistort_rectify_map_y_); + + // Comparision + for (int i = 0; i < keypoints_unrectified_actual.size(); i++) { + if (keypoints_rectified[i].first != KeypointStatus::VALID) continue; + // compare pixel coordinates of valid points + EXPECT_NEAR( + keypoints_unrectified_actual[i].x, keypoints_unrectified_gt[i].x, 1); + EXPECT_NEAR( + keypoints_unrectified_actual[i].y, keypoints_unrectified_gt[i].y, 1); + } +} + +/* ************************************************************************* */ +TEST_F(StereoFrameFixture, undistortRectifyPoints5thDistNeg) { + GenerateStereoFromParams("/sensorLeft5thDistNeg.yaml", + "/sensorRight5thDistNeg.yaml"); + // get image rows and cols + const int img_rows = sf_custom_->getLeftFrame().img_.rows; + const int img_cols = sf_custom_->getLeftFrame().img_.cols; + + // generate keypoints regularly spaced along a grid of size numRows x numRows + KeypointsCV keypoints_unrectified_gt; + GeneratePointGrid(8, 10, img_rows, img_cols, &keypoints_unrectified_gt); + + // Actual value + StatusKeypointsCV keypoints_rectified; + sf_custom_->undistortRectifyPoints(keypoints_unrectified_gt, + sf_custom_->getLeftFrame().cam_param_, + sf_custom_->getLeftUndistRectCamMat(), + &keypoints_rectified); + + // Map back! + KeypointsCV keypoints_unrectified_actual; + std::vector status_unrectified; + tie(keypoints_unrectified_actual, status_unrectified) = + StereoFrame::distortUnrectifyPoints( + keypoints_rectified, + sf_custom_->getLeftFrame().cam_param_.undistort_rectify_map_x_, + sf_custom_->getLeftFrame().cam_param_.undistort_rectify_map_y_); + + // Comparision + for (int i = 0; i < keypoints_unrectified_actual.size(); i++) { + if (keypoints_rectified[i].first != KeypointStatus::VALID) continue; + // compare pixel coordinates of valid points + EXPECT_NEAR( + keypoints_unrectified_actual[i].x, keypoints_unrectified_gt[i].x, 1); + EXPECT_NEAR( + keypoints_unrectified_actual[i].y, keypoints_unrectified_gt[i].y, 1); + } +} + +/* ************************************************************************* */ +TEST_F(StereoFrameFixture, undistortRectifyPoints5thDistZero) { + GenerateStereoFromParams("/sensorLeft5thDistZero.yaml", + "/sensorRight5thDistZero.yaml"); + // get image rows and cols + const int img_rows = sf_custom_->getLeftFrame().img_.rows; + const int img_cols = sf_custom_->getLeftFrame().img_.cols; + + // generate keypoints regularly spaced along a grid of size numRows x numRows + KeypointsCV keypoints_unrectified_gt; + GeneratePointGrid(8, 10, img_rows, img_cols, &keypoints_unrectified_gt); + + // Actual value + StatusKeypointsCV keypoints_rectified; + sf_custom_->undistortRectifyPoints(keypoints_unrectified_gt, + sf_custom_->getLeftFrame().cam_param_, + sf_custom_->getLeftUndistRectCamMat(), + &keypoints_rectified); + + // Map back! + KeypointsCV keypoints_unrectified_actual; + std::vector status_unrectified; + tie(keypoints_unrectified_actual, status_unrectified) = + StereoFrame::distortUnrectifyPoints( + keypoints_rectified, + sf_custom_->getLeftFrame().cam_param_.undistort_rectify_map_x_, + sf_custom_->getLeftFrame().cam_param_.undistort_rectify_map_y_); + + // Comparision + for (int i = 0; i < keypoints_unrectified_actual.size(); i++) { + if (keypoints_rectified[i].first != KeypointStatus::VALID) continue; + // compare pixel coordinates of valid points + EXPECT_NEAR( + keypoints_unrectified_actual[i].x, keypoints_unrectified_gt[i].x, 1); + EXPECT_NEAR( + keypoints_unrectified_actual[i].y, keypoints_unrectified_gt[i].y, 1); + } +} + /* ************************************************************************* */ TEST_F(StereoFrameFixture, getDepthFromRectifiedMatches) { // depth = fx * b / disparity @@ -759,6 +933,13 @@ TEST_F(StereoFrameFixture, getRightKeypointsRectified) { TEST_F(StereoFrameFixture, sparseStereoMatching) { // create a brand new stereo frame initializeDataStereo(); + CHECK(sfnew); + + gtsam::Cal3DS2 gtsam_calib; + CameraParams::createGtsamCalibration( + sfnew->getLeftFrame().cam_param_.distortion_coeff_mat_, + sfnew->getLeftFrame().cam_param_.intrinsics_, + >sam_calib); ///////////////////////////////////////////////////////////////////////////////////////////////////// // check that data is correctly populated: @@ -829,8 +1010,7 @@ TEST_F(StereoFrameFixture, sparseStereoMatching) { versor_i = versor_i / versor_i(2); // set last element to 1, instead of norm 1 Point2 kp_i_distUnrect_gtsam = - sfnew->getLeftFrame().cam_param_.calibration_.uncalibrate( - Point2(versor_i(0), versor_i(1))); + gtsam_calib.uncalibrate(Point2(versor_i(0), versor_i(1))); EXPECT_TRUE(assert_equal(Point2(kp_i_distUnrect.x, kp_i_distUnrect.y), kp_i_distUnrect_gtsam, 1)); @@ -880,7 +1060,7 @@ TEST_F(StereoFrameFixture, sparseStereoMatching) { // original distorted unrectified point (CHECK BACKPROJECTION WORKS) Point3 point3d_unrect = actual_camL_R_camLrect.rotate( point3d); // compensate for the rotation induced by rectification - Cal3DS2 KdistUnrect = sfnew->getLeftFrame().cam_param_.calibration_; + Cal3DS2 KdistUnrect = gtsam_calib; PinholeCamera leftCam_distUnrect(gtsam::Pose3(), KdistUnrect); Point2 p2_distUnrect = leftCam_distUnrect.project(point3d_unrect); EXPECT_TRUE(assert_equal( diff --git a/tests/testStereoVisionFrontEnd.cpp b/tests/testStereoVisionFrontEnd.cpp index 31c50e866..3d92f5bc9 100644 --- a/tests/testStereoVisionFrontEnd.cpp +++ b/tests/testStereoVisionFrontEnd.cpp @@ -540,9 +540,14 @@ TEST_F(StereoVisionFrontEndFixture, DISABLED_processFirstFrame) { // left_keypoints_rectified! std::vector left_undistort_corners = loadCorners(synthetic_stereo_path + "/corners_undistort_left.txt"); + const CameraParams& left_cam_params = sf.getLeftFrame().cam_param_; + gtsam::Cal3DS2 gtsam_left_cam_calib; + CameraParams::createGtsamCalibration(left_cam_params.distortion_coeff_mat_, + left_cam_params.intrinsics_, + >sam_left_cam_calib); std::vector left_rect_corners = convertCornersAcrossCameras(left_undistort_corners, - sf.getLeftFrame().cam_param_.calibration_, + gtsam_left_cam_calib, sf.getLeftUndistRectCamMat()); for (size_t i = 0u; i < num_corners; i++) { int idx_gt = corner_id_map_frame2gt[i]; @@ -553,11 +558,16 @@ TEST_F(StereoVisionFrontEndFixture, DISABLED_processFirstFrame) { } // right_keypoints_rectified + const CameraParams& right_cam_params = sf.getRightFrame().cam_param_; + gtsam::Cal3DS2 gtsam_right_cam_calib; + CameraParams::createGtsamCalibration(right_cam_params.distortion_coeff_mat_, + right_cam_params.intrinsics_, + >sam_right_cam_calib); std::vector right_undistort_corners = loadCorners(synthetic_stereo_path + "/corners_undistort_right.txt"); std::vector right_rect_corners = convertCornersAcrossCameras(right_undistort_corners, - sf.getRightFrame().cam_param_.calibration_, + gtsam_right_cam_calib, sf.getRightUndistRectCamMat()); for (size_t i = 0u; i < num_corners; i++) { int idx_gt = corner_id_map_frame2gt[i]; diff --git a/tests/testTracker.cpp b/tests/testTracker.cpp index 7b8fca963..f21c51a71 100644 --- a/tests/testTracker.cpp +++ b/tests/testTracker.cpp @@ -204,7 +204,11 @@ class TestTracker : public ::testing::Test { camRef_pose_camCur.inverse().translation(); versor_cur = versor_cur / versor_cur.norm(); - Point2 pt_cur_gtsam = f_ref->cam_param_.calibration_.uncalibrate( + gtsam::Cal3DS2 gtsam_calib; + CameraParams::createGtsamCalibration(f_ref->cam_param_.distortion_coeff_mat_, + f_ref->cam_param_.intrinsics_, + >sam_calib); + Point2 pt_cur_gtsam = gtsam_calib.uncalibrate( Point2(versor_cur[0] / versor_cur[2], versor_cur[1] / versor_cur[2])); KeypointCV pt_cur(pt_cur_gtsam.x(), pt_cur_gtsam.y()); @@ -238,7 +242,11 @@ class TestTracker : public ::testing::Test { camRef_pose_camCur.inverse().translation(); versor_cur = versor_cur / versor_cur.norm(); - Point2 pt_cur_gtsam = f_ref->cam_param_.calibration_.uncalibrate( + gtsam::Cal3DS2 gtsam_calib; + CameraParams::createGtsamCalibration(f_ref->cam_param_.distortion_coeff_mat_, + f_ref->cam_param_.intrinsics_, + >sam_calib); + Point2 pt_cur_gtsam = gtsam_calib.uncalibrate( Point2(versor_cur[0] / versor_cur[2], versor_cur[1] / versor_cur[2])); KeypointCV pt_cur(pt_cur_gtsam.x(), pt_cur_gtsam.y()); @@ -1313,18 +1321,15 @@ TEST_F(TestTracker, MahalanobisDistance) { O(1, 0) * (O(0, 1) * O(2, 2) - O(0, 2) * O(2, 1)) + O(2, 0) * (O(0, 1) * O(1, 2) - O(1, 1) * O(0, 2))); float innovationMahalanobisNorm3 = - dinv * v(0) * - (v(0) * (O(1, 1) * O(2, 2) - O(1, 2) * O(2, 1)) - - v(1) * (O(0, 1) * O(2, 2) - O(0, 2) * O(2, 1)) + - v(2) * (O(0, 1) * O(1, 2) - O(1, 1) * O(0, 2))) + - dinv * v(1) * - (O(0, 0) * (v(1) * O(2, 2) - O(1, 2) * v(2)) - - O(1, 0) * (v(0) * O(2, 2) - O(0, 2) * v(2)) + - O(2, 0) * (v(0) * O(1, 2) - v(1) * O(0, 2))) + - dinv * v(2) * - (O(0, 0) * (O(1, 1) * v(2) - v(1) * O(2, 1)) - - O(1, 0) * (O(0, 1) * v(2) - v(0) * O(2, 1)) + - O(2, 0) * (O(0, 1) * v(1) - O(1, 1) * v(0))); + dinv * v(0) * (v(0) * (O(1, 1) * O(2, 2) - O(1, 2) * O(2, 1)) - + v(1) * (O(0, 1) * O(2, 2) - O(0, 2) * O(2, 1)) + + v(2) * (O(0, 1) * O(1, 2) - O(1, 1) * O(0, 2))) + + dinv * v(1) * (O(0, 0) * (v(1) * O(2, 2) - O(1, 2) * v(2)) - + O(1, 0) * (v(0) * O(2, 2) - O(0, 2) * v(2)) + + O(2, 0) * (v(0) * O(1, 2) - v(1) * O(0, 2))) + + dinv * v(2) * (O(0, 0) * (O(1, 1) * v(2) - v(1) * O(2, 1)) - + O(1, 0) * (O(0, 1) * v(2) - v(0) * O(2, 1)) + + O(2, 0) * (O(0, 1) * v(1) - O(1, 1) * v(0))); time3 += VIO::utils::Timer::toc(timeBefore).count(); EXPECT_NEAR(double(innovationMahalanobisNorm1),