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

management of degeneracy

release/4.3a0
Luca Carlone 2013-09-28 00:49:37 +00:00
parent 6f463166a9
commit d10ffee4d4
3 changed files with 76 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/geometry/triangulation.cpp
View File

@ -46,7 +46,7 @@ Point3 triangulateDLT(const vector<Matrix>& projection_matrices,
double error;
Vector v;
boost::tie(rank, error, v) = DLT(A, rank_tol);
// std::cout << "s " << s << std:endl;
if(rank < 3)
throw(TriangulationUnderconstrainedException());

95
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -34,6 +34,10 @@ namespace gtsam {
static double defaultLinThreshold = -1; // 1e-7; // 0.01
// default threshold for retriangulation
static double defaultTriangThreshold = 1e-7;
// if set to true will use the rotation-only version for degenerate cases
static bool manageDegeneracy = true;
// if set to true will use the rotation-only version for degenerate cases
static double rankTolerance = 50;
/**
* Structure for storing some state memory, used to speed up optimization
@ -327,6 +331,13 @@ namespace gtsam {
cameraPoses.push_back(cameraPose);
}
if(cameraPoses.size() < 2){ // if we have a single pose the corresponding factor is uninformative
state_->degenerate = true;
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
BOOST_FOREACH(Vector& v, gs) v = zero(6);
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed
}
bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold);
if(retriangulate) {// we store the current poses used for triangulation
@ -336,39 +347,34 @@ namespace gtsam {
if (retriangulate) {
// We triangulate the 3D position of the landmark
try {
Point3 newPoint = triangulatePoint3(cameraPoses, measured_, *K_);
// changeLinPoint = newPoint - state_->point; // TODO: implement this check for the degenerate case
state_->point = newPoint;
state_->point = triangulatePoint3(cameraPoses, measured_, *K_, rankTolerance);
state_->degenerate = false;
state_->cheiralityException = false;
} catch( TriangulationUnderconstrainedException& e) {
// point is triangulated at infinity
//std::cout << "Triangulation failed " << e.what() << std::endl;
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
BOOST_FOREACH(Vector& v, gs) v = zero(6);
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
//std::cout << "Triangulation failed " << e.what() << std::endl; // point triangulated at infinity
state_->degenerate = true;
state_->cheiralityException = false;
dim_landmark = 2;
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed
} catch( TriangulationCheiralityException& e) {
// point is behind one of the cameras, turn factor off by setting everything to 0
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
//std::cout << e.what() << std::end;
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
BOOST_FOREACH(Vector& v, gs) v = zero(6);
state_->cheiralityException = true; // TODO: Debug condition, uncomment when fixed
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed
// TODO: this is a debug condition, should be removed the comment
state_->cheiralityException = true;
}
}
// state_->degenerate = true; // TODO: this is a debug condition, should be removed
// dim_landmark = 2; // TODO: this is a debug condition, should be removed the comment
if (!retriangulate && state_->cheiralityException) {
if (!manageDegeneracy && (state_->cheiralityException || state_->degenerate) ){
std::cout << "In linearize: exception" << std::endl;
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
BOOST_FOREACH(Vector& v, gs) v = zero(6);
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
}
if (!retriangulate && state_->degenerate) {
if (state_->cheiralityException || state_->degenerate){ // if we want to manage the exceptions with rotation-only factors
state_->degenerate = true;
dim_landmark = 2;
}
@ -388,8 +394,8 @@ namespace gtsam {
if(!doLinearize){ // return the previous Hessian factor
return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
}
//otherwise redo linearization
//otherwise redo linearization
if (blockwise){
// ==========================================================================================================
std::cout << "Deprecated use of blockwise version. This is slower and no longer supported" << std::endl;
@ -498,6 +504,7 @@ namespace gtsam {
subInsert(b2,bi,2*i);
}
}
// Shur complement trick
@ -524,12 +531,12 @@ namespace gtsam {
}
// ==========================================================================================================
// if(linearizationThreshold >= 0){ // if we do not use selective relinearization we don't need to store these variables
// state_->calculatedHessian = true;
// state_->Gs = Gs;
// state_->gs = gs;
// state_->f = f;
// }
if(linearizationThreshold >= 0){ // if we do not use selective relinearization we don't need to store these variables
state_->calculatedHessian = true;
state_->Gs = Gs;
state_->gs = gs;
state_->f = f;
}
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
}
@ -553,37 +560,47 @@ namespace gtsam {
cameraPoses.push_back(cameraPose);
}
if(cameraPoses.size() < 2){ // if we have a single pose the corresponding factor is uninformative
return 0.0;
}
bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold);
if(retriangulate) {// we store the current poses used for triangulation
state_->cameraPosesTriangulation = cameraPoses;
}
// We triangulate the 3D position of the landmark
if (retriangulate) {
// We triangulate the 3D position of the landmark
try {
state_->point = triangulatePoint3(cameraPoses, measured_, *K_);
state_->point = triangulatePoint3(cameraPoses, measured_, *K_, rankTolerance);
state_->degenerate = false;
state_->cheiralityException = false;
} catch( TriangulationCheiralityException& e) {
// std::cout << "TriangulationCheiralityException " << std::endl;
// point is behind one of the cameras, turn factor off by setting everything to 0
//std::cout << e.what() << std::end;
state_->cheiralityException = true; // TODO: Debug condition, remove comment
return 0.0; // TODO: this is a debug condition, should be removed the comment
} catch( TriangulationUnderconstrainedException& e) {
// point is triangulated at infinity
//std::cout << e.what() << std::endl;
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
//std::cout << "Triangulation failed " << e.what() << std::endl; // point triangulated at infinity
state_->degenerate = true;
state_->cheiralityException = false;
} catch( TriangulationCheiralityException& e) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
//std::cout << e.what() << std::end;
state_->cheiralityException = true;
}
}
// state_->degenerate = true; // TODO: this is a debug condition, should be removed
if (!retriangulate && state_->cheiralityException) {
if (!manageDegeneracy && (state_->cheiralityException || state_->degenerate) ){ // if we want to manage the exceptions with rotation-only factors
std::cout << "In error evaluation: exception" << std::endl;
return 0.0;
}
if (state_->cheiralityException || state_->degenerate){ // if we want to manage the exceptions with rotation-only factors
state_->degenerate = true;
}
if(state_->degenerate){
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
@ -604,7 +621,7 @@ namespace gtsam {
try {
Point2 reprojectionError(camera.project(state_->point) - measured_.at(i));
//std::cout << "Reprojection error: " << reprojectionError << std::endl;
//std::cout << "Reprojection error: " << reprojectionError << std::endl;
overallError += 0.5 * noise_->distance( reprojectionError.vector() );
//overallError += reprojectionError.vector().norm();
} catch ( CheiralityException& e) {

38
gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp
View File

@ -120,7 +120,7 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) {
}
/* *************************************************************************
/* *************************************************************************/
TEST( SmartProjectionFactor, noisy ){
cout << " ************************ SmartProjectionFactor: noisy ****************************" << endl;
@ -159,7 +159,7 @@ TEST( SmartProjectionFactor, noisy ){
measurements += level_uv, level_uv_right;
SmartProjectionFactor<Pose3, Point3, Cal3_S2>::shared_ptr
smartFactor(new SmartProjectionFactor<Pose3, Point3, Cal3_S2>(measurements, noiseProjection, views, K));
smartFactor(new SmartProjectionFactor<Pose3, Point3, Cal3_S2>(views, measurements, noiseProjection, K));
double actualError = smartFactor->error(values);
std::cout << "Error: " << actualError << std::endl;
@ -224,9 +224,9 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K, boost::make_optional<Point3>(landmark1) ));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K, boost::make_optional<Point3>(landmark2) ));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K, boost::make_optional<Point3>(landmark3) ));
SmartFactor::shared_ptr smartFactor1(new SmartFactor(views, measurements_cam1, noiseProjection, K, boost::make_optional<Point3>(landmark1) ));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(views, measurements_cam2, noiseProjection, K, boost::make_optional<Point3>(landmark2) ));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(views, measurements_cam3, noiseProjection, K, boost::make_optional<Point3>(landmark3) ));
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -311,7 +311,7 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
}
/* *************************************************************************
/* *************************************************************************/
TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
@ -365,9 +365,9 @@ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){
typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K));
SmartFactor::shared_ptr smartFactor1(new SmartFactor(views, measurements_cam1, noiseProjection, K));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(views, measurements_cam2, noiseProjection, K));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(views, measurements_cam3, noiseProjection, K));
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -405,7 +405,7 @@ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){
}
/* *************************************************************************
/* *************************************************************************/
TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
@ -510,7 +510,7 @@ TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){
}
/* *************************************************************************
/* *************************************************************************/
TEST( SmartProjectionFactor, 3poses_projection_factor ){
// cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
@ -583,7 +583,7 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){
}
/* *************************************************************************
/* *************************************************************************/
TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor ){
cout << " ************************ SmartProjectionFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl;
@ -670,7 +670,7 @@ TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor
tictoc_print_();
}
/* *************************************************************************
/* *************************************************************************/
TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;
@ -724,9 +724,9 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K));
SmartFactor::shared_ptr smartFactor1(new SmartFactor(views, measurements_cam1, noiseProjection, K));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(views, measurements_cam2, noiseProjection, K));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(views, measurements_cam3, noiseProjection, K));
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
@ -767,7 +767,7 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){
}
/* *************************************************************************
/* *************************************************************************/
TEST( SmartProjectionFactor, Hessian ){
cout << " ************************ SmartProjectionFactor: Hessian **********************" << endl;
@ -797,7 +797,7 @@ TEST( SmartProjectionFactor, Hessian ){
vector<Point2> measurements_cam1;
measurements_cam1 += cam1_uv1, cam2_uv1;
SmartProjectionFactor<Pose3, Point3, Cal3_S2> smartFactor(measurements_cam1, noiseProjection, views, K);
SmartProjectionFactor<Pose3, Point3, Cal3_S2> smartFactor(views, measurements_cam1, noiseProjection, K);
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
Values values;
@ -806,7 +806,7 @@ TEST( SmartProjectionFactor, Hessian ){
// values.insert(L(1), landmark1);
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor.linearize(values);
hessianFactor->print("Hessian factor \n");
// hessianFactor->print("Hessian factor \n");
// compute triangulation from linearization point
// compute reprojection errors (sum squared)