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

Math fix in linearize() and error_vector()

release/4.3a0
Richard Roberts 2009-12-18 00:10:20 +00:00
parent 7161878285
commit 3c0ae0ec1c
2 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cpp/Pose2Factor.h
View File

@ -20,14 +20,18 @@ private:
std::string key1_, key2_; /** The keys of the two poses, order matters */
Pose2 measured_;
Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
std::list<std::string> keys_;
public:
typedef boost::shared_ptr<Pose2Factor> shared_ptr; // shorthand for a smart pointer to a factor
Pose2Factor(const std::string& key1, const std::string& key2,
const Pose2& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) {
const Pose2& measured, const Matrix& measurement_covariance) :
key1_(key1),key2_(key2),measured_(measured) {
square_root_inverse_covariance_ = inverse_square_root(measurement_covariance);
keys_.push_back(key1);
keys_.push_back(key2);
}
/** implement functions needed for Testable */
@ -45,16 +49,16 @@ public:
Vector error_vector(const Pose2Config& config) const {
//z-h
Pose2 p1 = config.get(key1_), p2 = config.get(key2_);
return (measured_ - between(p1,p2)).vector();
return -between(p1,p2).log(measured_);
}
std::list<std::string> keys() const { std::list<std::string> l; return l; }
std::size_t size() const { return 2;}
std::list<std::string> keys() const { return keys_; }
std::size_t size() const { return keys_.size(); }
/** 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();
Vector b = -between(p1,p2).log(measured_);
Matrix H1 = Dbetween1(p1,p2);
Matrix H2 = Dbetween2(p1,p2);
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(

5
cpp/testPose2Factor.cpp
View File

@ -28,7 +28,7 @@ TEST( Pose2Factor, constructor )
// 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)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at 4.1,2)
Pose2Config config;
config.insert("p1",p1);
config.insert("p2",p2);
@ -59,11 +59,12 @@ TEST( Pose2Factor, constructor )
GaussianFactor expected(
"p1", square_root_inverse_covariance*expectedH1,
"p2", square_root_inverse_covariance*expectedH2,
Vector_(3,-0.1,-0.1,0.0), 1.0);
Vector_(3,0.1,0.1,0.0), 1.0);
CHECK(assert_equal(expected,*actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;