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

Restored old version of between factor. Uncommented tests

release/4.3a0
Christian Forster 2014-12-02 18:30:12 -05:00
parent c06334af5f
commit c43e77cdfa
2 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/slam/BetweenFactor.h
View File

@ -90,12 +90,12 @@ namespace gtsam {
Vector evaluateError(const T& p1, const T& p2,
boost::optional<Matrix&> H1 = boost::none,boost::optional<Matrix&> H2 =
boost::none) const {
T hx = p1.between(p2, H1, H2); // 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));
//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);

3
gtsam/slam/tests/testBetweenFactor.cpp
View File

@ -45,7 +45,7 @@ TEST(BetweenFactor, Rot3) {
}
/* ************************************************************************* */
/*
// Constructor scalar
TEST(BetweenFactor, ConstructorScalar) {
SharedNoiseModel model;
@ -66,6 +66,7 @@ TEST(BetweenFactor, ConstructorDynamicSizeVector) {
Vector measured_value(5); measured_value << 1, 2, 3, 4, 5;
BetweenFactor<Vector> factor(1, 2, measured_value, model);
}
*/
/* ************************************************************************* */
int main() {