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

add nonlinear triangulation back. Some unit tests fail again

release/4.3a0
cbeall3 2015-07-27 23:27:20 -04:00
parent b5aa7fb7f0
commit da9078cf3b
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -285,6 +285,10 @@ public:
if(cheirality_ok == false) { if(cheirality_ok == false) {
result_ = TriangulationResult::BehindCamera(); result_ = TriangulationResult::BehindCamera();
} }
pw_avg = triangulateNonlinear(cameras, measured_, pw_avg);
result_ = TriangulationResult(pw_avg); result_ = TriangulationResult(pw_avg);
} }
@ -537,6 +541,7 @@ public:
// //
// return Base::totalReprojectionError(cameras, backprojected); // return Base::totalReprojectionError(cameras, backprojected);
} else { } else {
std::cout << "Degenerate factor" << std::endl;
// if we don't want to manage the exceptions we discard the factor // if we don't want to manage the exceptions we discard the factor
return 0.0; return 0.0;
} }