Fixed double map lookup
parent
769763a35d
commit
bea55b5f5b
|
@ -63,14 +63,18 @@ namespace gtsam {
|
|||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
Vector error_vector(const Config& x) const {
|
||||
Vector error_vector(const T& p1, const T& p2) const {
|
||||
//z-h
|
||||
const T& p1 = x.get(key1_), p2 = x.get(key2_);
|
||||
T hx = between(p1,p2);
|
||||
// manifold equivalent of z-h(x) -> log(h(x),z)
|
||||
return square_root_inverse_covariance_ * logmap(hx,measured_);
|
||||
}
|
||||
|
||||
/** vector of errors */
|
||||
Vector error_vector(const Config& c) const {
|
||||
return error_vector(c.get(key1_), c.get(key2_));
|
||||
}
|
||||
|
||||
/** methods to retrieve both keys */
|
||||
inline const std::string& key1() const { return key1_;}
|
||||
inline const std::string& key2() const { return key2_;}
|
||||
|
@ -86,7 +90,7 @@ namespace gtsam {
|
|||
const T& p1 = x0.get(key1_), p2 = x0.get(key2_);
|
||||
Matrix A1 = Dbetween1(p1, p2);
|
||||
Matrix A2 = Dbetween2(p1, p2);
|
||||
Vector b = error_vector(x0); // already has sigmas in !
|
||||
Vector b = error_vector(p1, p2); // already has sigmas in !
|
||||
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
|
||||
key1_, square_root_inverse_covariance_ * A1,
|
||||
key2_, square_root_inverse_covariance_ * A2, b, 1.0));
|
||||
|
|
Loading…
Reference in New Issue