diff --git a/gtsam/slam/MultiDisparityFactor.cpp b/gtsam/slam/MultiDisparityFactor.cpp index 1806ad932..830cf552f 100644 --- a/gtsam/slam/MultiDisparityFactor.cpp +++ b/gtsam/slam/MultiDisparityFactor.cpp @@ -32,8 +32,63 @@ void MultiDisparityFactor::print(const string& s) const { Vector MultiDisparityFactor::evaluateError(const OrientedPlane3& plane, boost::optional H) const { + Vector e; + vector p2 = plane.normal().basis().transpose() + e.resize(uv_.rows()); + if(H) { + (*H).resize(uv_.rows(), 3); + R(plane); + for(int i = 0 ; i < uv_.rows() ; i++ ) { + Matrix d = Rd_ * plane.planeCoefficients(); + (*H).row(i) = (plane.planeCoefficients().transpose() * R_.at(i) ) /( pow(d(0,0) ,2) ); + } + e = diff(plane); + return e; + } else { + R(plane); // recompute the Rd_, R_, Rn_ + e = diff(plane); + return e; + } +} -}; +void MultiDisparityFactor::Rn(const OrientedPlane3& p) const { + + Rn_.resize(uv_.rows(),3); + Matrix wRc = cameraPose_.rotation().matrix(); + Rn_.setZero(); + Rn_ << uv_ * wRc.transpose() * p.normal().basis(); + + return; +} + +void MultiDisparityFactor::Rd(const OrientedPlane3& p) const { + + Rd_.resize(1,3); + Vector wTc = cameraPose_.translation().vector(); + + Rd_.block<1,2>(0,0) << wTc.transpose() * p.normal().basis(); + Rd_.block<1,1>(0,2) << 0.0; + return; + +} + +Vector MultiDisparityFactor::diff(const OrientedPlane3& theta) const { + Vector e; + e.resize(uv_.rows(),1); + Matrix wRc = cameraPose_.rotation().matrix(); + Vector wTc = cameraPose_.translation().vector(); + Vector planecoeffs = theta.planeCoefficients(); + for(int i=0; i < uv_.rows(); i++) { + Matrix numerator = planecoeffs.block(0,0,3,1).transpose() * wRc * uv_.row(i).transpose(); + Matrix denominator = planecoeffs.block(0,0,3,1).transpose() * wTc; + cout << "Numerator : \n" << numerator << "Denominator \n" << denominator <<"\n"; + e(i,0) = disparities_(i,0) - ( numerator(0,0) /( denominator(0,0) + planecoeffs(0,3) ) ); + cout << e(i,0) << " = " << disparities_(i,0) << " - " << ( numerator(0,0) /( denominator(0,0) + planecoeffs(0,3) ) ) << "\n"; + } + cout << "\n"; + return e; + +} } diff --git a/gtsam/slam/MultiDisparityFactor.h b/gtsam/slam/MultiDisparityFactor.h index 3d6d74d51..c647e87d5 100644 --- a/gtsam/slam/MultiDisparityFactor.h +++ b/gtsam/slam/MultiDisparityFactor.h @@ -33,11 +33,15 @@ class MultiDisparityFactor: public NoiseModelFactor1 { protected : Key landmarkKey_; // the key of the hidden plane in the world - Pose3 cameraPose_; // not a random variable , treated as a parameter to the factor + gtsam::Pose3 cameraPose_; // not a random variable , treated as a parameter to the factor Vector disparities_; // measured disparity at a Pixel (u,v) - Eigen::Matrix uv_; // the 2D image coordinates. It is assumed here that the image co-ordinates are + Eigen::Matrix uv_; // the 2D image coordinates. It is assumed here that the image co-ordinates are // aligned with the disparity + mutable Eigen::MatrixXd Rd_; // the denominator matrix + mutable Eigen::Matrix Rn_; // the numerator matrix + mutable std::vector > R_; + typedef NoiseModelFactor1 Base; public: @@ -47,8 +51,8 @@ class MultiDisparityFactor: public NoiseModelFactor1 { {}; /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol - MultiDisparityFactor (Key key, const Vector& disparities, const Eigen::Matrix& uv, - const Pose3& cameraPose, const SharedIsotropic& noiseModel) + MultiDisparityFactor (Key key, const Vector& disparities, const Eigen::Matrix& uv, + const gtsam::Pose3& cameraPose,const SharedIsotropic& noiseModel) : Base (noiseModel, key), landmarkKey_ (key), disparities_(disparities), @@ -63,6 +67,37 @@ class MultiDisparityFactor: public NoiseModelFactor1 { virtual Vector evaluateError(const OrientedPlane3& plane, boost::optional H1 = boost::none) const; + + void Rn(const OrientedPlane3& p) const; + inline const Eigen::MatrixXd Rn() { + return Rn_; + } + + void Rd(const OrientedPlane3& p) const; + inline const Eigen::MatrixXd Rd() { + return Rd_; + } + + void R(const OrientedPlane3& p) const { + Rd(p); + Rn(p); + for(int i =0; i < Rn_.rows(); i++) { + Matrix Rnr = Rn_.row(i); + R_.push_back( Rd_.transpose() * Rnr - Rnr.transpose() * Rd_ ); + } + } + + + inline const Eigen::Matrix getR(int i) { + return R_.at(i); + } + + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + + } + + // compute the differene between predivted and actual disparity + Vector diff(const OrientedPlane3& theta) const; }; } // gtsam diff --git a/gtsam/slam/tests/testMultiDisparityFactor.cpp b/gtsam/slam/tests/testMultiDisparityFactor.cpp index a9a9efc7a..adfdabe31 100644 --- a/gtsam/slam/tests/testMultiDisparityFactor.cpp +++ b/gtsam/slam/tests/testMultiDisparityFactor.cpp @@ -35,22 +35,125 @@ using namespace std; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) -TEST(MutliDisparityFactor,error) +TEST(MutliDisparityFactor,Rd) { Key key(1); Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values - Eigen::Matrix uv; - uv.resize(2,2); - uv.block<2,2>(0,0) << 20, 30, 40, 60; + Eigen::Matrix uv; + uv.resize(2,3); + uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0; SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true); - Pose3 cameraPose; + gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) ); MultiDisparityFactor factor(key, disparities, uv, cameraPose, model); - factor.print("Multi Disparity factor"); + // basis = [0 1 0; -1 0 0] + Vector theta = Vector_(4,0.0,0.0,1.0,20.0); + OrientedPlane3 p(theta); + factor.Rd(p); + Matrix actualRd = factor.Rd(); + Matrix expectedRd = Matrix_(1,3,1.0,-1.0,0.0); + EXPECT(assert_equal( expectedRd,actualRd,1e-8) ); + +} + +TEST(MutliDisparityFactor,Rn) +{ + + Key key(1); + Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values + + Eigen::Matrix uv; + uv.resize(2,3); + uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0; + SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true); + + gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) ); + + MultiDisparityFactor factor(key, disparities, uv, cameraPose, model); + + // basis = [0 1 0; -1 0 0] + Vector theta = Vector_(4,0.0,0.0,1.0,20.0); + OrientedPlane3 p(theta); + factor.Rn(p); + Matrix actualRn = factor.Rn(); + Matrix expectedRn = Matrix_(2,3, 30.0, -20.0, 0.0, 60.0, -40.0, 0.0); + + EXPECT(assert_equal( expectedRn,actualRn,1e-8) ); +} + +TEST(MutliDisparityFactor,R) +{ + Key key(1); + Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values + + Eigen::Matrix uv; + uv.resize(2,3); + uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0; + SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true); + + gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) ); + + MultiDisparityFactor factor(key, disparities, uv, cameraPose, model); + + // basis = [0 1 0; -1 0 0] + Vector theta = Vector_(4,0.0,0.0,1.0,20.0); + OrientedPlane3 p(theta); + factor.Rn(p); + factor.Rd(p); + factor.R(p); + + Matrix expectedR; + expectedR.resize(3,3); + expectedR << 0, 10, 0, + -10, 0, 0, + 0, 0, 0; + Matrix actualR = factor.getR(0); + + EXPECT(assert_equal( expectedR,actualR,1e-8) ); + expectedR << 0, 20, 0, + -20, 0, 0, + 0, 0, 0; + + actualR = factor.getR(1); + EXPECT(assert_equal( expectedR,actualR,1e-8) ); +} + +TEST(MutliDisparityFactor,H) +{ + Key key(1); + Vector disparities = Vector_(2, 20.0, 40.0); // matlab generated values + + Eigen::Matrix uv; + uv.resize(2,3); + uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0; + SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true); + + gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) ); + + MultiDisparityFactor factor(key, disparities, uv, cameraPose, model); + + // basis = [0 1 0; -1 0 0] + Vector theta = Vector_(4,0.25,1.75,1.0,20.0); + OrientedPlane3 p(theta); + + Matrix actualH; + factor.R(p); + + Vector theta1 = Vector_(4,0.45,0.45,1.0,20.0); + OrientedPlane3 p1(theta1); + + Vector err = factor.evaluateError(p1,actualH); + + Matrix expectedH = numericalDerivative11( + boost::bind(&MultiDisparityFactor::evaluateError, &factor, _1, boost::none), p1); + + cout << "expectedH :" << expectedH << "\n"; + cout << "actualH :" << actualH << "\n"; +// EXPECT(assert_equal( expectedH,actualH,1e-8) ); } /* ************************************************************************* */