Add Camera::ref2body()

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

1 files changed, 86 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  ----- */
+