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

fixed another test.

release/4.3a0
lcarlone 2021-07-23 17:57:24 -04:00
parent 9c288d90ce
commit 81526e8917
2 changed files with 13 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -319,7 +319,6 @@ PinholePose<CALIBRATION> > {
return boost::make_shared < RegularHessianFactor<DimPose> return boost::make_shared < RegularHessianFactor<DimPose>
> (this->keys_, Gs, gs, 0.0); > (this->keys_, Gs, gs, 0.0);
} }
// compute Jacobian given triangulated 3D Point // compute Jacobian given triangulated 3D Point
FBlocks Fs; FBlocks Fs;
Matrix E; Matrix E;

31
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -580,9 +580,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista
EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3))); EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
} }
/* ************************************************************************* /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) {
std::cout << "===================" << std::endl;
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
// add fourth landmark // add fourth landmark
Point3 landmark4(5, -0.5, 1); Point3 landmark4(5, -0.5, 1);
@ -607,7 +606,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
double excludeLandmarksFutherThanDist = 1e10; double excludeLandmarksFutherThanDist = 1e10;
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error double dynamicOutlierRejectionThreshold = 3; // max 3 pixel of average reprojection error
SmartProjectionParams params; SmartProjectionParams params;
params.setRankTolerance(1.0); params.setRankTolerance(1.0);
@ -617,17 +616,17 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
params.setEnableEPI(false); params.setEnableEPI(false);
SmartFactorRS smartFactor1(model, params); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model,params));
smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); smartFactor1->add(measurements_cam1, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor2(model, params); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model,params));
smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); smartFactor2->add(measurements_cam2, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor3(model, params); SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model,params));
smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); smartFactor3->add(measurements_cam3, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor4(model, params); SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model,params));
smartFactor4.add(measurements_cam4, key_pairs, interp_factors, sharedK); smartFactor4->add(measurements_cam4, key_pairs, interp_factors, sharedK);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -639,8 +638,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
graph.addPrior(x1, level_pose, noisePrior); graph.addPrior(x1, level_pose, noisePrior);
graph.addPrior(x2, pose_right, noisePrior); graph.addPrior(x2, pose_right, noisePrior);
Pose3 noise_pose = Pose3::identity(); // Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
// Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.01, 0.01, 0.01)); // smaller noise, otherwise outlier rejection will kick in
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
@ -652,15 +651,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
smartFactor1.print("smartFactor1");
smartFactor2.print("smartFactor2");
smartFactor3.print("smartFactor3");
smartFactor4.print("smartFactor4");
} }
/* ************************************************************************* /* *************************************************************************
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) {
std::cout << "===================" << std::endl;
using namespace vanillaPose2; using namespace vanillaPose2;