From da9078cf3bb75b3477f568032e427ea88c2e78e5 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 27 Jul 2015 23:27:20 -0400 Subject: [PATCH] add nonlinear triangulation back. Some unit tests fail again --- gtsam_unstable/slam/SmartStereoProjectionFactor.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index d57d1ca25..f0a4dd4f5 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -285,6 +285,10 @@ public: if(cheirality_ok == false) { result_ = TriangulationResult::BehindCamera(); } + + pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); + + result_ = TriangulationResult(pw_avg); } @@ -537,6 +541,7 @@ public: // // return Base::totalReprojectionError(cameras, backprojected); } else { + std::cout << "Degenerate factor" << std::endl; // if we don't want to manage the exceptions we discard the factor return 0.0; }