Add Camera::ref2body()

Method converts a source reflection pair into a body coordinate with
depth.

1 files changed, 5 insertions, 0 deletions

diff --git a/src/camera.h b/src/camera.h
index 328db25..be0bd54 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -7,6 +7,7 @@
 using Eigen::Matrix;
 using Eigen::Vector4d;
 using Eigen::Vector3d;
+using Eigen::Quaterniond;
 
 /*
  * =====================================================================================
@@ -22,13 +23,17 @@ class Camera
 
         /* ====================  ACCESSORS     ======================================= */
         Matrix<double,3,3> K();
+        Matrix<double,4,4> K4();
         Vector4d d();
         Matrix<double,3,3> Rc2b();
+        Matrix<double,4,4> Rc2b4();
 
         /* ====================  MUTATORS      ======================================= */
 
         /* ====================  OPERATORS     ======================================= */
         Vector3d img2body(Vector3d &xi);
+        Vector3d ref2body(double z, const Quaterniond &qbw, const Vector3d &xs,
+                const Vector3d &xr);
 
     protected:
         /* ====================  METHODS       ======================================= */