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

DegeneracyMode

release/4.3a0
dellaert 2015-03-12 07:56:59 -07:00
parent 2bdeac30f0
commit 695d080e4d
3 changed files with 84 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/slam/SmartProjectionFactor.h
View File

@ -44,13 +44,19 @@ public:
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
/// How to manage degeneracy
enum DegeneracyMode {
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
};
private:
typedef SmartFactorBase<CAMERA> Base;
typedef SmartProjectionFactor<CAMERA> This;
protected:
LinearizationMode linearizeTo_; ///< How to linearize the factor
LinearizationMode linearizationMode_; ///< How to linearize the factor
DegeneracyMode degeneracyMode_; ///< How to linearize the factor
/// @name Caching triangulation
/// @{
@ -63,7 +69,6 @@ protected:
/// @name Parameters governing how triangulation result is treated
/// @{
const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
@ -85,13 +90,16 @@ public:
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
*/
SmartProjectionFactor(LinearizationMode linearizationMode = HESSIAN,
double rankTolerance = 1, bool manageDegeneracy = false, bool enableEPI =
false, double landmarkDistanceThreshold = 1e10,
double rankTolerance = 1, DegeneracyMode degeneracyMode =
IGNORE_DEGENERACY, bool enableEPI = false,
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
linearizeTo_(linearizationMode), parameters_(rankTolerance, enableEPI,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), //
linearizationMode_(linearizationMode), //
degeneracyMode_(degeneracyMode), //
parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
dynamicOutlierRejectionThreshold), //
result_(TriangulationResult::Degenerate()), //
retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), //
retriangulationThreshold_(1e-5), //
throwCheirality_(false), verboseCheirality_(false) {
}
@ -107,7 +115,7 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartProjectionFactor\n";
std::cout << "linearizationMode:\n" << linearizeTo_ << std::endl;
std::cout << "linearizationMode:\n" << linearizationMode_ << std::endl;
std::cout << "triangulationParameters:\n" << parameters_ << std::endl;
std::cout << "result:\n" << result_ << std::endl;
Base::print("", keyFormatter);
@ -116,7 +124,8 @@ public:
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e && linearizeTo_ == e->linearizeTo_ && Base::equals(p, tol);
return e && linearizationMode_ == e->linearizationMode_
&& Base::equals(p, tol);
}
/// Check if the new linearization point is the same as the one used for previous triangulation
@ -194,7 +203,7 @@ public:
triangulateSafe(cameras);
if (!manageDegeneracy_ && !result_) {
if (degeneracyMode_ == ZERO_ON_DEGENERACY && !result_) {
// put in "empty" Hessian
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(Base::Dim, Base::Dim);
@ -281,7 +290,7 @@ public:
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
Cameras cameras = this->cameras(values);
switch (linearizeTo_) {
switch (linearizationMode_) {
case HESSIAN:
return createHessianFactor(cameras, lambda);
case IMPLICIT_SCHUR:
@ -381,7 +390,7 @@ public:
if (result_)
// All good, just use version in base class
return Base::totalReprojectionError(cameras, *result_);
else if (manageDegeneracy_) {
else if (degeneracyMode_ == HANDLE_INFINITY) {
// Otherwise, manage the exceptions with rotation-only factors
const Point2& z0 = this->measured_.at(0);
Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0);

42
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -31,7 +31,6 @@ using namespace boost::assign;
static bool isDebugTest = false;
static double rankTol = 1.0;
static double linThreshold = -1.0;
// Convenience for named keys
using symbol_shorthand::X;
@ -79,7 +78,7 @@ TEST( SmartProjectionCameraFactor, Constructor) {
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor2) {
using namespace vanilla;
SmartFactor factor1(SmartFactor::HESSIAN, rankTol, linThreshold);
SmartFactor factor1(SmartFactor::HESSIAN, rankTol);
}
/* ************************************************************************* */
@ -92,7 +91,7 @@ TEST( SmartProjectionCameraFactor, Constructor3) {
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor4) {
using namespace vanilla;
SmartFactor factor1(SmartFactor::HESSIAN, rankTol, linThreshold);
SmartFactor factor1(SmartFactor::HESSIAN, rankTol);
factor1.add(measurement1, x1, unit2);
}
@ -108,7 +107,6 @@ TEST( SmartProjectionCameraFactor, Equals ) {
/* *************************************************************************/
TEST( SmartProjectionCameraFactor, noiseless ) {
// cout << " ************************ SmartProjectionCameraFactor: noisy ****************************" << endl;
using namespace vanilla;
Values values;
@ -153,7 +151,8 @@ TEST( SmartProjectionCameraFactor, noisy ) {
// Check triangulated point
CHECK(factor1->point());
EXPECT(assert_equal(Point3(13.7587, 1.43851, -1.14274),*factor1->point(), 1e-4));
EXPECT(
assert_equal(Point3(13.7587, 1.43851, -1.14274), *factor1->point(), 1e-4));
// Check whitened errors
Vector expected(4);
@ -236,16 +235,23 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
CHECK(smartFactor2->point());
CHECK(smartFactor3->point());
EXPECT(assert_equal(Point3(2.57696, -0.182566, 1.04085),*smartFactor1->point(), 1e-4));
EXPECT(assert_equal(Point3(2.80114, -0.702153, 1.06594),*smartFactor2->point(), 1e-4));
EXPECT(assert_equal(Point3(1.82593, -0.289569, 2.13438),*smartFactor3->point(), 1e-4));
EXPECT(
assert_equal(Point3(2.57696, -0.182566, 1.04085), *smartFactor1->point(),
1e-4));
EXPECT(
assert_equal(Point3(2.80114, -0.702153, 1.06594), *smartFactor2->point(),
1e-4));
EXPECT(
assert_equal(Point3(1.82593, -0.289569, 2.13438), *smartFactor3->point(),
1e-4));
// Check whitened errors
Vector expected(6);
expected << 256, 29, -26, 29, -206, -202;
Point3 point1 = *smartFactor1->point();
SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial);
Vector reprojectionError = cameras1.reprojectionError(point1, measurements_cam1);
Vector reprojectionError = cameras1.reprojectionError(point1,
measurements_cam1);
EXPECT(assert_equal(expected, reprojectionError, 1));
Vector actual = smartFactor1->whitenedError(cameras1, point1);
EXPECT(assert_equal(expected, actual, 1));
@ -259,9 +265,9 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
Values result = optimizer.optimize();
EXPECT(assert_equal(landmark1,*smartFactor1->point(), 1e-7));
EXPECT(assert_equal(landmark2,*smartFactor2->point(), 1e-7));
EXPECT(assert_equal(landmark3,*smartFactor3->point(), 1e-7));
EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-7));
EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-7));
EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-7));
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial);
// VectorValues delta = GFG->optimize();
@ -796,15 +802,12 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
double rankTol = 1;
double linThreshold = -1;
bool manageDegeneracy = false;
bool useEPI = false;
bool isImplicit = false;
// Hessian version
SmartFactor::shared_ptr explicitFactor(
new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold, manageDegeneracy, useEPI,
isImplicit));
new SmartFactor(SmartFactor::HESSIAN, rankTol,
SmartFactor::IGNORE_DEGENERACY, useEPI));
explicitFactor->add(level_uv, c1, unit2);
explicitFactor->add(level_uv_right, c2, unit2);
@ -814,10 +817,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
dynamic_cast<HessianFactor&>(*gaussianHessianFactor);
// Implicit Schur version
isImplicit = true;
SmartFactor::shared_ptr implicitFactor(
new SmartFactor(SmartFactor::IMPLICIT_SCHUR, rankTol, linThreshold, manageDegeneracy, useEPI,
isImplicit));
new SmartFactor(SmartFactor::IMPLICIT_SCHUR, rankTol,
SmartFactor::IGNORE_DEGENERACY, useEPI));
implicitFactor->add(level_uv, c1, unit2);
implicitFactor->add(level_uv_right, c2, unit2);
GaussianFactor::shared_ptr gaussianImplicitSchurFactor =

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

@ -31,8 +31,6 @@
using namespace boost::assign;
static const double rankTol = 1.0;
static const bool manageDegeneracy = true;
// Create a noise model for the pixel error
static const double sigma = 0.1;
static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
@ -501,15 +499,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false));
new SmartFactor(SmartFactor::JACOBIAN_SVD));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false));
new SmartFactor(SmartFactor::JACOBIAN_SVD));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false));
new SmartFactor(SmartFactor::JACOBIAN_SVD));
smartFactor3->add(measurements_cam3, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -530,7 +528,6 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
values.insert(x3, Camera(pose_above * noise_pose, sharedK));
Values result;
params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(pose_above, result.at<Camera>(x3).pose(), 1e-8));
@ -556,17 +553,20 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
SmartFactor::IGNORE_DEGENERACY, false,
excludeLandmarksFutherThanDist));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
SmartFactor::IGNORE_DEGENERACY, false,
excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
SmartFactor::IGNORE_DEGENERACY, false,
excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model);
@ -621,23 +621,27 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
dynamicOutlierRejectionThreshold));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model);
SmartFactor::shared_ptr smartFactor4(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -680,15 +684,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::JACOBIAN_Q, 1, false, false));
new SmartFactor(SmartFactor::JACOBIAN_Q));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::JACOBIAN_Q, 1, false, false));
new SmartFactor(SmartFactor::JACOBIAN_Q));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::JACOBIAN_Q, 1, false, false));
new SmartFactor(SmartFactor::JACOBIAN_Q));
smartFactor3->add(measurements_cam3, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -885,11 +889,13 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
double rankTol = 50;
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
new SmartFactor(SmartFactor::HESSIAN, rankTol,
SmartFactor::ZERO_ON_DEGENERACY));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
new SmartFactor(SmartFactor::HESSIAN, rankTol,
SmartFactor::ZERO_ON_DEGENERACY));
smartFactor2->add(measurements_cam2, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -923,6 +929,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
values.at<Camera>(x3).pose()));
Values result;
params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
@ -964,15 +971,18 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
double rankTol = 10;
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
new SmartFactor(SmartFactor::HESSIAN, rankTol,
SmartFactor::ZERO_ON_DEGENERACY));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
new SmartFactor(SmartFactor::HESSIAN, rankTol,
SmartFactor::ZERO_ON_DEGENERACY));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
new SmartFactor(SmartFactor::HESSIAN, rankTol,
SmartFactor::ZERO_ON_DEGENERACY));
smartFactor3->add(measurements_cam3, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1265,15 +1275,18 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
double rankTol = 10;
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
new SmartFactor(SmartFactor::HESSIAN, rankTol,
SmartFactor::ZERO_ON_DEGENERACY));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
new SmartFactor(SmartFactor::HESSIAN, rankTol,
SmartFactor::ZERO_ON_DEGENERACY));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
new SmartFactor(SmartFactor::HESSIAN, rankTol,
SmartFactor::ZERO_ON_DEGENERACY));
smartFactor3->add(measurements_cam3, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);