From 51dca5b2d0dc944b7d9e0c7630bc07292451abc1 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 30 Jan 2012 21:23:02 +0000 Subject: [PATCH] (in branch) Fixed Value base class assignment operator, ISAM2 expmap function, and unit tests --- gtsam/base/DerivedValue.h | 17 +++++++++++++++++ gtsam/base/Value.h | 3 +++ gtsam/linear/VectorValues.h | 2 +- gtsam/nonlinear/ISAM2-impl-inl.h | 6 ++++-- gtsam/nonlinear/tests/testDynamicValues.cpp | 2 +- gtsam/slam/tests/testProjectionFactor.cpp | 2 -- tests/Makefile.am | 2 +- tests/testExtendedKalmanFilter.cpp | 4 ++-- tests/testNonlinearFactorGraph.cpp | 6 +++++- 9 files changed, 34 insertions(+), 10 deletions(-) diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index c30258919..f59303675 100644 --- a/gtsam/base/DerivedValue.h +++ b/gtsam/base/DerivedValue.h @@ -73,6 +73,23 @@ public: return (static_cast(this))->localCoordinates(derivedValue2); } + /// Assignment operator + virtual Value& operator=(const Value& rhs) { + // Cast the base class Value pointer to a derived class pointer + const DERIVED& derivedRhs = dynamic_cast(rhs); + + // Do the assignment and return the result + return (static_cast(this))->operator=(derivedRhs); + } + +protected: + /// Assignment operator, protected because only the Value or DERIVED + /// assignment operators should be used. + DerivedValue& operator=(const DerivedValue& rhs) { + // Nothing to do, do not call base class assignment operator + return *this; + } + }; } /* namespace gtsam */ diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 1e3c809c3..8d0b2dd05 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -136,6 +136,9 @@ namespace gtsam { */ virtual Vector localCoordinates_(const Value& value) const = 0; + /** Assignment operator */ + virtual Value& operator=(const Value& rhs) = 0; + /** Virutal destructor */ virtual ~Value() {} diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 33351ceb7..0be2cad19 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -175,7 +175,7 @@ namespace gtsam { /// @name Advanced Constructors /// @{ - /** Construct from a container of variable dimensions (in variable order). */ + /** Construct from a container of variable dimensions (in variable order), without initializing any values. */ template VectorValues(const CONTAINER& dimensions) { append(dimensions); } diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index 8b0c0b99c..7ddd2f8c6 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -245,10 +245,12 @@ struct _SelectiveExpmapAndClear { else cout << " " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; } - assert(delta[var].size() == (int)it_x->second.dim()); + assert(delta[var].size() == (int)it_x->second->dim()); assert(delta[var].unaryExpr(&isfinite).all()); if(mask[var]) { - it_x->second = it_x->second.retract(delta[var]); + Value* retracted = it_x->second->retract_(delta[var]); + *it_x->second = *retracted; + retracted->deallocate_(); if(invalidate) (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) } diff --git a/gtsam/nonlinear/tests/testDynamicValues.cpp b/gtsam/nonlinear/tests/testDynamicValues.cpp index f7493f1b1..51ffd0320 100644 --- a/gtsam/nonlinear/tests/testDynamicValues.cpp +++ b/gtsam/nonlinear/tests/testDynamicValues.cpp @@ -162,7 +162,7 @@ TEST(DynamicValues, expmap_b) config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); Ordering ordering(*config0.orderingArbitrary()); - VectorValues increment(config0.dims(ordering)); + VectorValues increment(VectorValues::Zero(config0.dims(ordering))); increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5); DynamicValues expected; diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index ae2aee55f..f5f41fd2e 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -85,8 +85,6 @@ TEST( ProjectionFactor, error ) Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2); Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2); VectorValues delta(expected_config.dims(ordering)); - ordering.print("ordering: "); - delta.print("delta: "); delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.); delta[ordering["l1"]] = Vector_(3, 1.,2.,3.); DynamicValues actual_config = config.retract(delta, ordering); diff --git a/tests/Makefile.am b/tests/Makefile.am index 567a1ff0a..086904640 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -20,7 +20,7 @@ check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph check_PROGRAMS += testNonlinearISAM check_PROGRAMS += testBoundingConstraint #check_PROGRAMS += testPose2SLAMwSPCG -#check_PROGRAMS += testGaussianISAM2 +check_PROGRAMS += testGaussianISAM2 check_PROGRAMS += testExtendedKalmanFilter check_PROGRAMS += testRot3Optimization diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 712c986ae..1ff5fe047 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -365,8 +365,8 @@ public: TEST( ExtendedKalmanFilter, nonlinear ) { // Create the set of expected output TestValues (generated using Matlab Kalman Filter) - Point2 expected_predict[10]; - Point2 expected_update[10]; + Point2 expected_predict[11]; + Point2 expected_update[11]; expected_predict[1] = Point2(0.81,0.99); expected_update[1] = Point2(0.824926197027,0.29509808); expected_predict[2] = Point2(0.680503230541,0.24343413); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 6b926c6d7..8606c027f 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -75,8 +75,12 @@ TEST( Graph, keys ) /* ************************************************************************* */ TEST( Graph, GET_ORDERING) { - Ordering expected; expected += "x1","l1","x2"; +// Ordering expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1 + Ordering expected; expected += "l1","x2","x1"; // For starting with l1,x1,x2 Graph nlfg = createNonlinearFactorGraph(); + SymbolicFactorGraph::shared_ptr symbolic; + Ordering::shared_ptr ordering; + boost::tie(symbolic, ordering) = nlfg.symbolic(createNoisyValues()); Ordering actual = *nlfg.orderingCOLAMD(createNoisyValues()); CHECK(assert_equal(expected,actual)); }