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

Reversed arguments to Local to work with Unit3

release/4.3a0
Frank 2015-10-19 15:02:12 -07:00
parent 901fb56993
commit 935801d8e1
2 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/nonlinear/ExpressionFactor.h
View File

@ -23,6 +23,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <numeric> #include <numeric>
namespace gtsam { namespace gtsam {
/** /**
@ -74,7 +75,7 @@ public:
} }
/** /**
* Error function *without* the NoiseModel, \f$ h(x)-z \f$. * Error function *without* the NoiseModel, \f$ z-h(x) -> Local(h(x),z) \f$.
* We override this method to provide * We override this method to provide
* both the function evaluation and its derivative(s) in H. * both the function evaluation and its derivative(s) in H.
*/ */
@ -82,10 +83,12 @@ public:
boost::optional<std::vector<Matrix>&> H = boost::none) const { boost::optional<std::vector<Matrix>&> H = boost::none) const {
if (H) { if (H) {
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
return traits<T>::Local(measured_, value); // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
// because it would use the tangent space of the measurement instead of the value.
return -traits<T>::Local(value, measured_);
} else { } else {
const T value = expression_.value(x); const T value = expression_.value(x);
return traits<T>::Local(measured_, value); return -traits<T>::Local(value, measured_);
} }
} }
@ -96,7 +99,7 @@ public:
// In case noise model is constrained, we need to provide a noise model // In case noise model is constrained, we need to provide a noise model
SharedDiagonal noiseModel; SharedDiagonal noiseModel;
if (noiseModel_->isConstrained()) { if (noiseModel_ && noiseModel_->isConstrained()) {
noiseModel = boost::static_pointer_cast<noiseModel::Constrained>( noiseModel = boost::static_pointer_cast<noiseModel::Constrained>(
noiseModel_)->unit(); noiseModel_)->unit();
} }
@ -116,11 +119,13 @@ public:
T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here ! T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here !
// Evaluate error and set RHS vector b // Evaluate error and set RHS vector b
Ab(size()).col(0) = -traits<T>::Local(measured_, value); Ab(size()).col(0) = traits<T>::Local(value, measured_);
// Whiten the corresponding system, Ab already contains RHS // Whiten the corresponding system, Ab already contains RHS
if (noiseModel_) {
Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models
noiseModel_->WhitenSystem(Ab.matrix(), b); noiseModel_->WhitenSystem(Ab.matrix(), b);
}
return factor; return factor;
} }

8
gtsam/slam/PriorFactor.h
View File

@ -85,11 +85,11 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = eye(traits<T>::GetDimension(p)); if (H) (*H) = eye(traits<T>::GetDimension(x));
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of z-x -> Local(x,z)
// TODO(ASL) Add Jacobians. // TODO(ASL) Add Jacobians.
return traits<T>::Local(prior_,p); return -traits<T>::Local(x, prior_);
} }
const VALUE & prior() const { return prior_; } const VALUE & prior() const { return prior_; }