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

added dynamic outlier rejection (with unit tests) for smart pose factors

release/4.3a0
Luca 2014-05-22 22:32:12 -04:00
parent e0500f7b18
commit c2705902cb
3 changed files with 183 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -95,6 +95,13 @@ protected:
/// shorthand for base class type
typedef SmartFactorBase<POSE, CALIBRATION, D> Base;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the
// average reprojection error is smaller than this threshold after triangulation,
// and the factor is disregarded if the error is large
/// shorthand for this class
typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
@ -119,12 +126,15 @@ public:
SmartProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI,
boost::optional<POSE> body_P_sensor = boost::none,
SmartFactorStatePtr state = SmartFactorStatePtr(
new SmartProjectionFactorState())) :
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1,
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false), state_(state) {
false), verboseCheirality_(false), state_(state),
landmarkDistanceThreshold_(landmarkDistanceThreshold),
dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
}
/** Virtual destructor */
@ -238,6 +248,31 @@ public:
rankTolerance_, enableEPI_);
degenerate_ = false;
cheiralityException_ = false;
// Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0;
size_t i=0;
BOOST_FOREACH(const Camera& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away
if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){
degenerate_ = true;
break;
}
const Point2& zi = this->measured_.at(i);
try {
Point2 reprojectionError(camera.project(point_) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException& e) {
cheiralityException_ = true;
}
i += 1;
}
// we discard smart factors that have large reprojection error
if(dynamicOutlierRejectionThreshold_ > 0 &&
totalReprojError/m > dynamicOutlierRejectionThreshold_)
degenerate_ = true;
} catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before

6
gtsam_unstable/slam/SmartProjectionPoseFactor.h
View File

@ -59,8 +59,10 @@ public:
SmartProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false,
const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
linearizationType linearizeTo = HESSIAN) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor), linearizeTo_(linearizeTo) {}
linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
/** Virtual destructor */
virtual ~SmartProjectionPoseFactor() {}

141
gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -431,6 +431,147 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, landmarkDistance ){
double excludeLandmarksFutherThanDist = 2;
std::vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera cam1(pose1, *K);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
SimpleCamera cam2(pose2, *K);
// create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam3(pose3, *K);
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(3, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// 1. Project three landmarks into three cameras and triangulate
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
values.insert(x2, pose2);
values.insert(x3, pose3*noise_pose);
// All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(values.at<Pose3>(x3),result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){
double excludeLandmarksFutherThanDist = 1e10;
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
std::vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera cam1(pose1, *K);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
SimpleCamera cam2(pose2, *K);
// create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam3(pose3, *K);
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(3, 0, 3.0);
Point3 landmark4(5, -0.5, 1);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4;
// 1. Project three landmarks into three cameras and triangulate
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
SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none,
JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model, K);
SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
values.insert(x2, pose2);
values.insert(x3, pose3);
// All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, jacobianQ ){