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

attempt to fix evaluateError in BetweenFactor. Doesn't compile

release/4.3a0
Christian Forster 2014-12-01 21:18:46 -05:00
parent 1d7c895031
commit c06334af5f
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/slam/BetweenFactor.h
View File

@ -88,11 +88,16 @@ namespace gtsam {
/** vector of errors */
Vector evaluateError(const T& p1, const T& p2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::optional<Matrix&> H1 = boost::none,boost::optional<Matrix&> H2 =
boost::none) const {
T hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
DefaultChart<T> chart;
// TODO check:
//T hx = p1.between(p2, H1, H2); // h(x)
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);
}