diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h new file mode 100644 index 000000000..d1c88466b --- /dev/null +++ b/gtsam/slam/AntiFactor.h @@ -0,0 +1,138 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file AntiFactor.h + * @author Stephen Williams + **/ +#pragma once + +#include + +#include +#include + +namespace gtsam { + + /** + * A class for a measurement predicted by "between(config[key1],config[key2])" + * KEY1::Value is the Lie Group type + * T is the measurement type, by default the same + */ + template + class AntiFactor: public NonlinearFactor { + + private: + + typedef AntiFactor This; + typedef NonlinearFactor Base; + typedef typename NonlinearFactor::shared_ptr sharedFactor; + + sharedFactor factor_; + + public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + AntiFactor() {} + + /** constructor - Creates the equivalent AntiFactor from an existing factor */ + AntiFactor(typename NonlinearFactor::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {} + + virtual ~AntiFactor() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s) const { + std::cout << s << "AntiFactor version of:" << std::endl; + factor_->print(s); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** + * Calculate the error of the factor + * This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian. + * You can override this for systems with unusual noise models. + */ + double error(const VALUES& c) const { return -factor_->error(c); } + + /** get the dimension of the factor (number of rows on linearization) */ + size_t dim() const { return factor_->dim(); } + + /** + * Checks whether a factor should be used based on a set of values. + * This is primarily used to implment inequality constraints that + * require a variable active set. For all others, the default implementation + * returning true solves this problem. + * + * In an inequality/bounding constraint, this active() returns true + * when the constraint is *NOT* fulfilled. + * @return true if the constraint is active + */ + bool active(const VALUES& c) const { return factor_->active(c); } + + /** linearize to a GaussianFactor */ + boost::shared_ptr + linearize(const VALUES& c, const Ordering& ordering) const { + + // Generate the linearized factor from the contained nonlinear factor + GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering); + + // Cast the GaussianFactor to a Hessian + HessianFactor::shared_ptr hessianFactor = boost::dynamic_pointer_cast(gaussianFactor); + + // If the cast fails, convert it to a Hessian + if(!hessianFactor){ + hessianFactor = HessianFactor::shared_ptr(new HessianFactor(*gaussianFactor)); + } + + // Copy Hessian Blocks from Hessian factor and invert + std::vector js; + std::vector Gs; + std::vector gs; + double f; + js.insert(js.end(), hessianFactor->begin(), hessianFactor->end()); + for(size_t i = 0; i < js.size(); ++i){ + for(size_t j = i; j < js.size(); ++j){ + Gs.push_back( -hessianFactor->info(hessianFactor->begin()+i, hessianFactor->begin()+j) ); + } + gs.push_back( -hessianFactor->linearTerm(hessianFactor->begin()+i) ); + } + f = -hessianFactor->constantTerm(); + + // Create the Anti-Hessian Factor from the negated blocks + return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); + } + + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("AntiFactor", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(factor_); + } + }; // \class AntiFactor + +} /// namespace gtsam diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index fa51a9512..71eb94d64 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -26,6 +26,8 @@ check_PROGRAMS += tests/testSimulated3D # Generic SLAM headers headers += BetweenFactor.h PriorFactor.h PartialPriorFactor.h headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h +headers += AntiFactor.h +check_PROGRAMS += tests/testAntiFactor # Generic constraint headers headers += BoundingConstraint.h diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp new file mode 100644 index 000000000..a7f91c320 --- /dev/null +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -0,0 +1,139 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testAntiFactor.cpp + * @brief Unit test for the AntiFactor + * @author Stephen Williams + */ + +#include + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( AntiFactor, NegativeHessian) +{ + // The AntiFactor should produce a Hessian Factor with negative matrices + + // Create linearization points + Pose3 pose1(Rot3(), Point3(0, 0, 0)); + Pose3 pose2(Rot3(), Point3(2, 1, 3)); + Pose3 z(Rot3(), Point3(1, 1, 1)); + SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); + + // Create a configuration corresponding to the ground truth + boost::shared_ptr values(new pose3SLAM::Values()); + values->insert(1, pose1); + values->insert(2, pose2); + + // Define an elimination ordering + Ordering::shared_ptr ordering(new Ordering()); + ordering->insert(pose3SLAM::Key(1), 0); + ordering->insert(pose3SLAM::Key(2), 1); + + + // Create a "standard" factor + BetweenFactor::shared_ptr originalFactor(new BetweenFactor(1, 2, z, sigma)); + + // Linearize it into a Jacobian Factor + GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); + + // Convert it to a Hessian Factor + HessianFactor::shared_ptr originalHessian = HessianFactor::shared_ptr(new HessianFactor(*originalJacobian)); + + // Create the AntiFactor version of the original nonlinear factor + AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor)); + + // Linearize the AntiFactor into a Hessian Factor + GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering); + HessianFactor::shared_ptr antiHessian = boost::dynamic_pointer_cast(antiGaussian); + + + // Compare Hessian blocks + size_t variable_count = originalFactor->size(); + for(size_t i = 0; i < variable_count; ++i){ + for(size_t j = i; j < variable_count; ++j){ + Matrix expected_G = -originalHessian->info(originalHessian->begin()+i, originalHessian->begin()+j); + Matrix actual_G = antiHessian->info(antiHessian->begin()+i, antiHessian->begin()+j); + CHECK(assert_equal(expected_G, actual_G, 1e-5)); + } + Vector expected_g = -originalHessian->linearTerm(originalHessian->begin()+i); + Vector actual_g = antiHessian->linearTerm(antiHessian->begin()+i); + CHECK(assert_equal(expected_g, actual_g, 1e-5)); + } + double expected_f = -originalHessian->constantTerm(); + double actual_f = antiHessian->constantTerm(); + EXPECT_DOUBLES_EQUAL(expected_f, actual_f, 1e-5); +} + +/* ************************************************************************* */ +TEST( AntiFactor, EquivalentBayesNet) +{ + // Test the AntiFactor by creating a simple graph and eliminating into a BayesNet + // Then add an additional factor and the corresponding AntiFactor and eliminate + // The resulting BayesNet should be identical to the first + + Pose3 pose1(Rot3(), Point3(0, 0, 0)); + Pose3 pose2(Rot3(), Point3(2, 1, 3)); + Pose3 z(Rot3(), Point3(1, 1, 1)); + SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); + + boost::shared_ptr graph(new pose3SLAM::Graph()); + graph->addPrior(1, pose1, sigma); + graph->addConstraint(1, 2, pose1.between(pose2), sigma); + + // Create a configuration corresponding to the ground truth + boost::shared_ptr values(new pose3SLAM::Values()); + values->insert(1, pose1); + values->insert(2, pose2); + + // Define an elimination ordering + Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); + + // Eliminate into a BayesNet + GaussianSequentialSolver solver1(*graph->linearize(*values, *ordering)); + GaussianBayesNet::shared_ptr expectedBayesNet = solver1.eliminate(); + + // Back-substitute to find the optimal deltas + VectorValues expectedDeltas = optimize(*expectedBayesNet); + + // Add an additional factor between Pose1 and Pose2 + pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(1, 2, z, sigma)); + graph->push_back(f1); + + // Add the corresponding AntiFactor between Pose1 and Pose2 + AntiFactor::shared_ptr f2(new AntiFactor(f1)); + graph->push_back(f2); + + // Again, Eliminate into a BayesNet + GaussianSequentialSolver solver2(*graph->linearize(*values, *ordering)); + GaussianBayesNet::shared_ptr actualBayesNet = solver2.eliminate(); + + // Back-substitute to find the optimal deltas + VectorValues actualDeltas = optimize(*actualBayesNet); + + // Verify the BayesNets are identical + CHECK(assert_equal(*expectedBayesNet, *actualBayesNet, 1e-5)); + CHECK(assert_equal(expectedDeltas, actualDeltas, 1e-5)); +} + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */