diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 3c4de36d4..5fcdb24fe 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -130,13 +130,13 @@ namespace gtsam { } // Linearize is over-written, because base linearization tries to whiten - virtual JacobianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { + virtual GaussianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { const T& xj = x[this->key_]; Matrix A; Vector b = evaluateError(xj, A); // TODO pass unwhitened + noise model to Gaussian factor SharedDiagonal model = noiseModel::Constrained::All(b.size()); - return JacobianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model)); + return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model)); } private: diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 5986882e0..8242bbfa8 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -145,7 +145,7 @@ namespace gtsam { } /** linearize to a GaussianFactor */ - virtual boost::shared_ptr + virtual boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const = 0; /** @@ -237,7 +237,7 @@ namespace gtsam { * Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z * Hence b = z - h(x0) = - error_vector(x) */ - virtual boost::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { + virtual boost::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { const X& xj = x[key_]; Matrix A; Vector b = - evaluateError(xj, A); @@ -246,10 +246,10 @@ namespace gtsam { SharedDiagonal constrained = boost::shared_dynamic_cast(this->noiseModel_); if (constrained.get() != NULL) - return JacobianFactor::shared_ptr(new JacobianFactor(var, A, b, constrained)); + return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b, constrained)); this->noiseModel_->WhitenInPlace(A); this->noiseModel_->whitenInPlace(b); - return JacobianFactor::shared_ptr(new JacobianFactor(var, A, b, + return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b, noiseModel::Unit::Create(b.size()))); } @@ -348,7 +348,7 @@ namespace gtsam { * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z * Hence b = z - h(x1,x2) = - error_vector(x) */ - boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const { + boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const { const X1& x1 = c[key1_]; const X2& x2 = c[key2_]; Matrix A1, A2; @@ -365,10 +365,10 @@ namespace gtsam { this->noiseModel_->WhitenInPlace(A2); this->noiseModel_->whitenInPlace(b); if(var1 < var2) - return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, + return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, A2, b, noiseModel::Unit::Create(b.size()))); else - return JacobianFactor::shared_ptr(new JacobianFactor(var2, A2, var1, + return GaussianFactor::shared_ptr(new JacobianFactor(var2, A2, var1, A1, b, noiseModel::Unit::Create(b.size()))); } @@ -492,7 +492,7 @@ namespace gtsam { * Ax-b \approx h(x1+dx1,x2+dx2,x3+dx3)-z = h(x1,x2,x3) + A2*dx1 + A2*dx2 + A3*dx3 - z * Hence b = z - h(x1,x2,x3) = - error_vector(x) */ - boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const { + boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const { const X1& x1 = c[key1_]; const X2& x2 = c[key2_]; const X3& x3 = c[key3_]; @@ -503,7 +503,7 @@ namespace gtsam { SharedDiagonal constrained = boost::shared_dynamic_cast(this->noiseModel_); if (constrained.get() != NULL) { - return JacobianFactor::shared_ptr( + return GaussianFactor::shared_ptr( new JacobianFactor(var1, A1, var2, A2, var3, A3, b, constrained)); } this->noiseModel_->WhitenInPlace(A1); @@ -511,26 +511,26 @@ namespace gtsam { this->noiseModel_->WhitenInPlace(A3); this->noiseModel_->whitenInPlace(b); if(var1 < var2 && var2 < var3) - return JacobianFactor::shared_ptr( + return GaussianFactor::shared_ptr( new JacobianFactor(var1, A1, var2, A2, var3, A3, b, noiseModel::Unit::Create(b.size()))); else if(var2 < var1 && var1 < var3) - return JacobianFactor::shared_ptr( + return GaussianFactor::shared_ptr( new JacobianFactor(var2, A2, var1, A1, var3, A3, b, noiseModel::Unit::Create(b.size()))); else if(var1 < var3 && var3 < var2) - return JacobianFactor::shared_ptr( + return GaussianFactor::shared_ptr( new JacobianFactor(var1, A1, var3, A3, var2, A2, b, noiseModel::Unit::Create(b.size()))); else if(var2 < var3 && var3 < var1) - return JacobianFactor::shared_ptr( + return GaussianFactor::shared_ptr( new JacobianFactor(var2, A2, var3, A3, var1, A1, b, noiseModel::Unit::Create(b.size()))); else if(var3 < var1 && var1 < var2) - return JacobianFactor::shared_ptr( + return GaussianFactor::shared_ptr( new JacobianFactor(var3, A3, var1, A1, var2, A2, b, noiseModel::Unit::Create(b.size()))); else if(var3 < var2 && var2 < var1) - return JacobianFactor::shared_ptr( + return GaussianFactor::shared_ptr( new JacobianFactor(var3, A3, var2, A2, var1, A1, b, noiseModel::Unit::Create(b.size()))); else { assert(false); - return JacobianFactor::shared_ptr(); + return GaussianFactor::shared_ptr(); } } diff --git a/gtsam/nonlinear/NonlinearFactorGraph-inl.h b/gtsam/nonlinear/NonlinearFactorGraph-inl.h index 254343401..638c2da8b 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph-inl.h +++ b/gtsam/nonlinear/NonlinearFactorGraph-inl.h @@ -139,21 +139,20 @@ namespace gtsam { /* ************************************************************************* */ template - typename FactorGraph::shared_ptr NonlinearFactorGraph::linearize( + typename FactorGraph::shared_ptr NonlinearFactorGraph::linearize( const VALUES& config, const Ordering& ordering) const { // create an empty linear FG - typename FactorGraph::shared_ptr linearFG(new FactorGraph< - JacobianFactor> ); + typename FactorGraph::shared_ptr linearFG(new FactorGraph); linearFG->reserve(this->size()); // linearize all factors BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) { - JacobianFactor::shared_ptr lf = factor->linearize(config, ordering); + GaussianFactor::shared_ptr lf = factor->linearize(config, ordering); if (lf) linearFG->push_back(lf); } else - linearFG->push_back(FactorGraph::sharedFactor()); + linearFG->push_back(GaussianFactor::shared_ptr()); } return linearFG; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index e3f3f2eea..8179ee6cf 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -89,7 +89,7 @@ namespace gtsam { /** * linearize a nonlinear factor graph */ - boost::shared_ptr > + boost::shared_ptr > linearize(const VALUES& config, const Ordering& ordering) const; private: diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 6a8339c8e..fe0feffcc 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -273,7 +273,7 @@ namespace example { } /* ************************************************************************* */ - pair, Ordering> createSmoother(int T, boost::optional ordering) { + pair, Ordering> createSmoother(int T, boost::optional ordering) { Graph nlfg; Values poses; boost::tie(nlfg, poses) = createNonlinearSmoother(T); @@ -431,7 +431,7 @@ namespace example { } /* ************************************************************************* */ - boost::tuple, Ordering, VectorValues> planarGraph(size_t N) { + boost::tuple, Ordering, VectorValues> planarGraph(size_t N) { // create empty graph NonlinearFactorGraph nlfg; diff --git a/gtsam/slam/smallExample.h b/gtsam/slam/smallExample.h index b88c9a442..3cf4a7719 100644 --- a/gtsam/slam/smallExample.h +++ b/gtsam/slam/smallExample.h @@ -93,7 +93,7 @@ namespace gtsam { * Create a Kalman smoother by linearizing a non-linear factor graph * @param T number of time-steps */ - std::pair, Ordering> createSmoother(int T, boost::optional ordering = boost::none); + std::pair, Ordering> createSmoother(int T, boost::optional ordering = boost::none); /* ******************************************************* */ // Linear Constrained Examples @@ -133,7 +133,7 @@ namespace gtsam { * -x11-x21-x31 * with x11 clamped at (1,1), and others related by 2D odometry. */ - boost::tuple, Ordering, VectorValues> planarGraph(size_t N); + boost::tuple, Ordering, VectorValues> planarGraph(size_t N); /* * Create canonical ordering for planar graph that also works for tree diff --git a/gtsam/slam/tests/testPose2Factor.cpp b/gtsam/slam/tests/testPose2Factor.cpp index 87a7335a0..282fb4ba9 100644 --- a/gtsam/slam/tests/testPose2Factor.cpp +++ b/gtsam/slam/tests/testPose2Factor.cpp @@ -52,7 +52,8 @@ TEST( Pose2Factor, error ) // Actual linearization Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = factor.linearize(x0, ordering); + boost::shared_ptr linear = + boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); // Check error at x0, i.e. delta = zero ! VectorValues delta(x0.dims(ordering)); @@ -88,7 +89,8 @@ TEST( Pose2Factor, rhs ) // Actual linearization Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = factor.linearize(x0, ordering); + boost::shared_ptr linear = + boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); // Check RHS Pose2 hx0 = p1.between(p2); @@ -134,7 +136,8 @@ TEST( Pose2Factor, linearize ) JacobianFactor expected(ordering["x1"], expectedH1, ordering["x2"], expectedH2, expected_b, probModel1); // Actual linearization - boost::shared_ptr actual = factor.linearize(x0, ordering); + boost::shared_ptr actual = + boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); CHECK(assert_equal(expected,*actual)); // Numerical do not work out because BetweenFactor is approximate ? diff --git a/gtsam/slam/tests/testPose2Prior.cpp b/gtsam/slam/tests/testPose2Prior.cpp index b83ac0681..1a5be817a 100644 --- a/gtsam/slam/tests/testPose2Prior.cpp +++ b/gtsam/slam/tests/testPose2Prior.cpp @@ -44,7 +44,8 @@ TEST( Pose2Prior, error ) // Actual linearization Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = factor.linearize(x0, ordering); + boost::shared_ptr linear = + boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); // Check error at x0, i.e. delta = zero ! VectorValues delta(x0.dims(ordering)); @@ -86,7 +87,8 @@ TEST( Pose2Prior, linearize ) // Actual linearization Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr actual = factor.linearize(x0, ordering); + boost::shared_ptr actual = + boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); // Test with numerical derivative Matrix numericalH = numericalDerivative11(h, prior); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index f7b3419b3..6c1530508 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -74,11 +74,11 @@ TEST( Pose2Graph, linearization ) config.insert(2,p2); // Linearize Ordering ordering(*config.orderingArbitrary()); - boost::shared_ptr > lfg_linearized = graph.linearize(config, ordering); + boost::shared_ptr > lfg_linearized = graph.linearize(config, ordering); //lfg_linearized->print("lfg_actual"); // the expected linear factor - FactorGraph lfg_expected; + FactorGraph lfg_expected; Matrix A1 = Matrix_(3,3, 0.0,-2.0, -4.2, 2.0, 0.0, -4.2, diff --git a/gtsam/slam/tests/testVSLAMFactor.cpp b/gtsam/slam/tests/testVSLAMFactor.cpp index fdea48ad0..9dd849aa2 100644 --- a/gtsam/slam/tests/testVSLAMFactor.cpp +++ b/gtsam/slam/tests/testVSLAMFactor.cpp @@ -67,15 +67,16 @@ TEST( ProjectionFactor, error ) Vector b = Vector_(2,3.,0.); SharedDiagonal probModel1 = noiseModel::Unit::Create(2); JacobianFactor expected(ordering["x1"], Ax1, ordering["l1"], Al1, b, probModel1); - JacobianFactor::shared_ptr actual = factor->linearize(config, ordering); + JacobianFactor::shared_ptr actual = + boost::dynamic_pointer_cast(factor->linearize(config, ordering)); CHECK(assert_equal(expected,*actual,1e-3)); // linearize graph Graph graph; graph.push_back(factor); - FactorGraph expected_lfg; + FactorGraph expected_lfg; expected_lfg.push_back(actual); - boost::shared_ptr > actual_lfg = graph.linearize(config, ordering); + boost::shared_ptr > actual_lfg = graph.linearize(config, ordering); CHECK(assert_equal(expected_lfg,*actual_lfg)); // expmap on a config diff --git a/tests/testInference.cpp b/tests/testInference.cpp index 4bcd2d56d..76eec9b0d 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -85,7 +85,7 @@ TEST( Inference, marginals2) init.insert(PointKey(0), Point2(1.0,1.0)); Ordering ordering(*fg.orderingCOLAMD(init)); - FactorGraph::shared_ptr gfg(fg.linearize(init, ordering)); + FactorGraph::shared_ptr gfg(fg.linearize(init, ordering)); GaussianMultifrontalSolver solver(*gfg); solver.marginalFactor(ordering[PointKey(0)]); } diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 7d96dd7d9..6292d1f20 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -50,8 +50,8 @@ TEST ( NonlinearEquality, linearization ) { // check linearize SharedDiagonal constraintModel = noiseModel::Constrained::All(3); JacobianFactor expLF(0, eye(3), zero(3), constraintModel); - JacobianFactor::shared_ptr actualLF = nle->linearize(linearize, *linearize.orderingArbitrary()); - EXPECT(assert_equal(*actualLF, expLF)); + GaussianFactor::shared_ptr actualLF = nle->linearize(linearize, *linearize.orderingArbitrary()); + EXPECT(assert_equal(*actualLF, (const GaussianFactor&)expLF)); } /* ********************************************************************** */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index b32defaed..a55960912 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -200,13 +200,13 @@ TEST( NonlinearFactor, linearize_constraint1 ) Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); - JacobianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary()); + GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary()); // create expected Ordering ord(*config.orderingArbitrary()); Vector b = Vector_(2, 0., -3.); JacobianFactor expected(ord["x1"], eye(2), b, constraint); - CHECK(assert_equal(expected, *actual)); + CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } /* ************************************************************************* */ @@ -221,13 +221,13 @@ TEST( NonlinearFactor, linearize_constraint2 ) Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0)); - JacobianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); + GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); // create expected Ordering ord(*config.orderingArbitrary()); Vector b = Vector_(2, -3., -3.); JacobianFactor expected(ord["x1"], -1*eye(2), ord["l1"], eye(2), b, constraint); - CHECK(assert_equal(expected, *actual)); + CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } /* ************************************************************************* */ diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index bd938a97b..3135b71e5 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -96,8 +96,8 @@ TEST( Graph, linearize ) { Graph fg = createNonlinearFactorGraph(); Values initial = createNoisyValues(); - boost::shared_ptr > linearized = fg.linearize(initial, *initial.orderingArbitrary()); - FactorGraph expected = createGaussianFactorGraph(*initial.orderingArbitrary()); + boost::shared_ptr > linearized = fg.linearize(initial, *initial.orderingArbitrary()); + FactorGraph expected = createGaussianFactorGraph(*initial.orderingArbitrary()); CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations }