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, 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 { boost::none) const {
T hx = p1.between(p2, H1, H2); // h(x)
DefaultChart<T> chart; DefaultChart<T> chart;
// TODO check: //T hx = chart.local(p2, p1);
//T hx = p1.between(p2, H1, H2); // h(x) //if(H1) (*H1) = -eye(chart.getDimension(p1));
T hx = chart.local(p2, p1); //if(H2) (*H2) = eye(chart.getDimension(p2));
if(H1) (*H1) = -eye(chart.getDimension(p1));
if(H2) (*H2) = eye(chart.getDimension(p2));
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return chart.local(measured_, hx); return chart.local(measured_, hx);

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

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