1 files changed, 66 insertions, 0 deletions
diff --git a/src/camera.cpp b/src/camera.cpp
index 415bf35..2040641 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -192,3 +192,69 @@ Camera::principalPoint (  ) const
     return _K.col(2);
 }		/* -----  end of method Camera::principalPoint  ----- */
 
+cv::Mat
+Camera::warpPatch ( const cv::Mat &p, const Quaterniond &q0, const Quaterniond &q1 ) const
+{
+    cv::Mat w;
+    Matrix<double,3,3> R;
+    R = Rc2b().transpose() * q1.toRotationMatrix().transpose() *
+        q0.toRotationMatrix() * Rc2b();
+    cv::Matx23d Rmat;
+    Rmat << R(0,0), R(0,1), R(0,2),
+            R(1,0), R(1,1), R(1,2);
+            //R(2,0), R(2,1), R(2,2);
+    cv::Size sz(PATCHSIZE,PATCHSIZE);
+    cv::warpAffine(p, w, Rmat, sz);
+
+    return w;
+}		/* -----  end of method Camera::warpPatch  ----- */
+
+cv::Mat
+Camera::reflectPatch( const cv::Mat &p, const Quaterniond &q0, const Quaterniond &q1 ) const
+{
+    using Eigen::AngleAxisd;
+    cv::Mat w1, w2, w3;
+    Matrix<double,3,3> R1, R2;
+    Matrix<double,3,3> J;
+    Matrix<double,3,3> Rbw, Rb0w;
+    double roll0, pitch0;
+    double roll1, pitch1;
+
+    roll0 = roll(q0);
+    pitch0 = pitch(q0);
+    roll1 = roll(q1);
+    pitch1 = pitch(q1);
+
+    Rb0w = AngleAxisd(roll0, Vector3d::UnitX())
+      *  AngleAxisd(pitch0, Vector3d::UnitY());
+    Rbw = AngleAxisd(roll1, Vector3d::UnitX())
+      *  AngleAxisd(pitch1, Vector3d::UnitY());
+
+    R1 = Rc2b().transpose() * Rb0w * Rc2b();
+    R2 = Rc2b().transpose() * Rbw.transpose() * Rc2b();
+    cv::Matx23d Rmat1, Rmat2;
+
+    Rmat1 << R1(0,0), R1(0,1), R1(0,2),
+            R1(1,0), R1(1,1), R1(1,2);
+    Rmat2 << R2(0,0), R2(0,1), R2(0,2),
+            R2(1,0), R2(1,1), R2(1,2);
+
+    cv::Size sz(PATCHSIZE,PATCHSIZE);
+    cv::warpAffine(p, w1, Rmat1, sz);
+    cv::flip(w1, w2, 0);
+    cv::warpAffine(w2, w3, Rmat2, sz);
+
+    return w3;
+}		/* -----  end of method Camera::warpPatch  ----- */
+
+double
+Camera::roll(const Quaterniond &q) const
+{
+    return atan2( 2*(q.w()*q.x() + q.y()*q.z()), 1-2*(q.x()*q.x() + q.y()+q.y()));
+}
+
+double
+Camera::pitch(const Quaterniond &q) const
+{
+    return asin( 2*(q.w()*q.y() - q.z()*q.x()));
+}