diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index 3b2b68c18cf8b8c347ce78037e38d00d4f990406..cde8790298ad7e3709c1e2dc2a0ecd16f1c398a1 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/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;
diff --git a/src/sensor_camera.cpp b/src/sensor_camera.cpp
index 7a9a9a2c7904b0ea47143c96a4d0bdd1b36c8c50..ec41c09faf89385b3c43f4ea90f528194775e486 100644
--- a/src/sensor_camera.cpp
+++ b/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_);
}
diff --git a/src/sensor_camera.h b/src/sensor_camera.h
index d8641510db8225e874e06526665199e8bc316d2f..6e5e422ed7e82bed5df6de8c1a1da5786e170dbe 100644
--- a/src/sensor_camera.h
+++ b/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
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index cd99f0476dfe9eacbf0b7b63566f9306def8e554..219a8a27999e9a512ab6d1585a0f67d0a425494f 100644
--- a/src/test/CMakeLists.txt
+++ b/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 ----------
diff --git a/src/test/gtest_sensor_camera.cpp b/src/test/gtest_sensor_camera.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a75431e50f596372604ad49bec5618a9fb0664f1
--- /dev/null
+++ b/src/test/gtest_sensor_camera.cpp
@@ -0,0 +1,123 @@
+/**
+ * \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();
+}
+
diff --git a/src/yaml/sensor_camera_yaml.cpp b/src/yaml/sensor_camera_yaml.cpp
index 583ecef953accc5a04c25d0825ceb3d1908f6525..22e2b85b8a688ecb5e4067ac3c55e0e68d062cc3 100644
--- a/src/yaml/sensor_camera_yaml.cpp
+++ b/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;
//=========================================