diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index bc9cd9d6c..3587f7b71 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -90,13 +90,8 @@ namespace gtsam { Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = boost::none,boost::optional H2 = boost::none) const { - T hx = p1.between(p2, H1, H2); // h(x) DefaultChart chart; - //T hx = chart.local(p2, p1); - //if(H1) (*H1) = -eye(chart.getDimension(p1)); - //if(H2) (*H2) = eye(chart.getDimension(p2)); - // manifold equivalent of h(x)-z -> log(z,h(x)) return chart.local(measured_, hx); }