diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp index 6ac2264b9..c6d7daa21 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp @@ -457,22 +457,21 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor ) { // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; EXPECT_DOUBLES_EQUAL(833953.92789459578, graph.error(values), 1e-7); // initial error -// // get triangulated landmarks from smart factors -// Point3 landmark1_smart = *smartFactor1->point(); -// Point3 landmark2_smart = *smartFactor2->point(); -// Point3 landmark3_smart = *smartFactor3->point(); -// -// Values result; -// gttic_(SmartStereoProjectionFactorPP); -// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); -// result = optimizer.optimize(); -// gttoc_(SmartStereoProjectionFactorPP); -// tictoc_finishedIteration_(); -// -// EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); + // get triangulated landmarks from smart factors + Point3 landmark1_smart = *smartFactor1->point(); + Point3 landmark2_smart = *smartFactor2->point(); + Point3 landmark3_smart = *smartFactor3->point(); + + Values result; + gttic_(SmartStereoProjectionFactorPP); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); + result = optimizer.optimize(); + gttoc_(SmartStereoProjectionFactorPP); + tictoc_finishedIteration_(); + + // cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl; + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); -//// cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl; -// // GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); // VectorValues delta = GFG->optimize(); // VectorValues expected = VectorValues::Zero(delta);