Camera intrinsics choice: raw or rectified

src/examples/test_wolf_factories.cpp

@@ -65,7 +65,7 @@ int main(void)
     IntrinsicsBasePtr intr_cam_ptr = IntrinsicsFactory::get().create("CAMERA", wolf_config + "/camera_params_ueye_sim.yaml");
     ProcessorParamsBasePtr params_ptr = ProcessorParamsFactory::get().create("IMAGE FEATURE", wolf_config + "/processor_image_feature.yaml");
 
-    cout << "CAMERA with intrinsics      : " << (static_pointer_cast<IntrinsicsCamera>(intr_cam_ptr))->pinhole_model.transpose() << endl;
+    cout << "CAMERA with intrinsics      : " << (static_pointer_cast<IntrinsicsCamera>(intr_cam_ptr))->pinhole_model_raw.transpose() << endl;
 //    cout << "Processor IMAGE image width : " << (static_pointer_cast<ProcessorParamsImage>(params_ptr))->image.width << endl;
 
     cout << "\n==================== Install Sensors ====================" << endl;

src/sensor_camera.cpp

@@ -10,20 +10,24 @@ namespace wolf
 SensorCamera::SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, //
                            int _img_width, int _img_height) :
         SensorBase("CAMERA", _p_ptr, _o_ptr, _intr_ptr, 2), //
-        img_width_(_img_width), img_height_(_img_height)
+        img_width_(_img_width), img_height_(_img_height),
+        using_raw_(true)
 {
-    //
+    useRawImages();
 }
 
 SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsCamera& _intrinsics) :
-                SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics.pinhole_model, true), 1),
+                SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics.pinhole_model_raw, true), 1),
                 img_width_(_intrinsics.width), //
                 img_height_(_intrinsics.height), //
                 distortion_(_intrinsics.distortion), //
-                correction_(distortion_.size()+2) // make correction vector of the same size as distortion vector
+                correction_(distortion_.size() + 1), // make correction vector slightly larger in size than the distortion vector
+                pinhole_model_raw_(_intrinsics.pinhole_model_raw), //
+                pinhole_model_rectified_(_intrinsics.pinhole_model_rectified), //
+                using_raw_(true)
 {
     assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D");
-    K_ = setIntrinsicMatrix(_intrinsics.pinhole_model);
+    useRawImages();
     pinhole::computeCorrectionModel(getIntrinsicPtr()->getState(), distortion_, correction_);
 }
 

src/sensor_camera.h

@@ -12,10 +12,11 @@ WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsCamera);
  */
 struct IntrinsicsCamera : public IntrinsicsBase
 {
-        unsigned int width;                 ///< Image width in pixels
-        unsigned int height;                ///< Image height in pixels
-        Eigen::Vector4s pinhole_model;      ///< k = [u_0, v_0, alpha_u, alpha_v]  vector of pinhole intrinsic parameters
-        Eigen::VectorXs distortion;         ///< d = [d_1, d_2, d_3, ...] radial distortion coefficients
+        unsigned int width;                     ///< Image width in pixels
+        unsigned int height;                    ///< Image height in pixels
+        Eigen::Vector4s pinhole_model_raw;      ///< k = [u_0, v_0, alpha_u, alpha_v]  vector of pinhole intrinsic parameters
+        Eigen::Vector4s pinhole_model_rectified;///< k = [u_0, v_0, alpha_u, alpha_v]  vector of pinhole intrinsic parameters
+        Eigen::VectorXs distortion;             ///< d = [d_1, d_2, d_3, ...] radial distortion coefficients
 
         virtual ~IntrinsicsCamera() = default;
 };
@@ -44,9 +45,14 @@ class SensorCamera : public SensorBase
 
         virtual ~SensorCamera();
 
-        Eigen::VectorXs getDistortionVector(){return distortion_;}
-        Eigen::VectorXs getCorrectionVector(){return correction_;}
-        Eigen::Matrix3s getIntrinsicMatrix() {return K_;}
+        Eigen::VectorXs getDistortionVector()   { return distortion_; }
+        Eigen::VectorXs getCorrectionVector()   { return correction_; }
+        Eigen::Matrix3s getIntrinsicMatrix()    { return K_; }
+
+        bool isUsingRawImages() { return using_raw_; }
+        bool useRawImages();
+        bool useRectifiedImages();
+
 
         int getImgWidth(){return img_width_;}
         int getImgHeight(){return img_height_;}
@@ -58,7 +64,9 @@ class SensorCamera : public SensorBase
         int img_height_;
         Eigen::VectorXs distortion_;
         Eigen::VectorXs correction_;
+        Eigen::Vector4s pinhole_model_raw_, pinhole_model_rectified_;
         Eigen::Matrix3s K_;
+        bool using_raw_;
 
         virtual Eigen::Matrix3s setIntrinsicMatrix(Eigen::Vector4s _pinhole_model);
 
@@ -71,6 +79,24 @@ class SensorCamera : public SensorBase
 
 };
 
+inline bool SensorCamera::useRawImages()
+{
+    getIntrinsicPtr()->setState(pinhole_model_raw_);
+    K_ = setIntrinsicMatrix(pinhole_model_raw_);
+    using_raw_ = true;
+
+    return true;
+}
+
+inline bool SensorCamera::useRectifiedImages()
+{
+    getIntrinsicPtr()->setState(pinhole_model_rectified_);
+    K_ = setIntrinsicMatrix(pinhole_model_rectified_);
+    using_raw_ = false;
+
+    return true;
+}
+
 } // namespace wolf
 
 #endif // SENSOR_CAMERA_H

src/test/CMakeLists.txt

@@ -102,6 +102,10 @@ wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
 wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
 target_link_libraries(gtest_sensor_base ${PROJECT_NAME})
 
+# shared_from_this test
+wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
+target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
+
 # SolverManager test
 wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
 target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
@@ -194,9 +198,10 @@ target_link_libraries(gtest_odom_2D ${PROJECT_NAME})
 wolf_add_gtest(gtest_odom_3D gtest_odom_3D.cpp)
 target_link_libraries(gtest_odom_3D ${PROJECT_NAME})
 
-# shared_from_this test
-wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
-target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
+# SensorBase test
+wolf_add_gtest(gtest_sensor_camera gtest_sensor_camera.cpp)
+target_link_libraries(gtest_sensor_camera ${PROJECT_NAME})
+
 
 # ------- Now Core classes Serialization ----------
 

src/test/gtest_sensor_camera.cpp

+/**
+ * \file gtest_sensor_camera.cpp
+ *
+ *  Created on: Feb 7, 2019
+ *      \author: jsola
+ */
+
+
+#include "utils_gtest.h"
+
+#include "sensor_camera.cpp"
+#include "sensor_factory.h"
+
+using namespace wolf;
+
+TEST(SensorCamera, Img_size)
+{
+    Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+    IntrinsicsCamera params;
+    params.width  = 640;
+    params.height = 480;
+    SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
+
+    ASSERT_EQ(cam->getImgWidth() , 640);
+    ASSERT_EQ(cam->getImgHeight(), 480);
+
+    cam->setImgWidth(100);
+    ASSERT_EQ(cam->getImgWidth() , 100);
+
+    cam->setImgHeight(100);
+    ASSERT_EQ(cam->getImgHeight(), 100);
+}
+
+TEST(SensorCamera, Intrinsics_Raw_Rectified)
+{
+    Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+    IntrinsicsCamera params;
+    params.pinhole_model_raw       << 321, 241, 321, 321;
+    params.pinhole_model_rectified << 320, 240, 320, 320;
+    SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
+
+    Eigen::Matrix3s K_raw, K_rectified;
+    K_raw       << 321, 0, 321,    0, 321, 241,    0, 0, 1;
+    K_rectified << 320, 0, 320,    0, 320, 240,    0, 0, 1;
+    Eigen::Vector4s k_raw(321,241,321,321);
+    Eigen::Vector4s k_rectified(320,240,320,320);
+
+    // default is raw
+    ASSERT_TRUE(cam->isUsingRawImages());
+    ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
+    ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsicPtr()->getState(), 1e-8);
+
+    cam->useRectifiedImages();
+    ASSERT_FALSE(cam->isUsingRawImages());
+    ASSERT_MATRIX_APPROX(K_rectified, cam->getIntrinsicMatrix(), 1e-8);
+    ASSERT_MATRIX_APPROX(k_rectified, cam->getIntrinsicPtr()->getState(), 1e-8);
+
+    cam->useRawImages();
+    ASSERT_TRUE(cam->isUsingRawImages());
+    ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
+    ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsicPtr()->getState(), 1e-8);
+}
+
+TEST(SensorCamera, Distortion)
+{
+    Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+    IntrinsicsCamera params;
+    params.width  = 640;
+    params.height = 480;
+    params.pinhole_model_raw       << 321, 241, 321, 321;
+    params.pinhole_model_rectified << 320, 240, 320, 320;
+    params.distortion = Eigen::Vector3s( -0.3, 0.096, 0 );
+    SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
+
+    Eigen::Vector3s d(-0.3, 0.096, 0);
+
+    ASSERT_MATRIX_APPROX(d, cam->getDistortionVector(), 1e-8);
+}
+
+TEST(SensorCamera, Correction_zero)
+{
+    Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+    IntrinsicsCamera params;
+    params.width  = 640;
+    params.height = 480;
+    params.pinhole_model_raw       << 321, 241, 321, 321;
+    params.pinhole_model_rectified << 320, 240, 320, 320;
+    params.distortion = Eigen::Vector3s( 0, 0, 0 );
+    SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
+
+    Eigen::MatrixXs c(cam->getCorrectionVector().size(), 1);
+    c.setZero();
+
+    ASSERT_GE(cam->getCorrectionVector().size(), cam->getDistortionVector().size());
+    ASSERT_MATRIX_APPROX(c, cam->getCorrectionVector(), 1e-8);
+}
+
+TEST(SensorCamera, create)
+{
+    Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+    IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>();
+    params->width  = 640;
+    params->height = 480;
+    params->pinhole_model_raw       << 321, 241, 321, 321;
+    params->pinhole_model_rectified << 320, 240, 320, 320;
+    params->distortion = Eigen::Vector3s( 0, 0, 0 );
+
+    SensorBasePtr   sen = SensorCamera::create("camera", extrinsics, params);
+
+    ASSERT_NE(sen, nullptr);
+
+    SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen);
+
+    ASSERT_NE(cam, nullptr);
+    ASSERT_EQ(cam->getImgWidth(), 640);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+

src/yaml/sensor_camera_yaml.cpp

@@ -32,29 +32,50 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do
         using namespace Eigen;
         unsigned int width      = camera_config["image_width"]                      .as<unsigned int>();
         unsigned int height     = camera_config["image_height"]                     .as<unsigned int>();
-        VectorXd intrinsic      = camera_config["camera_matrix"]["data"]            .as<VectorXd>();
         VectorXd distortion     = camera_config["distortion_coefficients"]["data"]  .as<VectorXd>();
+        VectorXd intrinsic      = camera_config["camera_matrix"]["data"]            .as<VectorXd>();
+        VectorXd projection     = camera_config["projection_matrix"]["data"]        .as<VectorXd>();
 
         // Eigen:: to wolf::
         std::shared_ptr<IntrinsicsCamera> intrinsics_cam = std::make_shared<IntrinsicsCamera>();
-        intrinsics_cam->pinhole_model[0] = intrinsic[2];
-        intrinsics_cam->pinhole_model[1] = intrinsic[5];
-        intrinsics_cam->pinhole_model[2] = intrinsic[0];
-        intrinsics_cam->pinhole_model[3] = intrinsic[4];
+
+        intrinsics_cam->width   = width;
+        intrinsics_cam->height  = height;
+
+        intrinsics_cam->pinhole_model_raw[0] = intrinsic[2];
+        intrinsics_cam->pinhole_model_raw[1] = intrinsic[5];
+        intrinsics_cam->pinhole_model_raw[2] = intrinsic[0];
+        intrinsics_cam->pinhole_model_raw[3] = intrinsic[4];
+
+        intrinsics_cam->pinhole_model_rectified[0] = projection[2];
+        intrinsics_cam->pinhole_model_rectified[1] = projection[6];
+        intrinsics_cam->pinhole_model_rectified[2] = projection[0];
+        intrinsics_cam->pinhole_model_rectified[3] = projection[5];
+
         assert (distortion.size() == 5 && "Distortion size must be size 5!");
-        assert (distortion(2) == 0 && distortion(3) == 0 && "Cannot handle tangential distortion. Please re-calibrate without tangential distortion!");
+
+        WOLF_WARN_COND( distortion(2) != 0 || distortion(3) != 0 , "Wolf does not handle tangential distortion. Please consider re-calibrating without tangential distortion!");
+
         if (distortion(4) == 0)
-            intrinsics_cam->distortion = distortion.head<2>();
+            if (distortion(1) == 0)
+                if (distortion(0) == 0)
+                    intrinsics_cam->distortion.resize(0);
+                else
+                {
+                    intrinsics_cam->distortion.resize(1);
+                    intrinsics_cam->distortion = distortion.head<1>();
+                }
+            else
+            {
+                intrinsics_cam->distortion.resize(2);
+                intrinsics_cam->distortion = distortion.head<2>();
+            }
         else
         {
-            unsigned int dist_size = distortion.size() - 2;
-            unsigned int dist_tail_size = dist_size - 2;
-            intrinsics_cam->distortion.resize(dist_size);
+            intrinsics_cam->distortion.resize(3);
             intrinsics_cam->distortion.head<2>() = distortion.head<2>();
-            intrinsics_cam->distortion.tail(dist_tail_size) = distortion.tail(dist_tail_size);
+            intrinsics_cam->distortion.tail<1>() = distortion.tail<1>();
         }
-        intrinsics_cam->width = width;
-        intrinsics_cam->height = height;
 
 
         //=========================================