12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

test failure: now we can start computing jacobians

release/4.3a0
lcarlone 2021-03-13 18:12:12 -05:00
parent c1da490c2d
commit 8a37a86441
1 changed files with 14 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
View File

@ -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);