diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 001e6b997..72fd031e6 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -524,85 +524,6 @@ TEST( SmartProjectionPoseFactor, Factors ){ } -/* *************************************************************************/ -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, 50 * G11, 50 * G12, g1, 50 * 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)=10; E(1,1)=10; E(2,0)=10; E(2,2)=1;E(3,1)=10; - const vector > Fblocks = list_of > // - (make_pair(x1, F1))(make_pair(x2, 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;