diff options
| -rw-r--r-- | src/camera.cpp | 42 |
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 |