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

Fixed another test

release/4.3a0
dellaert 2015-03-04 17:21:11 -08:00
parent 48d8de50d0
commit a95e5c7e05
1 changed files with 22 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -256,7 +256,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
// VectorValues delta = GFG->optimize();
// result.print("results of 3 camera, 3 landmark optimization \n");
EXPECT(assert_equal(cam3, result.at<Camera>(x3), 1e-8));
EXPECT(assert_equal(pose_above, result.at<Camera>(x3).pose(), 1e-8));
if (isDebugTest)
tictoc_print_();
}
@ -467,7 +467,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
tictoc_finishedIteration_();
// result.print("results of 3 camera, 3 landmark optimization \n");
EXPECT(assert_equal(cam3, result.at<Camera>(x3), 1e-7));
EXPECT(assert_equal(pose_above, result.at<Camera>(x3).pose(), 1e-7));
if (isDebugTest)
tictoc_print_();
}
@ -519,10 +519,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
values.insert(x3, Camera(pose_above * noise_pose, sharedK));
LevenbergMarquardtParams params;
if (isDebugTest)
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if (isDebugTest)
params.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(cam3, result.at<Camera>(x3), 1e-8));
EXPECT(assert_equal(pose_above, result.at<Camera>(x3).pose(), 1e-8));
}
/* *************************************************************************/
@ -640,12 +645,10 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(x2, cam2, noisePrior));
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);
values.insert(x2, cam2);
values.insert(x3, Camera(pose_above * noise_pose, sharedK));
values.insert(x3, cam3);
// All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params;
@ -701,10 +704,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
values.insert(x3, Camera(pose_above * noise_pose, sharedK));
LevenbergMarquardtParams params;
if (isDebugTest)
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if (isDebugTest)
params.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(cam3, result.at<Camera>(x3), 1e-8));
EXPECT(assert_equal(pose_above, result.at<Camera>(x3).pose(), 1e-8));
}
/* *************************************************************************/