Add Camera::ref2body()

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

2 files changed, 91 insertions, 0 deletions

diff --git a/src/camera.cpp b/src/camera.cpp
index b698925..9b7c134 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -78,6 +78,39 @@ Camera::Rc2b (  )
     return _T.block<3,3>(0,0).transpose();
 }		/* -----  end of method Camera::Rc2b  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Camera
+ *      Method:  Camera :: Rc2b4
+ * Description:  Returns the 4x4 Rc2b matrix
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,4,4>
+Camera::Rc2b4 ( )
+{
+    Matrix<double,4,4> R;
+    R = Matrix<double,4,4>::Identity();
+    R.block<3,3>(0,0) = Rc2b();
+    return R ;
+}		/* -----  end of method Camera::Rc2b4  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Camera
+ *      Method:  Camera :: K4
+ * Description:  Returns the 4x4 camera matrix.
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,4,4>
+Camera::K4 ( )
+{
+    Matrix<double,4,4> K4;
+    K4 = Matrix<double,4,4>::Identity();
+    K4.block<3,3>(0,0) = K();
+    return K4 ;
+}		/* -----  end of method Camera::K4  ----- */
+
+
 Matrix<double,3,3>
 Camera::K (  )
 {
@@ -101,3 +134,56 @@ Camera::img2body ( Vector3d &xi )
     return xb;
 }		/* -----  end of method Camera::img2body  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Camera
+ *      Method:  Camera :: ref2body
+ * Description:  Determines the feature position with respect to the body from a
+ * source reflection correspondence xs & sr given the body rotation and height.
+ *--------------------------------------------------------------------------------------
+ */
+Vector3d
+Camera::ref2body ( double z, const Quaterniond &qbw, const Vector3d &xs, 
+        const Vector3d &xr )
+{
+    // The output vector
+    Vector3d xb;
+    Vector4d xb4;
+
+    // Rotation from body to camera
+    Matrix<double,4,4> Rbc4;
+    Rbc4 = Rc2b4().transpose();
+    
+    // Projection matrices
+    Matrix<double,4,4> P1,P2,P;
+    P1 = K4()*Rbc4;
+
+    // Rotation from body to world
+    Matrix<double,3,3> Rbw;
+    Matrix<double,4,4> Rbw4,Rwb4;
+    Rbw = qbw.toRotationMatrix();
+    Rbw4 = Matrix<double,4,4>::Identity();
+    Rbw4.block<3,3>(0,0) = Rbw;
+    Rwb4 = Rbw4.transpose();
+
+
+    Matrix<double,4,4> J; // Reflection matrix
+    J = Matrix<double,4,4>::Identity();
+    J(2,2) = -1;
+    J(2,3) = -2*z;
+
+    P2 = K4()*Rbc4*Rwb4*J*Rbw4;
+    P.block<3,4>(0,0) = P1.block<3,4>(0,0);
+    P.block<1,4>(3,0) = P2.block<1,4>(1,0);
+
+    Vector4d b;
+    b.segment<3>(0) = xs;
+    b[3] = xr[1];
+
+    xb4 = P.inverse()*b;
+    xb4 /= xb4[3];
+    xb = xb4.segment<3>(0);
+
+    return xb ;
+}		/* -----  end of method Camera::ref2body  ----- */
+
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       ======================================= */