From 50834b4ba0904a13242476cc515f85fda4f30844 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Wed, 22 Mar 2017 15:47:00 -0500
Subject: Add feature handling to state

---
 src/camera.cpp | 11 +++++++++++
 1 file changed, 11 insertions(+)

(limited to 'src/camera.cpp')

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  ----- */
+
-- 
cgit v1.1