diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index dbe374958..c272eac8e 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -43,6 +43,15 @@ public: HessianFactor(js, Gs, gs, f) { } + /** Construct a binary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12, + const Vector& g1, const Matrix& G22, const Vector& g2, double f) : + HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) { + } + /** Constructor with an arbitrary number of keys and with the augmented information matrix * specified as a block matrix. */ template @@ -169,6 +178,13 @@ public: } } + /* ************************************************************************* */ + +}; // end class RegularHessianFactor + +// traits +template struct traits > : public Testable< + RegularHessianFactor > { }; } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index b1d9cd9f6..b1490d465 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -87,18 +87,27 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << " RegularImplicitSchurFactor " << std::endl; Factor::print(s); - std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; - std::cout << " E_ \n" << E_ << std::endl; - std::cout << " b_ \n" << b_.transpose() << std::endl; + for (size_t pos = 0; pos < size(); ++pos) { + std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl; + } + std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl; + std::cout << "E:\n" << E_ << std::endl; + std::cout << "b:\n" << b_.transpose() << std::endl; } /// equals bool equals(const GaussianFactor& lf, double tol) const { - if (!dynamic_cast(&lf)) - return false; - else { + const This* f = dynamic_cast(&lf); + if (!f) return false; + for (size_t pos = 0; pos < size(); ++pos) { + if (keys_[pos] != f->keys_[pos]) return false; + if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false; + if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false; } + return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol) + && equal_with_abs_tol(E_, f->E_, tol) + && equal_with_abs_tol(b_, f->b_, tol); } /// Degrees of freedom of camera @@ -458,7 +467,12 @@ public: }; -// RegularImplicitSchurFactor +// end class RegularImplicitSchurFactor + +// traits +template struct traits > : public Testable< + RegularImplicitSchurFactor > { +}; } diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 02e4fd3b6..4f982f456 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -446,6 +446,85 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { EXPECT(assert_equal(-f(*point), actualErrors, 1e-7)); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Factors ){ + + // Default cameras for simple derivatives + Rot3 R; + static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0)); + SimpleCamera cam1(Pose3(R,Point3(0,0,0)),*K), cam2(Pose3(R,Point3(1,0,0)),*K); + + // one landmarks 1m in front of camera + Point3 landmark1(0,0,10); + + vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + cout << cam1.project(landmark1) << endl; + cout << cam2.project(landmark1) << endl; + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + LONGS_EQUAL(2,smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1,*p)); + + { + // RegularHessianFactor<6> + Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; + Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; + Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1; + + Vector6 g1; g1.setZero(); + Vector6 g2; g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, 5000 * G11, 5000 * G12, g1, 5000 * G22, g2, f); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 0.0); + CHECK(assert_equal(expected.information(),actual->information(),1e-8)); + CHECK(assert_equal(expected,*actual,1e-8)); + } + + { + // RegularImplicitSchurFactor<6> + Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; + Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; + Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100; + const vector > Fblocks = list_of > // + (make_pair(x1, 10*F1))(make_pair(x2, 10*F2)); + Matrix3 P = (E.transpose() * E).inverse(); + Vector4 b; b.setZero(); + + RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); + CHECK(assert_equal(expected,*actual)); + } + +} + + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl;