Add Camera::rc2b() method.

1 files changed, 32 insertions, 0 deletions

diff --git a/src/camera.cpp b/src/camera.cpp
index a78746f..dcc5a44 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -28,6 +28,7 @@
 Camera::Camera (const char *fin)
 {
     YAML::Node config = YAML::LoadFile(fin);
+
     _K(0,0) = BINNING*config["camera_matrix"]["data"][0].as<double>();
     _K(0,1) = BINNING*config["camera_matrix"]["data"][1].as<double>();
     _K(0,2) = BINNING*config["camera_matrix"]["data"][2].as<double>();
@@ -43,9 +44,40 @@ Camera::Camera (const char *fin)
     _d[2] = config["distortion_coefficients"]["data"][2].as<double>();
     _d[3] = config["distortion_coefficients"]["data"][3].as<double>();
 
+    _T(0,0) = config["T_cam_imu"][0][0].as<double>();
+    _T(0,1) = config["T_cam_imu"][0][1].as<double>();
+    _T(0,2) = config["T_cam_imu"][0][2].as<double>();
+    _T(0,3) = config["T_cam_imu"][0][3].as<double>();
+    _T(1,0) = config["T_cam_imu"][1][0].as<double>();
+    _T(1,1) = config["T_cam_imu"][1][1].as<double>();
+    _T(1,2) = config["T_cam_imu"][1][2].as<double>();
+    _T(1,3) = config["T_cam_imu"][1][3].as<double>();
+    _T(2,0) = config["T_cam_imu"][2][0].as<double>();
+    _T(2,1) = config["T_cam_imu"][2][1].as<double>();
+    _T(2,2) = config["T_cam_imu"][2][2].as<double>();
+    _T(2,3) = config["T_cam_imu"][2][3].as<double>();
+    _T(3,0) = config["T_cam_imu"][3][0].as<double>();
+    _T(3,1) = config["T_cam_imu"][3][1].as<double>();
+    _T(3,2) = config["T_cam_imu"][3][2].as<double>();
+    _T(3,3) = config["T_cam_imu"][3][3].as<double>();
+
 }  /* -----  end of method Camera::Camera  (constructor)  ----- */
 
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Camera
+ *      Method:  Camera :: Rc2b
+ * Description:  Returns the rotation matrix that transforms a point from the
+ * camera frame to the body frame.
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,3,3>
+Camera::Rc2b (  )
+{
+    return _T.block<3,3>(0,0).transpose();
+}		/* -----  end of method Camera::Rc2b  ----- */
+
 Matrix<double,3,3>
 Camera::K (  )
 {