diff options
author | Martin Miller | 2017-03-31 15:51:15 -0500 |
---|---|---|
committer | Martin Miller | 2017-03-31 15:51:15 -0500 |
commit | 3c81b70308977377d6d7d7837aa476d4d5721d70 (patch) | |
tree | d00015fb6d74a24c9467a92f17d1187d125ab70e /src | |
parent | b93af7454f3cb654620a73f4bfb60c8c3bfcc71e (diff) | |
download | refslam-3c81b70308977377d6d7d7837aa476d4d5721d70.zip refslam-3c81b70308977377d6d7d7837aa476d4d5721d70.tar.gz |
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.
Diffstat (limited to 'src')
-rw-r--r-- | src/camera.cpp | 13 | ||||
-rw-r--r-- | src/camera.h | 1 |
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; |