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

Forgot pre-multiply of error vector

release/4.3a0
Frank Dellaert 2010-01-16 07:00:07 +00:00
parent 91a0fb23df
commit a8afe0da7c
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cpp/Pose2Prior.h
View File

@ -46,7 +46,7 @@ public:
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
Vector error_vector(const Pose2Config& config) const { Vector error_vector(const Pose2Config& config) const {
Pose2 p = config[key_]; Pose2 p = config[key_];
return -logmap(p,measured_); return square_root_inverse_covariance_ * logmap(measured_,p);
} }
//std::list<Key> keys() const { return keys_; } //std::list<Key> keys() const { return keys_; }
@ -55,7 +55,7 @@ public:
/** linearize */ /** linearize */
boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const { boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const {
Pose2 p = config[key_]; Pose2 p = config[key_];
Vector b = logmap(p,measured_); Vector b = - error_vector(config);
Matrix H(3,3); Matrix H(3,3);
H(0,0)=1; H(0,1)=0; H(0,2)=0; H(0,0)=1; H(0,1)=0; H(0,2)=0;
H(1,0)=0; H(1,1)=1; H(1,2)=0; H(1,0)=0; H(1,1)=1; H(1,2)=0;