diff --git a/.cproject b/.cproject index 7f13b1809..80a7c399e 100644 --- a/.cproject +++ b/.cproject @@ -309,14 +309,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -343,6 +335,7 @@ make + tests/testBayesTree.run true false @@ -350,6 +343,7 @@ make + testBinaryBayesNet.run true false @@ -397,6 +391,7 @@ make + testSymbolicBayesNet.run true false @@ -404,6 +399,7 @@ make + tests/testSymbolicFactor.run true false @@ -411,6 +407,7 @@ make + testSymbolicFactorGraph.run true false @@ -426,11 +423,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -519,22 +525,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -551,6 +541,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -575,26 +581,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -679,26 +685,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -977,6 +983,7 @@ make + testGraph.run true false @@ -984,6 +991,7 @@ make + testJunctionTree.run true false @@ -991,6 +999,7 @@ make + testSymbolicBayesNetB.run true false @@ -1126,6 +1135,7 @@ make + testErrors.run true false @@ -1171,10 +1181,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testLinearContainerFactor.run true true true @@ -1259,6 +1269,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1589,7 +1607,6 @@ make - testSimulated2DOriented.run true false @@ -1629,7 +1646,6 @@ make - testSimulated2D.run true false @@ -1637,7 +1653,6 @@ make - testSimulated3D.run true false @@ -1829,7 +1844,6 @@ make - tests/testGaussianISAM2 true false @@ -1851,102 +1865,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j1 @@ -2148,6 +2066,7 @@ cpack + -G DEB true false @@ -2155,6 +2074,7 @@ cpack + -G RPM true false @@ -2162,6 +2082,7 @@ cpack + -G TGZ true false @@ -2169,6 +2090,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2302,34 +2224,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2373,6 +2359,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 636c87c73..eab37ff17 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -90,7 +90,15 @@ namespace gtsam { /* ************************************************************************* */ VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const { VectorValues result(this->dims(ordering)); - localCoordinates(cp, ordering, result); + if(this->size() != cp.size()) + throw DynamicValuesMismatched(); + for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) { + if(it1->key != it2->key) + throw DynamicValuesMismatched(); // If keys do not match + // Will throw a dynamic_cast exception if types do not match + // NOTE: this is separate from localCoordinates(cp, ordering, result) due to at() vs. insert + result.at(ordering[it1->key]) = it1->value.localCoordinates_(it2->value); + } return result; } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index c96831f05..25bd3dc2e 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -212,7 +212,7 @@ namespace gtsam { /** Get a delta config about a linearization point c0 (*this) */ VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const; - /** Get a delta config about a linearization point c0 (*this) */ + /** Get a delta config about a linearization point c0 (*this) - assumes uninitialized delta */ void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const; ///@} diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 5242fef1c..ffef740a6 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -234,6 +234,24 @@ TEST(Values, expmap_d) CHECK(config0.equals(config0)); } +/* ************************************************************************* */ +TEST(Values, localCoordinates) +{ + Values valuesA; + valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + + Ordering ordering = *valuesA.orderingArbitrary(); + + VectorValues expDelta = valuesA.zeroVectors(ordering); +// expDelta.at(ordering[key1]) = Vector_(3, 0.1, 0.2, 0.3); +// expDelta.at(ordering[key2]) = Vector_(3, 0.4, 0.5, 0.6); + + Values valuesB = valuesA.retract(expDelta, ordering); + + EXPECT(assert_equal(expDelta, valuesA.localCoordinates(valuesB, ordering))); +} + /* ************************************************************************* */ TEST(Values, extract_keys) { diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp index ea324704b..048f4a0c4 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp @@ -27,56 +27,82 @@ void LinearContainerFactor::rekeyFactor(const Ordering::InvertedMap& invOrdering } /* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor( - const JacobianFactor& factor, const Ordering& ordering) -: factor_(factor.clone()) { - rekeyFactor(ordering); +void LinearContainerFactor::initializeLinearizationPoint(const Values& linearizationPoint) { + if (!linearizationPoint.empty()) { + linearizationPoint_ = Values(); + BOOST_FOREACH(const gtsam::Key& key, this->keys()) { + linearizationPoint_->insert(key, linearizationPoint.at(key)); + } + } else { + linearizationPoint_ = boost::none; + } } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( - const HessianFactor& factor, const Ordering& ordering) + const JacobianFactor& factor, const Ordering& ordering, + const Values& linearizationPoint) : factor_(factor.clone()) { rekeyFactor(ordering); + initializeLinearizationPoint(linearizationPoint); } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( - const GaussianFactor::shared_ptr& factor, const Ordering& ordering) + const HessianFactor& factor, const Ordering& ordering, + const Values& linearizationPoint) +: factor_(factor.clone()) { + rekeyFactor(ordering); + initializeLinearizationPoint(linearizationPoint); +} + +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor( + const GaussianFactor::shared_ptr& factor, const Ordering& ordering, + const Values& linearizationPoint) : factor_(factor->clone()) { rekeyFactor(ordering); -} - -/* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor( - const GaussianFactor::shared_ptr& factor) -: factor_(factor->clone()) -{ - // Extract keys stashed in linear factor - BOOST_FOREACH(const Index& idx, factor_->keys()) - keys_.push_back(idx); -} - -/* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor, - const Ordering::InvertedMap& inverted_ordering) -: factor_(factor.clone()) { - rekeyFactor(inverted_ordering); -} - -/* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor, - const Ordering::InvertedMap& inverted_ordering) -: factor_(factor.clone()) { - rekeyFactor(inverted_ordering); + initializeLinearizationPoint(linearizationPoint); } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( const GaussianFactor::shared_ptr& factor, - const Ordering::InvertedMap& ordering) + const Values& linearizationPoint) +: factor_(factor->clone()) +{ + // Extract keys stashed in linear factor + BOOST_FOREACH(const Index& idx, factor_->keys()) + keys_.push_back(idx); + initializeLinearizationPoint(linearizationPoint); +} + +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor, + const Ordering::InvertedMap& inverted_ordering, + const Values& linearizationPoint) +: factor_(factor.clone()) { + rekeyFactor(inverted_ordering); + initializeLinearizationPoint(linearizationPoint); +} + +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor, + const Ordering::InvertedMap& inverted_ordering, + const Values& linearizationPoint) +: factor_(factor.clone()) { + rekeyFactor(inverted_ordering); + initializeLinearizationPoint(linearizationPoint); +} + +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor( + const GaussianFactor::shared_ptr& factor, + const Ordering::InvertedMap& ordering, + const Values& linearizationPoint) : factor_(factor->clone()) { rekeyFactor(ordering); + initializeLinearizationPoint(linearizationPoint); } /* ************************************************************************* */ @@ -84,20 +110,45 @@ void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyF Base::print(s+"LinearContainerFactor", keyFormatter); if (factor_) factor_->print(" Stored Factor", keyFormatter); + if (linearizationPoint_) + linearizationPoint_->print(" LinearizationPoint", keyFormatter); } /* ************************************************************************* */ bool LinearContainerFactor::equals(const NonlinearFactor& f, double tol) const { const LinearContainerFactor* jcf = dynamic_cast(&f); - return jcf && factor_->equals(*jcf->factor_, tol) && NonlinearFactor::equals(f); + if (!jcf || factor_->equals(*jcf->factor_, tol) || NonlinearFactor::equals(f)) + return false; + if (!linearizationPoint_ && !jcf->linearizationPoint_) + return true; + return jcf->linearizationPoint_ && linearizationPoint_->equals(*jcf->linearizationPoint_, tol); } /* ************************************************************************* */ double LinearContainerFactor::error(const Values& c) const { - // VectorValues vecvalues; - // // FIXME: add values correctly here - // return factor_.error(vecvalues); - return 0; // FIXME: placeholder + if (!linearizationPoint_) + return 0; + + // Extract subset of values for comparision + Values csub; + BOOST_FOREACH(const gtsam::Key& key, keys()) + csub.insert(key, c.at(key)); + + // create dummy ordering for evaluation + Ordering ordering = *csub.orderingArbitrary(); + VectorValues delta = linearizationPoint_->localCoordinates(csub, ordering); + + // Change keys on stored factor + BOOST_FOREACH(gtsam::Index& index, factor_->keys()) + index = ordering[index]; + + // compute error + double error = factor_->error(delta); + + // change keys back + factor_->keys() = keys(); + + return error; } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.h b/gtsam_unstable/nonlinear/LinearContainerFactor.h index 25a1ed98b..9affcc29e 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.h +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.h @@ -21,32 +21,40 @@ class LinearContainerFactor : public NonlinearFactor { protected: GaussianFactor::shared_ptr factor_; + boost::optional linearizationPoint_; public: /** Primary constructor: store a linear factor and decode the ordering */ - LinearContainerFactor(const JacobianFactor& factor, const Ordering& ordering); + LinearContainerFactor(const JacobianFactor& factor, const Ordering& ordering, + const Values& linearizationPoint = Values()); /** Primary constructor: store a linear factor and decode the ordering */ - LinearContainerFactor(const HessianFactor& factor, const Ordering& ordering); + LinearContainerFactor(const HessianFactor& factor, const Ordering& ordering, + const Values& linearizationPoint = Values()); /** Constructor from shared_ptr */ - LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering); + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, + const Values& linearizationPoint = Values()); /** Constructor from re-keyed factor: all indices assumed replaced with Key */ - LinearContainerFactor(const GaussianFactor::shared_ptr& factor); + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, + const Values& linearizationPoint = Values()); /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/ LinearContainerFactor(const JacobianFactor& factor, - const Ordering::InvertedMap& inverted_ordering); + const Ordering::InvertedMap& inverted_ordering, + const Values& linearizationPoint = Values()); /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/ LinearContainerFactor(const HessianFactor& factor, - const Ordering::InvertedMap& inverted_ordering); + const Ordering::InvertedMap& inverted_ordering, + const Values& linearizationPoint = Values()); /** Constructor from shared_ptr with inverted ordering*/ LinearContainerFactor(const GaussianFactor::shared_ptr& factor, - const Ordering::InvertedMap& ordering); + const Ordering::InvertedMap& ordering, + const Values& linearizationPoint = Values()); // Access @@ -70,6 +78,9 @@ public: /** get the dimension of the factor: rows of linear factor */ size_t dim() const; + /** Extract the linearization point used in recalculating error */ + const boost::optional& linearizationPoint() const { return linearizationPoint_; } + /** Apply the ordering to a graph - same as linearize(), but without needing a linearization point */ GaussianFactor::shared_ptr order(const Ordering& ordering) const; @@ -125,6 +136,7 @@ public: protected: void rekeyFactor(const Ordering& ordering); void rekeyFactor(const Ordering::InvertedMap& invOrdering); + void initializeLinearizationPoint(const Values& linearizationPoint); }; // \class LinearContainerFactor diff --git a/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp index e85b880e8..1bce66ce3 100644 --- a/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp @@ -70,6 +70,49 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol)); } +/* ************************************************************************* */ +TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { + + Ordering ordering; ordering += x1, x2, l1, l2; + + JacobianFactor expLinFactor( + ordering[l1], + Matrix_(2,2, + 2.74222, -0.0067457, + 0.0, 2.63624), + ordering[l2], + Matrix_(2,2, + -0.0455167, -0.0443573, + -0.0222154, -0.102489), + Vector_(2, 0.0277052, + -0.0533393), + diag_model2); + + Values values; + values.insert(l1, landmark1); + values.insert(l2, landmark2); + values.insert(x1, poseA1); + values.insert(x2, poseA2); + + LinearContainerFactor actFactor(expLinFactor, ordering, values); + + // Check contents + Values expLinPoint; + expLinPoint.insert(l1, landmark1); + expLinPoint.insert(l2, landmark2); + CHECK(actFactor.linearizationPoint()); + EXPECT(assert_equal(expLinPoint, *actFactor.linearizationPoint())); + + // Check error evaluation + VectorValues delta = values.zeroVectors(ordering); + delta.at(ordering[l1]) = Vector_(2, 1.0, 2.0); + delta.at(ordering[l2]) = Vector_(2, 3.0, 4.0); + Values noisyValues = values.retract(delta, ordering); + double expError = expLinFactor.error(delta); + EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol); + EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors(ordering)), actFactor.error(values), tol); +} + /* ************************************************************************* */ TEST( testLinearContainerFactor, generic_hessian_factor ) { Matrix G11 = Matrix_(1,1, 1.0);