12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add Pose2Constraint class

release/4.3a0
Viorela Ila 2009-12-09 17:29:43 +00:00
parent 787f6e0299
commit b87aa58c1f
5 changed files with 141 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cpp/Makefile.am
View File

@ -121,9 +121,10 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
headers += SQPOptimizer.h SQPOptimizer-inl.h
headers += NonlinearConstraint.h NonlinearConstraint-inl.h
headers += Pose2Constraint.h
sources += NonlinearFactor.cpp
sources += NonlinearEquality.h
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Constraint
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
testNonlinearFactor_LDADD = libgtsam.la
testNonlinearConstraint_SOURCES = $(example) testNonlinearConstraint.cpp
@ -138,6 +139,8 @@ testNonlinearEquality_SOURCES = testNonlinearEquality.cpp
testNonlinearEquality_LDADD = libgtsam.la
testSQP_SOURCES = $(example) testSQP.cpp
testSQP_LDADD = libgtsam.la
testPose2Constraint_SOURCES = $(example) testPose2Constraint.cpp
testPose2Constraint_LDADD = libgtsam.la
# geometry
sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp

57
cpp/Pose2Constraint.h Normal file
View File

@ -0,0 +1,57 @@
/**
* @file Pose2Constraint.H
* @authors Frank Dellaert, Viorela Ila
**/
#include <map>
#include "GaussianFactor.h"
#include "VectorConfig.h"
#include "Pose2.h"
#include "Matrix.h"
namespace gtsam {
class Pose2Config : public std::map<std::string,Pose2> {
public:
Pose2 get(std::string key) const {
std::map<std::string,Pose2>::const_iterator it = find(key);
if (it==end()) throw std::invalid_argument("invalid key");
return it->second;
}
};
class Pose2Constraint : public Factor<Pose2Config> {
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 std::string& key1, const std::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<Pose2Config>& expected, double tol) const {return false;}
/** implement functions needed to derive from Factor */
double error(const Pose2Config& c) const {return 0;}
std::list<std::string> keys() const { std::list<std::string> l; return l; }
std::size_t size() const { return 2;}
/** linearize */
boost::shared_ptr<GaussianFactor> 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<GaussianFactor> linearized(new GaussianFactor(
key1_, square_root_inverse_covariance_ * H1,
key2_, square_root_inverse_covariance_ * H2,
b, 1.0));
return linearized;
}
};
} /// namespace gtsam

13
cpp/gtsam.h
View File

@ -158,3 +158,16 @@ class Simulated2DMeasurement {
void print(string s) const;
};
class Pose2Config{
Pose2 get(string key) const;
};
class Pose2Constraint {
Pose2Constraint(string key1, string key2,
const Pose2& measured, const Matrix& measurement_covariance);
void print(string name) const;
bool equals(const Pose2Constraint& expected, double tol) const;
double error(const Pose2Config& c) const;
size_t size() const;
GaussianFactor* linearize(const Pose2Config& config) const;
};

94
cpp/testNonlinearFactor.cpp
View File

@ -172,100 +172,6 @@ TEST( NonlinearFactor, size )
CHECK(factor3->size() == 2);
}
/* ************************************************************************* */
#include <map>
#include "Pose2.h"
class Pose2Config : public map<string,Pose2> {
public:
Pose2 get(string key) const {
map<string,Pose2>::const_iterator it = find(key);
if (it==end()) throw std::invalid_argument("invalid key");
return it->second;
}
};
class Pose2Constraint : public Factor<Pose2Config> {
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<Pose2Config>& expected, double tol) const {return false;}
/** implement functions needed to derive from Factor */
double error(const Pose2Config& c) const {return 0;}
std::list<std::string> keys() const { list<string> l; return l; }
std::size_t size() const { return 2;}
/** linearize */
boost::shared_ptr<GaussianFactor> 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<GaussianFactor> 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<GaussianFactor> 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);}
/* ************************************************************************* */

67
cpp/testPose2Constraint.cpp Normal file
View File

@ -0,0 +1,67 @@
/**
* @file testPose2Constraint.cpp
* @brief Unit tests for Pose2Constraint Class
* @authors Frank Dellaert, Viorela Ila
**/
/*STL/C++*/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include "Pose2Constraint.h"
using namespace std;
using namespace gtsam;
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<GaussianFactor> 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);
}
/* ************************************************************************* */