1 files changed, 11 insertions, 0 deletions
diff --git a/src/camera.cpp b/src/camera.cpp
index dcc5a44..b698925 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -90,3 +90,14 @@ Camera::d ( )
     return _d;
 }		/* -----  end of method Camera::d  ----- */
 
+Vector3d
+Camera::img2body ( Vector3d &xi )
+{
+    Vector3d xc, xb;
+    xc = K().inverse()*xi;
+    // Normalize
+    xc /= xc.norm();
+    xb = Rc2b()*xc;
+    return xb;
+}		/* -----  end of method Camera::img2body  ----- */
+