From d7e6f43fa7a2d23fbf282e12202d6ca9f992c534 Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Wed, 14 Aug 2013 19:12:21 +0000 Subject: [PATCH] Updated to work with unordered GTSAM --- gtsam_unstable/slam/SmartProjectionFactor.h | 13 ++-- .../slam/tests/testSmartProjectionFactor.cpp | 61 ++++++++----------- 2 files changed, 29 insertions(+), 45 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 5572f04e8..dca517e97 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -147,7 +148,7 @@ namespace gtsam { } /// linearize returns a Hessianfactor that is an approximation of error(p) - virtual boost::shared_ptr linearize(const Values& values, const Ordering& ordering) const { + virtual boost::shared_ptr linearize(const Values& values) const { bool debug = false; bool blockwise = true; @@ -185,22 +186,16 @@ namespace gtsam { std::cout << "point " << *point << std::endl; } - std::vector js; std::vector Gs(keys_.size()*(keys_.size()+1)/2); std::vector gs(keys_.size()); double f=0; - // fill in the keys - BOOST_FOREACH(const Key& k, keys_) { - js += ordering[k]; - } - // point is behind one of the cameras, turn factor off by setting everything to 0 if (!point) { std::cout << "WARNING: Could not triangulate during linearize" << 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(js, Gs, gs, f)); + return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); } // For debug only @@ -374,7 +369,7 @@ namespace gtsam { } // ========================================================================================================== - return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); + return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); } /** diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index 2e277ab76..a5df442f3 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -19,12 +19,7 @@ #include #include -TEST(SmartProjectionFactor, disabled) -{ - CHECK(("*** testSmartProjectionFactor is disabled *** - Needs conversion for unordered", 0)); -} -#if 0 #include #include @@ -234,20 +229,20 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); NonlinearFactorGraph graphWithOriginalFactor; - graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K)); + graphWithOriginalFactor.push_back(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K)); + graphWithOriginalFactor.push_back(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K)); + graphWithOriginalFactor.push_back(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K)); + graphWithOriginalFactor.push_back(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K)); + graphWithOriginalFactor.push_back(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K)); + graphWithOriginalFactor.push_back(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K)); - graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K)); + graphWithOriginalFactor.push_back(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K)); + graphWithOriginalFactor.push_back(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K)); + graphWithOriginalFactor.push_back(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K)); - graphWithOriginalFactor.add(PriorFactor(x1, pose1, noisePrior)); - graphWithOriginalFactor.add(PriorFactor(x2, pose2, noisePrior)); + graphWithOriginalFactor.push_back(PriorFactor(x1, pose1, noisePrior)); + graphWithOriginalFactor.push_back(PriorFactor(x2, pose2, noisePrior)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); Values valuesOriginalFactor; @@ -262,8 +257,8 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ graphWithSmartFactor.push_back(smartFactor1); graphWithSmartFactor.push_back(smartFactor2); graphWithSmartFactor.push_back(smartFactor3); - graphWithSmartFactor.add(PriorFactor(x1, pose1, noisePrior)); - graphWithSmartFactor.add(PriorFactor(x2, pose2, noisePrior)); + graphWithSmartFactor.push_back(PriorFactor(x1, pose1, noisePrior)); + graphWithSmartFactor.push_back(PriorFactor(x2, pose2, noisePrior)); Values valuesSmartFactor; valuesSmartFactor.insert(x1, pose1); @@ -442,22 +437,22 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){ NonlinearFactorGraph graph; // 1. Project three landmarks into three cameras and triangulate - graph.add(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K)); - graph.add(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K)); - graph.add(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K)); + graph.push_back(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K)); + graph.push_back(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K)); + graph.push_back(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K)); // - graph.add(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K)); - graph.add(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K)); - graph.add(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K)); + graph.push_back(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K)); + graph.push_back(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K)); + graph.push_back(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K)); - graph.add(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K)); - graph.add(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K)); - graph.add(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K)); + graph.push_back(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K)); + graph.push_back(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K)); + graph.push_back(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.add(PriorFactor(x1, pose1, noisePrior)); - graph.add(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); Values values; @@ -520,11 +515,7 @@ TEST( SmartProjectionFactor, Hessian ){ values.insert(x2, pose2); // values.insert(L(1), landmark1); - Ordering ordering; - ordering.push_back(x1); - ordering.push_back(x2); - - boost::shared_ptr hessianFactor = smartFactor.linearize(values, ordering); + boost::shared_ptr hessianFactor = smartFactor.linearize(values); hessianFactor->print("Hessian factor \n"); // compute triangulation from linearization point @@ -538,8 +529,6 @@ TEST( SmartProjectionFactor, Hessian ){ } -#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */