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.

2 files changed, 12 insertions, 2 deletions

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  ----- */
 
diff --git a/src/camera.h b/src/camera.h
index bee8246..599fa23 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -5,6 +5,7 @@
 #include <yaml-cpp/yaml.h>
 
 #define BINNING 0.5 // set the binning factor
+#define YAWCORRECT 7.5
 using Eigen::Matrix;
 using Eigen::Vector4d;
 using Eigen::Vector3d;