From 728cc9930cbea478ca732b6c19bab275ef50c6d0 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Fri, 7 Apr 2017 17:28:23 -0500
Subject: Begin Vision class.

This class can display measurements and featuers and find new features.
When complete it will also perform measurements.
---
 src/camera.cpp | 27 +++++++++++++++++++++------
 1 file changed, 21 insertions(+), 6 deletions(-)

(limited to 'src/camera.cpp')

diff --git a/src/camera.cpp b/src/camera.cpp
index f9a3ccf..7e1c1b3 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -73,7 +73,7 @@ Camera::Camera (const char *fin)
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,3,3>
-Camera::Rc2b (  )
+Camera::Rc2b (  ) const
 {
 #ifdef DOYAWCORRECT
     double yaw;
@@ -94,7 +94,7 @@ Camera::Rc2b (  )
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,4,4>
-Camera::Rc2b4 ( )
+Camera::Rc2b4 ( ) const
 {
 #ifdef DOYAWCORRECT
     double yaw;
@@ -121,7 +121,7 @@ Camera::Rc2b4 ( )
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,4,4>
-Camera::K4 ( )
+Camera::K4 ( ) const
 {
     Matrix<double,4,4> K4;
     K4 = Matrix<double,4,4>::Identity();
@@ -138,7 +138,7 @@ Camera::K4 ( )
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,3,3>
-Camera::K (  )
+Camera::K (  ) const
 {
     return _K ;
 }		/* -----  end of method Camera::K  ----- */
@@ -151,7 +151,7 @@ Camera::K (  )
  *--------------------------------------------------------------------------------------
  */
 Vector4d
-Camera::d ( )
+Camera::d ( ) const
 {
     return _d;
 }		/* -----  end of method Camera::d  ----- */
@@ -166,7 +166,7 @@ Camera::d ( )
  *--------------------------------------------------------------------------------------
  */
 Vector3d
-Camera::img2body ( Vector3d &xi )
+Camera::img2body ( const Vector3d &xi ) const
 {
     Vector3d xc, xb;
     xc = K().inverse()*xi;
@@ -176,3 +176,18 @@ Camera::img2body ( Vector3d &xi )
     return xb;
 }		/* -----  end of method Camera::img2body  ----- */
 
+Vector3d
+Camera::body2img ( const Vector3d &xb ) const
+{
+    Vector3d xc,xi;
+    xc = Rc2b().transpose()*xb;
+    xi = K()*xc;
+    return xi;
+}		/* -----  end of method Camera::body2img  ----- */
+
+Vector3d
+Camera::principalPoint (  ) const
+{
+    return _K.col(2);
+}		/* -----  end of method Camera::principalPoint  ----- */
+
-- 
cgit v1.1