diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index ffd8008ec..5f6c2fee3 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -932,7 +932,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); } -/* ************************************************************************* +/* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { using namespace vanillaPose2; @@ -941,13 +941,13 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac views.push_back(x2); views.push_back(x3); - // Two different cameras + // Two different cameras, at the same position, but different rotations Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); Camera cam2(pose2, sharedK2); Camera cam3(pose3, sharedK2); - vector measurements_cam1, measurements_cam2, measurements_cam3; + vector measurements_cam1, measurements_cam2; // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -975,38 +975,20 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac graph.push_back( PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, pose2 * noise_pose); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose3 * noise_pose * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814, - -0.572308662, -0.324093872, 0.639358349, -0.521617766, - -0.564921063), - Point3(0.145118171, -0.252907438, 0.819956033)), - values.at(x3))); + values.insert(x3, pose3 * noise_pose); - Values result; - params.verbosityLM = LevenbergMarquardtParams::SUMMARY; + // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - - EXPECT( - assert_equal( - Pose3( - Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814, - -0.572308662, -0.324093872, 0.639358349, -0.521617766, - -0.564921063), - Point3(0.145118171, -0.252907438, 0.819956033)), - result.at(x3))); + Values result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3))); } -/* ************************************************************************* +/* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { using namespace vanillaPose; @@ -1016,7 +998,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) views.push_back(x2); views.push_back(x3); - // Two different cameras + // Two different cameras, at the same position, but different rotations Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); @@ -1065,7 +1047,6 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above values.insert(x3, pose3 * noise_pose); EXPECT( assert_equal( @@ -1080,14 +1061,10 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - result.at(x3))); + // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) + // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior + EXPECT(assert_equal(Pose3(values.at(x3).rotation(), + Point3(0,0,1)), result.at(x3))); } /* *************************************************************************/ diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 9e8523ebb..e00dcee57 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -73,7 +73,7 @@ int main(int argc, char** argv){ while (pose_file >> i) { for (int i = 0; i < 16; i++) pose_file >> m.data()[i]; - initial_estimate.insert(i, Camera(Pose3(m),K)); + initial_estimate.insert(i, Pose3(m)); } // landmark keys @@ -87,24 +87,24 @@ int main(int argc, char** argv){ //read stereo measurements and construct smart factors - SmartFactor::shared_ptr factor(new SmartFactor()); + SmartFactor::shared_ptr factor(new SmartFactor(K)); size_t current_l = 3; // hardcoded landmark ID from first measurement while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) { if(current_l != l) { graph.push_back(factor); - factor = SmartFactor::shared_ptr(new SmartFactor()); + factor = SmartFactor::shared_ptr(new SmartFactor(K)); current_l = l; } factor->add(Point2(uL,v), i, model); } - Camera firstCamera = initial_estimate.at(1); + Pose3 firstPose = initial_estimate.at(1); //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(1,firstCamera)); + graph.push_back(NonlinearEquality(1,firstPose)); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; @@ -116,7 +116,7 @@ int main(int argc, char** argv){ Values result = optimizer.optimize(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = result.filter(); pose_values.print("Final camera poses:\n"); return 0;