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

working on testing + cosmetic improvements to print for smart factors

release/4.3a0
lcarlone 2021-07-23 17:48:08 -04:00
parent a7b7770310
commit 9c288d90ce
3 changed files with 72 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/slam/SmartFactorBase.h
View File

@ -178,7 +178,7 @@ protected:
DefaultKeyFormatter) const override { DefaultKeyFormatter) const override {
std::cout << s << "SmartFactorBase, z = \n"; std::cout << s << "SmartFactorBase, z = \n";
for (size_t k = 0; k < measured_.size(); ++k) { for (size_t k = 0; k < measured_.size(); ++k) {
std::cout << "measurement, p = " << measured_[k] << "\t"; std::cout << "measurement " << k<<", px = \n" << measured_[k] << "\n";
noiseModel_->print("noise model = "); noiseModel_->print("noise model = ");
} }
if(body_P_sensor_) if(body_P_sensor_)

2
gtsam/slam/SmartProjectionFactor.h
View File

@ -101,7 +101,7 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override { DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionFactor\n"; std::cout << s << "SmartProjectionFactor\n";
std::cout << "linearizationMode:\n" << params_.linearizationMode std::cout << "linearizationMode: " << params_.linearizationMode
<< std::endl; << std::endl;
std::cout << "triangulationParameters:\n" << params_.triangulation std::cout << "triangulationParameters:\n" << params_.triangulation
<< std::endl; << std::endl;

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

@ -319,14 +319,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) {
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
SmartFactorRS smartFactor1(model); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); smartFactor1->add(measurements_cam1, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor2(model); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model));
smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); smartFactor2->add(measurements_cam2, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor3(model); SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model));
smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); smartFactor3->add(measurements_cam3, key_pairs, interp_factors, sharedK);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -457,7 +457,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, EPI ) { TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@ -519,7 +519,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, EPI ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@ -581,70 +581,82 @@ TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) {
} }
/* ************************************************************************* /* *************************************************************************
TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) {
std::cout << "===================" << std::endl; std::cout << "===================" << std::endl;
using namespace vanillaPoseRS;
// add fourth landmark
Point3 landmark4(5, -0.5, 1);
using namespace vanillaPose; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4;
// Project 4 landmarks into cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
double excludeLandmarksFutherThanDist = 1e10; // create inputs
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error std::vector<std::pair<Key, Key>> key_pairs;
key_pairs.push_back(std::make_pair(x1, x2));
key_pairs.push_back(std::make_pair(x2, x3));
key_pairs.push_back(std::make_pair(x3, x1));
KeyVector views {x1, x2, x3}; std::vector<double> interp_factors;
interp_factors.push_back(interp_factor1);
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
// add fourth landmark double excludeLandmarksFutherThanDist = 1e10;
Point3 landmark4(5, -0.5, 1); double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, SmartProjectionParams params;
measurements_cam4; params.setRankTolerance(1.0);
params.setLinearizationMode(gtsam::HESSIAN);
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
params.setEnableEPI(false);
// Project 4 landmarks into three cameras SmartFactorRS smartFactor1(model, params);
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
SmartProjectionParams params; SmartFactorRS smartFactor2(model, params);
params.setLinearizationMode(gtsam::JACOBIAN_SVD); smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK);
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
SmartFactor::shared_ptr smartFactor1( SmartFactorRS smartFactor3(model, params);
new SmartFactor(model, sharedK, params)); smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK);
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2( SmartFactorRS smartFactor4(model, params);
new SmartFactor(model, sharedK, params)); smartFactor4.add(measurements_cam4, key_pairs, interp_factors, sharedK);
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3( const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
new SmartFactor(model, sharedK, params));
smartFactor3->add(measurements_cam3, views);
SmartFactor::shared_ptr smartFactor4( NonlinearFactorGraph graph;
new SmartFactor(model, sharedK, params)); graph.push_back(smartFactor1);
smartFactor4->add(measurements_cam4, views); graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.addPrior(x1, level_pose, noisePrior);
graph.addPrior(x2, pose_right, noisePrior);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); Pose3 noise_pose = Pose3::identity(); // Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
// Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, level_pose);
values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, pose_above * noise_pose);
NonlinearFactorGraph graph; // Optimization should correct 3rd pose
graph.push_back(smartFactor1); Values result;
graph.push_back(smartFactor2); LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
graph.push_back(smartFactor3); result = optimizer.optimize();
graph.push_back(smartFactor4); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.pose(), noisePrior);
Values values; smartFactor1.print("smartFactor1");
values.insert(x1, cam1.pose()); smartFactor2.print("smartFactor2");
values.insert(x2, cam2.pose()); smartFactor3.print("smartFactor3");
values.insert(x3, cam3.pose()); smartFactor4.print("smartFactor4");
// All factors are disabled and pose should remain where it is
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3)));
} }
/* ************************************************************************* /* *************************************************************************