/
| 36 | |
| 37 | /* ************************************************************************** */ |
| 38 | TEST(testCameraParams, parseYAML) { |
| 39 | CameraParams cam_params; |
| 40 | cam_params.parseYAML(FLAGS_test_data_path + "/sensor.yaml"); |
| 41 | |
| 42 | // Frame rate. |
| 43 | const double frame_rate_expected = 1.0 / 20.0; |
| 44 | EXPECT_DOUBLE_EQ(frame_rate_expected, cam_params.frame_rate_); |
| 45 | |
| 46 | // Image size. |
| 47 | const Size size_expected(752, 480); |
| 48 | EXPECT_EQ(size_expected.width, cam_params.image_size_.width); |
| 49 | EXPECT_EQ(size_expected.height, cam_params.image_size_.height); |
| 50 | |
| 51 | // Intrinsics. |
| 52 | const std::vector<double> intrinsics_expected = {458.654, 457.296, 367.215, |
| 53 | 248.375}; |
| 54 | for (int c = 0u; c < 4u; c++) { |
| 55 | EXPECT_DOUBLE_EQ(intrinsics_expected[c], cam_params.intrinsics_[c]); |
| 56 | } |
| 57 | EXPECT_DOUBLE_EQ(intrinsics_expected[0], |
| 58 | cam_params.camera_matrix_.at<double>(0, 0)); |
| 59 | EXPECT_DOUBLE_EQ(intrinsics_expected[1], |
| 60 | cam_params.camera_matrix_.at<double>(1, 1)); |
| 61 | EXPECT_DOUBLE_EQ(intrinsics_expected[2], |
| 62 | cam_params.camera_matrix_.at<double>(0, 2)); |
| 63 | EXPECT_DOUBLE_EQ(intrinsics_expected[3], |
| 64 | cam_params.camera_matrix_.at<double>(1, 2)); |
| 65 | EXPECT_EQ(cam_params.intrinsics_.size(), 4u); |
| 66 | EXPECT_DOUBLE_EQ(intrinsics_expected[0], cam_params.calibration_.fx()); |
| 67 | EXPECT_DOUBLE_EQ(intrinsics_expected[1], cam_params.calibration_.fy()); |
| 68 | EXPECT_DOUBLE_EQ(0u, cam_params.calibration_.skew()); |
| 69 | EXPECT_DOUBLE_EQ(intrinsics_expected[2], cam_params.calibration_.px()); |
| 70 | EXPECT_DOUBLE_EQ(intrinsics_expected[3], cam_params.calibration_.py()); |
| 71 | |
| 72 | // Sensor extrinsics wrt. the body-frame. |
| 73 | Rot3 R_expected(0.0148655429818, -0.999880929698, 0.00414029679422, |
| 74 | 0.999557249008, 0.0149672133247, 0.025715529948, |
| 75 | -0.0257744366974, 0.00375618835797, 0.999660727178); |
| 76 | Point3 T_expected(-0.0216401454975, -0.064676986768, 0.00981073058949); |
| 77 | Pose3 pose_expected(R_expected, T_expected); |
| 78 | EXPECT_TRUE(assert_equal(pose_expected, cam_params.body_Pose_cam_)); |
| 79 | |
| 80 | // Distortion coefficients. |
| 81 | const std::vector<double> distortion_expected = {-0.28340811, 0.07395907, |
| 82 | 0.00019359, 1.76187114e-05}; |
| 83 | for (int c = 0u; c < 4u; c++) { |
| 84 | EXPECT_DOUBLE_EQ(distortion_expected[c], |
| 85 | cam_params.distortion_coeff_.at<double>(c)); |
| 86 | } |
| 87 | EXPECT_EQ(cam_params.distortion_coeff_.rows, 1u); |
| 88 | EXPECT_EQ(cam_params.distortion_coeff_.cols, 4u); |
| 89 | EXPECT_DOUBLE_EQ(distortion_expected[0], cam_params.calibration_.k1()); |
| 90 | EXPECT_DOUBLE_EQ(distortion_expected[1], cam_params.calibration_.k2()); |
| 91 | EXPECT_DOUBLE_EQ(distortion_expected[2], cam_params.calibration_.p1()); |
| 92 | EXPECT_DOUBLE_EQ(distortion_expected[3], cam_params.calibration_.p2()); |
| 93 | } |
| 94 | |
| 95 | /* ************************************************************************** */ |