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

Small to change to work well with LieConfig

release/4.3a0
Frank Dellaert 2010-01-10 14:55:19 +00:00
parent 794ffd3b2f
commit c957672656
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/BetweenFactor.h
View File

@ -65,7 +65,7 @@ namespace gtsam {
/** vector of errors */ /** vector of errors */
Vector error_vector(const Config& x) const { Vector error_vector(const Config& x) const {
//z-h //z-h
T p1 = x.get(key1_), p2 = x.get(key2_); const T& p1 = x.get(key1_), p2 = x.get(key2_);
T hx = between(p1,p2); T hx = between(p1,p2);
// manifold equivalent of z-h(x) -> log(h(x),z) // manifold equivalent of z-h(x) -> log(h(x),z)
return square_root_inverse_covariance_ * logmap(hx,measured_); return square_root_inverse_covariance_ * logmap(hx,measured_);
@ -79,7 +79,7 @@ namespace gtsam {
/** linearize */ /** linearize */
boost::shared_ptr<GaussianFactor> linearize(const Config& x0) const { boost::shared_ptr<GaussianFactor> linearize(const Config& x0) const {
T p1 = x0.get(key1_), p2 = x0.get(key2_); const T& p1 = x0.get(key1_), p2 = x0.get(key2_);
Matrix A1 = Dbetween1(p1, p2); Matrix A1 = Dbetween1(p1, p2);
Matrix A2 = Dbetween2(p1, p2); Matrix A2 = Dbetween2(p1, p2);
Vector b = error_vector(x0); // already has sigmas in ! Vector b = error_vector(x0); // already has sigmas in !