/** * @file testNonlinearFactor.cpp * @brief Unit tests for Non-Linear Factor, * create a non linear factor graph and a configuration for it and * calculate the error for the factor. * @author Christian Potthast **/ /*STL/C++*/ #include #include #include "Matrix.h" #include "smallExample.h" #include "Simulated2DMeasurement.h" #include "GaussianFactor.h" using namespace std; using namespace gtsam; typedef boost::shared_ptr > shared_nlf; /* ************************************************************************* */ TEST( NonlinearFactor, equals ) { double sigma = 1.0; // create two nonlinear2 factors Vector z3(2); z3(0) = 0. ; z3(1) = -1.; Simulated2DMeasurement f0(z3, sigma, "x1", "l1"); // measurement between x2 and l1 Vector z4(2); z4(0)= -1.5 ; z4(1) = -1.; Simulated2DMeasurement f1(z4, sigma, "x2", "l1"); CHECK(assert_equal(f0,f0)); CHECK(f0.equals(f0)); CHECK(!f0.equals(f1)); CHECK(!f1.equals(f0)); } /* ************************************************************************* */ TEST( NonlinearFactor, equals2 ) { // create a non linear factor graph ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); // get two factors shared_nlf f0 = fg[0], f1 = fg[1]; CHECK(f0->equals(*f0)); CHECK(!f0->equals(*f1)); CHECK(!f1->equals(*f0)); } /* ************************************************************************* */ TEST( NonlinearFactor, NonlinearFactor ) { // create a non linear factor graph ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); // create a configuration for the non linear factor graph VectorConfig cfg = createNoisyConfig(); // get the factor "f1" from the factor graph shared_nlf factor = fg[0]; // calculate the error_vector from the factor "f1" Vector actual_e = factor->error_vector(cfg); Vector e(2); e(0) = -0.1; e(1) = -0.1; CHECK(assert_equal(e,actual_e)); // the expected value for the error from the factor // error_vector / sigma = [0.1 0.1]/0.1 = [1;1] // error = 0.5 * [1 1] * [1;1] = 1 double expected = 1.0; // calculate the error from the factor "f1" double actual = factor->error(cfg); DOUBLES_EQUAL(expected,actual,0.00000001); } /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f1 ) { // Grab a non-linear factor ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[0]); // We linearize at noisy config from SmallExample VectorConfig c = createNoisyConfig(); GaussianFactor::shared_ptr actual = nlf->linearize(c); GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactor::shared_ptr expected = lfg[0]; CHECK(expected->equals(*actual)); } /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f2 ) { // Grab a non-linear factor ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[1]); // We linearize at noisy config from SmallExample VectorConfig c = createNoisyConfig(); GaussianFactor::shared_ptr actual = nlf->linearize(c); GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactor::shared_ptr expected = lfg[1]; CHECK(expected->equals(*actual)); } /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f3 ) { // Grab a non-linear factor ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[2]); // We linearize at noisy config from SmallExample VectorConfig c = createNoisyConfig(); GaussianFactor::shared_ptr actual = nlf->linearize(c); GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactor::shared_ptr expected = lfg[2]; CHECK(expected->equals(*actual)); } /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f4 ) { // Grab a non-linear factor ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[3]); // We linearize at noisy config from SmallExample VectorConfig c = createNoisyConfig(); GaussianFactor::shared_ptr actual = nlf->linearize(c); GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactor::shared_ptr expected = lfg[3]; CHECK(expected->equals(*actual)); } /* ************************************************************************* */ TEST( NonlinearFactor, size ) { // create a non linear factor graph ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); // create a configuration for the non linear factor graph VectorConfig cfg = createNoisyConfig(); // get some factors from the graph shared_nlf factor1 = fg[0]; shared_nlf factor2 = fg[1]; shared_nlf factor3 = fg[2]; CHECK(factor1->size() == 1); CHECK(factor2->size() == 2); CHECK(factor3->size() == 2); } /* ************************************************************************* */ #include #include "Pose2.h" class Pose2Config : public map { public: Pose2 get(string key) const { map::const_iterator it = find(key); if (it==end()) throw std::invalid_argument("invalid key"); return it->second; } }; class Pose2Constraint : public Factor { private: std::string key1_, key2_; /** The keys of the two poses, order matters */ Pose2 measured_; Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ public: Pose2Constraint(const string& key1, const string& key2, const Pose2& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) { square_root_inverse_covariance_ = inverse_square_root(measurement_covariance); } /** implement functions needed for Testable */ void print(const std::string& name) const {} bool equals(const Factor& expected, double tol) const {return false;} /** implement functions needed to derive from Factor */ double error(const Pose2Config& c) const {return 0;} std::list keys() const { list l; return l; } std::size_t size() const { return 2;} /** linearize */ boost::shared_ptr linearize(const Pose2Config& config) const { Pose2 p1 = config.get(key1_), p2 = config.get(key2_); Vector b = (measured_ - between(p1,p2)).vector(); Matrix H1 = Dbetween1(p1,p2); Matrix H2 = Dbetween2(p1,p2); boost::shared_ptr linearized(new GaussianFactor( key1_, square_root_inverse_covariance_ * H1, key2_, square_root_inverse_covariance_ * H2, b, 1.0)); return linearized; } }; TEST( Pose2Constraint, constructor ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); Matrix measurement_covariance = Matrix_(3,3, 0.25, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.01 ); Pose2Constraint constraint("p1","p2",measured, measurement_covariance); // Choose a linearization point Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) Pose2Config config; config.insert(make_pair("p1",p1)); config.insert(make_pair("p2",p2)); // Linearize boost::shared_ptr actual = constraint.linearize(config); // expected Matrix expectedH1 = Matrix_(3,3, 0.0,-1.0,2.1, 1.0,0.0,-2.1, 0.0,0.0,-1.0 ); Matrix expectedH2 = Matrix_(3,3, 0.0,1.0,0.0, -1.0,0.0,0.0, 0.0,0.0,1.0 ); // we need the minus signs as inverse_square_root uses SVD and sign is simply arbitrary (still a ssquare root!) Matrix square_root_inverse_covariance = Matrix_(3,3, -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -10.0 ); GaussianFactor expected( "p1", square_root_inverse_covariance*expectedH1, "p2", square_root_inverse_covariance*expectedH2, Vector_(3,-0.1,-0.1,0.0), 1.0); CHECK(assert_equal(expected,*actual)); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */