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

added robustness to triangulation

release/4.3a0
lcarlone 2022-01-08 20:25:00 -05:00
parent 4dafcc50e8
commit 3ef6974235
2 changed files with 128 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -182,6 +182,94 @@ TEST(triangulation, fourPoses) {
#endif
}
//******************************************************************************
TEST(triangulation, threePoses_robustNoiseModel) {
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
Point2 z3 = camera3.project(landmark);
vector<Pose3> poses;
Point2Vector measurements;
poses += pose1, pose2, pose3;
measurements += z1, z2, z3;
// noise free, so should give exactly the landmark
boost::optional<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2));
// Add outlier
measurements.at(0) += Point2(100, 120); // very large pixel noise!
// now estimate does not match landmark
boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
// DLT is surprisingly robust, but still off (actual error is around 0.26m):
EXPECT( (landmark - *actual2).norm() >= 0.2);
EXPECT( (landmark - *actual2).norm() <= 0.5);
// Again with nonlinear optimization
boost::optional<Point3> actual3 =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
// result from nonlinear (but non-robust optimization) is close to DLT and still off
EXPECT(assert_equal(*actual2, *actual3, 0.1));
// Again with nonlinear optimization, this time with robust loss
auto model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
poses, sharedCal, measurements, 1e-9, true, model);
// using the Huber loss we now have a quite small error!! nice!
EXPECT(assert_equal(landmark, *actual4, 0.05));
}
//******************************************************************************
TEST(triangulation, fourPoses_robustNoiseModel) {
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
Point2 z3 = camera3.project(landmark);
vector<Pose3> poses;
Point2Vector measurements;
poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1
measurements += z1, z1, z2, z3;
// noise free, so should give exactly the landmark
boost::optional<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2));
// Add outlier
measurements.at(0) += Point2(100, 120); // very large pixel noise!
// add noise on other measurements:
measurements.at(1) += Point2(0.1, 0.2); // small noise
measurements.at(2) += Point2(0.2, 0.2);
measurements.at(3) += Point2(0.3, 0.1);
// now estimate does not match landmark
boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
// DLT is surprisingly robust, but still off (actual error is around 0.17m):
EXPECT( (landmark - *actual2).norm() >= 0.1);
EXPECT( (landmark - *actual2).norm() <= 0.5);
// Again with nonlinear optimization
boost::optional<Point3> actual3 =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
// result from nonlinear (but non-robust optimization) is close to DLT and still off
EXPECT(assert_equal(*actual2, *actual3, 0.1));
// Again with nonlinear optimization, this time with robust loss
auto model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
poses, sharedCal, measurements, 1e-9, true, model);
// using the Huber loss we now have a quite small error!! nice!
EXPECT(assert_equal(landmark, *actual4, 0.05));
}
//******************************************************************************
TEST(triangulation, fourPoses_distinct_Ks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480);

59
gtsam/geometry/triangulation.h
View File

@ -14,6 +14,7 @@
* @brief Functions for triangulation
* @date July 31, 2013
* @author Chris Beall
* @author Luca Carlone
*/
#pragma once
@ -105,18 +106,18 @@ template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, Key landmarkKey,
const Point3& initialEstimate) {
const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
for (size_t i = 0; i < measurements.size(); i++) {
const Pose3& pose_i = poses[i];
typedef PinholePose<CALIBRATION> Camera;
Camera camera_i(pose_i, sharedCal);
graph.emplace_shared<TriangulationFactor<Camera> > //
(camera_i, measurements[i], unit2, landmarkKey);
(camera_i, measurements[i], model? model : unit2, landmarkKey);
}
return std::make_pair(graph, values);
}
@ -134,7 +135,8 @@ template<class CAMERA>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
const Point3& initialEstimate) {
const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
@ -143,7 +145,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
for (size_t i = 0; i < measurements.size(); i++) {
const CAMERA& camera_i = cameras[i];
graph.emplace_shared<TriangulationFactor<CAMERA> > //
(camera_i, measurements[i], unit, landmarkKey);
(camera_i, measurements[i], model? model : unit, landmarkKey);
}
return std::make_pair(graph, values);
}
@ -169,13 +171,14 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
template<class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, const Point3& initialEstimate) {
const Point2Vector& measurements, const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
// Create a factor graph and initial values
Values values;
NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
(poses, sharedCal, measurements, Symbol('p', 0), initialEstimate);
(poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
return optimize(graph, values, Symbol('p', 0));
}
@ -190,13 +193,14 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
template<class CAMERA>
Point3 triangulateNonlinear(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) {
const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
// Create a factor graph and initial values
Values values;
NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph<CAMERA> //
(cameras, measurements, Symbol('p', 0), initialEstimate);
(cameras, measurements, Symbol('p', 0), initialEstimate, model);
return optimize(graph, values, Symbol('p', 0));
}
@ -239,7 +243,8 @@ template<class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false) {
bool optimize = false,
const SharedNoiseModel& model = nullptr) {
assert(poses.size() == measurements.size());
if (poses.size() < 2)
@ -254,7 +259,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
// Then refine using non-linear optimization
if (optimize)
point = triangulateNonlinear<CALIBRATION> //
(poses, sharedCal, measurements, point);
(poses, sharedCal, measurements, point, model);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras
@ -284,7 +289,8 @@ template<class CAMERA>
Point3 triangulatePoint3(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
bool optimize = false) {
bool optimize = false,
const SharedNoiseModel& model = nullptr) {
size_t m = cameras.size();
assert(measurements.size() == m);
@ -298,7 +304,7 @@ Point3 triangulatePoint3(
// The n refine using non-linear optimization
if (optimize)
point = triangulateNonlinear<CAMERA>(cameras, measurements, point);
point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras
@ -317,9 +323,10 @@ template<class CALIBRATION>
Point3 triangulatePoint3(
const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false) {
bool optimize = false,
const SharedNoiseModel& model = nullptr) {
return triangulatePoint3<PinholeCamera<CALIBRATION> > //
(cameras, measurements, rank_tol, optimize);
(cameras, measurements, rank_tol, optimize, model);
}
struct GTSAM_EXPORT TriangulationParameters {
@ -341,20 +348,29 @@ struct GTSAM_EXPORT TriangulationParameters {
*/
double dynamicOutlierRejectionThreshold;
SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation
bool invDepthLinearTriangulation; ///< if set to true, we use an inverse depth alternative to DL
/**
* Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate
* @param enableEPI if true refine triangulation with embedded LM iterations
* @param landmarkDistanceThreshold flag as degenerate if point further than this
* @param dynamicOutlierRejectionThreshold or if average error larger than this
* @param invDepthLinearTriangulation use inverse depth approach to linear triangulation
*
*/
TriangulationParameters(const double _rankTolerance = 1.0,
const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
double _dynamicOutlierRejectionThreshold = -1) :
double _dynamicOutlierRejectionThreshold = -1,
const SharedNoiseModel& _noiseModel = nullptr,
const bool _invDepthLinearTriangulation = false) :
rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
landmarkDistanceThreshold(_landmarkDistanceThreshold), //
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) {
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
noiseModel(_noiseModel),
invDepthLinearTriangulation(_invDepthLinearTriangulation){
}
// stream to output
@ -366,6 +382,9 @@ struct GTSAM_EXPORT TriangulationParameters {
<< std::endl;
os << "dynamicOutlierRejectionThreshold = "
<< p.dynamicOutlierRejectionThreshold << std::endl;
os << "noise model" << std::endl;
os << "invDepthLinearTriangulation = " << p.invDepthLinearTriangulation
<< std::endl;
return os;
}
@ -379,6 +398,7 @@ private:
ar & BOOST_SERIALIZATION_NVP(enableEPI);
ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
ar & BOOST_SERIALIZATION_NVP(invDepthLinearTriangulation);
}
};
@ -468,8 +488,9 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
else
// We triangulate the 3D position of the landmark
try {
Point3 point = triangulatePoint3<CAMERA>(cameras, measured,
params.rankTolerance, params.enableEPI);
Point3 point =
triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
params.enableEPI, params.noiseModel);
// Check landmark distance and re-projection errors to avoid outliers
size_t i = 0;