From 3a61ab9e3951149c0f5d495db1bb2c5e7cb10f4f Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Sat, 23 Apr 2022 23:00:04 +0200
Subject: [PATCH] Improve API of SensorCamera
---
include/vision/sensor/sensor_camera.h | 21 +++++++++++----------
src/sensor/sensor_camera.cpp | 10 +++++++++-
2 files changed, 20 insertions(+), 11 deletions(-)
diff --git a/include/vision/sensor/sensor_camera.h b/include/vision/sensor/sensor_camera.h
index f677c9bcc..d4b6c34af 100644
--- a/include/vision/sensor/sensor_camera.h
+++ b/include/vision/sensor/sensor_camera.h
@@ -137,17 +137,18 @@ class SensorCamera : public SensorBase
~SensorCamera() override;
- Eigen::VectorXd getDistortionVector() { return distortion_; }
- Eigen::VectorXd getCorrectionVector() { return correction_; }
- Eigen::Matrix3d getIntrinsicMatrix() { return K_; }
- Eigen::Vector4d getPinholeModel() { if (using_raw_) return pinhole_model_raw_; else return pinhole_model_rectified_;}
+ Eigen::VectorXd getDistortionVector()const { return distortion_; }
+ Eigen::VectorXd getCorrectionVector()const { return correction_; }
+ Eigen::Matrix3d getIntrinsicMatrix() const { return K_; }
+ Eigen::Vector4d getPinholeModel() const { if (using_raw_) return pinhole_model_raw_; else return pinhole_model_rectified_;}
+ Eigen::Matrix<double, 3, 4>getProjectionMatrix() const;
- bool isUsingRawImages() { return using_raw_; }
+ bool isUsingRawImages() const { return using_raw_; }
bool useRawImages();
bool useRectifiedImages();
- int getImgWidth() {return img_width_;}
- int getImgHeight() {return img_height_;}
+ int getImgWidth() const {return img_width_;}
+ int getImgHeight() const {return img_height_;}
void setImgWidth(int _w) {img_width_ = _w;}
void setImgHeight(int _h) {img_height_ = _h;}
@@ -160,7 +161,7 @@ class SensorCamera : public SensorBase
Eigen::Matrix3d K_;
bool using_raw_;
- virtual Eigen::Matrix3d setIntrinsicMatrix(Eigen::Vector4d _pinhole_model);
+ Eigen::Matrix3d computeIntrinsicMatrix(const Eigen::Vector4d& _pinhole_model) const;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
@@ -169,7 +170,7 @@ class SensorCamera : public SensorBase
inline bool SensorCamera::useRawImages()
{
getIntrinsic()->setState(pinhole_model_raw_);
- K_ = setIntrinsicMatrix(pinhole_model_raw_);
+ K_ = computeIntrinsicMatrix(pinhole_model_raw_);
using_raw_ = true;
return true;
@@ -178,7 +179,7 @@ inline bool SensorCamera::useRawImages()
inline bool SensorCamera::useRectifiedImages()
{
getIntrinsic()->setState(pinhole_model_rectified_);
- K_ = setIntrinsicMatrix(pinhole_model_rectified_);
+ K_ = computeIntrinsicMatrix(pinhole_model_rectified_);
distortion_.resize(0); // remove distortion from model
correction_.resize(0);
using_raw_ = false;
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 3ecb7d084..311e2212b 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -62,7 +62,15 @@ SensorCamera::~SensorCamera()
//
}
-Eigen::Matrix3d SensorCamera::setIntrinsicMatrix(Eigen::Vector4d _pinhole_model)
+Eigen::Matrix<double, 3, 4> SensorCamera::getProjectionMatrix() const
+{
+ Eigen::Matrix<double, 3, 4> P;
+ P.setZero();
+ P.leftCols(3) = getIntrinsicMatrix();
+ return P;
+}
+
+Eigen::Matrix3d SensorCamera::computeIntrinsicMatrix(const Eigen::Vector4d& _pinhole_model) const
{
Eigen::Matrix3d K;
K(0, 0) = _pinhole_model(2);
--
GitLab