Update reflection transform into one step.

1 files changed, 12 insertions, 30 deletions

diff --git a/src/camera.cpp b/src/camera.cpp
index 8d0be89..7f1c7a6 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -228,39 +228,21 @@ Camera::warpPatch ( const cv::Mat &p, const Quaterniond &q0, const Quaterniond &
 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;
+    Matrix3d R,tc;
+    Vector3d nw(0.,0.,-1.);
+    Vector3d c(0,0,1);
+    cv::Size sz(PATCHSIZE,PATCHSIZE);
 
-    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);
+    tc = Matrix3d::Identity()+Rc2b().transpose()*q1.toRotationMatrix().transpose()*(PATCHSIZE+1)*nw*c.transpose();
+    R = Rc2b().transpose() * q1.toRotationMatrix().transpose() * (Matrix3d::Identity()-2*nw*nw.transpose())* q0.toRotationMatrix() * Rc2b() * tc;
 
-    cv::Size sz(PATCHSIZE,PATCHSIZE);
-    cv::warpAffine(p, w1, Rmat1, sz);
-    cv::flip(w1, w2, 0);
-    cv::warpAffine(w2, w3, Rmat2, sz);
+    cv::Matx23d Rmat;
+    cv::Mat w;
+    Rmat << R(0,0), R(0,1), R(0,2),
+            R(1,0), R(1,1), R(1,2);
+    cv::warpAffine(p,w,Rmat,sz);
 
-    return w3;
+    return w;
 }		/* -----  end of method Camera::warpPatch  ----- */
 
 double