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

update to smartFactor

release/4.3a0
Luca Carlone 2013-09-26 23:09:47 +00:00
parent b04d2f1512
commit 22ccd4b4b4
1 changed files with 23 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -31,7 +31,7 @@
namespace gtsam { namespace gtsam {
// default threshold for selective relinearization // default threshold for selective relinearization
static double defaultLinThreshold = 1e-7; // 0.01 static double defaultLinThreshold = 0; // 1e-7; // 0.01
// default threshold for retriangulation // default threshold for retriangulation
static double defaultTriangThreshold = 1e-7; static double defaultTriangThreshold = 1e-7;
@ -72,11 +72,6 @@ namespace gtsam {
std::vector<Vector> gs; std::vector<Vector> gs;
double f; double f;
// Jacobian representation (before Schur complement)
Matrix Hx;
Matrix Hl;
Vector b;
// C = Hl'Hl // C = Hl'Hl
// Cinv = inv(Hl'Hl) // Cinv = inv(Hl'Hl)
// Matrix3 Cinv; // Matrix3 Cinv;
@ -347,23 +342,26 @@ namespace gtsam {
state_->degenerate = false; state_->degenerate = false;
state_->cheiralityException = false; state_->cheiralityException = false;
} catch( TriangulationUnderconstrainedException& e) { } catch( TriangulationUnderconstrainedException& e) {
// point is triangulated at infinity // point is triangulated at infinity
//std::cout << e.what() << std::end; //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);
state_->degenerate = true; state_->degenerate = true;
state_->cheiralityException = false; state_->cheiralityException = false;
dim_landmark = 2; dim_landmark = 2;
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed
} catch( TriangulationCheiralityException& e) { } 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, turn factor off by setting everything to 0
//std::cout << e.what() << std::end; //std::cout << e.what() << std::end;
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6); BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
BOOST_FOREACH(Vector& v, gs) v = zero(6); BOOST_FOREACH(Vector& v, gs) v = zero(6);
//state_->cheiralityException = true; // TODO: Debug condition, uncomment when fixed 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 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 // TODO: this is a debug condition, should be removed the comment
} }
} }
state_->degenerate = true; // TODO: this is a debug condition, should be removed // 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 // dim_landmark = 2; // TODO: this is a debug condition, should be removed the comment
if (!retriangulate && state_->cheiralityException) { if (!retriangulate && state_->cheiralityException) {
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6); BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
@ -486,6 +484,7 @@ namespace gtsam {
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
noise_-> WhitenSystem(Hxi, Hli, bi); noise_-> WhitenSystem(Hxi, Hli, bi);
f += bi.squaredNorm();
Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
Hl2.block( 2*i, 0, 2, 3 ) = Hli; Hl2.block( 2*i, 0, 2, 3 ) = Hli;
@ -494,10 +493,6 @@ namespace gtsam {
} }
} }
state_->Hx = Hx2;
state_->Hl = Hl2;
state_->b = b2;
// Shur complement trick // Shur complement trick
Matrix H(6 * numKeys, 6 * numKeys); Matrix H(6 * numKeys, 6 * numKeys);
Matrix C2; Matrix C2;
@ -522,10 +517,10 @@ namespace gtsam {
} }
// ========================================================================================================== // ==========================================================================================================
state_->calculatedHessian = true; // state_->calculatedHessian = true;
state_->Gs = Gs; // state_->Gs = Gs;
state_->gs = gs; // state_->gs = gs;
state_->f = f; // state_->f = f;
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
} }
@ -565,8 +560,8 @@ namespace gtsam {
// std::cout << "TriangulationCheiralityException " << std::endl; // std::cout << "TriangulationCheiralityException " << std::endl;
// point is behind one of the cameras, turn factor off by setting everything to 0 // point is behind one of the cameras, turn factor off by setting everything to 0
//std::cout << e.what() << std::end; //std::cout << e.what() << std::end;
//state_->cheiralityException = true; // TODO: Debug condition, remove comment state_->cheiralityException = true; // TODO: Debug condition, remove comment
//return 0.0; // TODO: this is a debug condition, should be removed the comment return 0.0; // TODO: this is a debug condition, should be removed the comment
} catch( TriangulationUnderconstrainedException& e) { } catch( TriangulationUnderconstrainedException& e) {
// point is triangulated at infinity // point is triangulated at infinity
//std::cout << e.what() << std::endl; //std::cout << e.what() << std::endl;
@ -574,7 +569,7 @@ namespace gtsam {
state_->cheiralityException = false; state_->cheiralityException = false;
} }
} }
state_->degenerate = true; // TODO: this is a debug condition, should be removed // state_->degenerate = true; // TODO: this is a debug condition, should be removed
if (!retriangulate && state_->cheiralityException) { if (!retriangulate && state_->cheiralityException) {
return 0.0; return 0.0;
@ -588,7 +583,8 @@ namespace gtsam {
state_->point = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity state_->point = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity
} }
Point2 reprojectionError(camera.projectPointAtInfinity(state_->point) - measured_.at(i)); Point2 reprojectionError(camera.projectPointAtInfinity(state_->point) - measured_.at(i));
overallError += noise_->distance( reprojectionError.vector() ); overallError += 0.5 * noise_->distance( reprojectionError.vector() );
//overallError += reprojectionError.vector().norm();
} }
return overallError; return overallError;
} }
@ -599,7 +595,9 @@ namespace gtsam {
try { try {
Point2 reprojectionError(camera.project(state_->point) - measured_.at(i)); Point2 reprojectionError(camera.project(state_->point) - measured_.at(i));
overallError += noise_->distance( reprojectionError.vector() ); //std::cout << "Reprojection error: " << reprojectionError << std::endl;
overallError += 0.5 * noise_->distance( reprojectionError.vector() );
//overallError += reprojectionError.vector().norm();
} catch ( CheiralityException& e) { } catch ( CheiralityException& e) {
std::cout << "Cheirality exception " << state_->ID << std::endl; std::cout << "Cheirality exception " << state_->ID << std::endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);