From 3c81b70308977377d6d7d7837aa476d4d5721d70 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Fri, 31 Mar 2017 15:51:15 -0500
Subject: Add yaw correction factor.

This is sort of a fudge to compensate for poor camera to IMU
calibration. It injects additional yaw into the Rc2b matrices.
---
 src/camera.cpp | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

(limited to 'src/camera.cpp')

diff --git a/src/camera.cpp b/src/camera.cpp
index 60d33bd..a3816ac 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -75,7 +75,11 @@ Camera::Camera (const char *fin)
 Matrix<double,3,3>
 Camera::Rc2b (  )
 {
-    return _T.block<3,3>(0,0).transpose();
+    double yaw;
+    yaw = M_PI*( YAWCORRECT )/180.;
+    Matrix<double,3,3> Ry;
+    Ry = Eigen::AngleAxisd(yaw, Vector3d::UnitZ());
+    return Ry*_T.block<3,3>(0,0).transpose();
 }		/* -----  end of method Camera::Rc2b  ----- */
 
 /*
@@ -88,9 +92,14 @@ Camera::Rc2b (  )
 Matrix<double,4,4>
 Camera::Rc2b4 ( )
 {
+    double yaw;
+    yaw = M_PI*( YAWCORRECT )/180.;
+    Matrix<double,3,3> Ry;
+    Ry = Eigen::AngleAxisd(yaw, Vector3d::UnitZ());
+
     Matrix<double,4,4> R;
     R = Matrix<double,4,4>::Identity();
-    R.block<3,3>(0,0) = Rc2b();
+    R.block<3,3>(0,0) = Ry*Rc2b();
     return R ;
 }		/* -----  end of method Camera::Rc2b4  ----- */
 
-- 
cgit v1.1