diff options
Diffstat (limited to 'src/camera.cpp')
| -rw-r--r-- | src/camera.cpp | 27 |
1 files changed, 21 insertions, 6 deletions
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 ----- */ + |