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

Chery-picked Paul's changes

release/4.3a0
dellaert 2014-11-23 23:15:10 +01:00
parent 83abb4c6ed
commit 149bfe1729
2 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/Manifold.h
View File

@ -235,7 +235,7 @@ struct DefaultChart<double> {
static double retract(double origin, const vector& d) { static double retract(double origin, const vector& d) {
return origin + d[0]; return origin + d[0];
} }
static const int getDimension(double) { static int getDimension(double) {
return 1; return 1;
} }
}; };

13
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -81,6 +81,8 @@ public:
*/ */
virtual Vector unwhitenedError(const Values& x, virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const { boost::optional<std::vector<Matrix>&> H = boost::none) const {
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
if (H) { if (H) {
// H should be pre-allocated // H should be pre-allocated
assert(H->size()==size()); assert(H->size()==size());
@ -95,18 +97,19 @@ public:
T value = expression_.value(x, map); // <<< Reverse AD happens here ! T value = expression_.value(x, map); // <<< Reverse AD happens here !
// Copy blocks into the vector of jacobians passed in // Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < size(); i++) for (DenseIndex i = 0; i < static_cast<DenseIndex>(size()); i++)
H->at(i) = Ab(i); H->at(i) = Ab(i);
return measurement_.localCoordinates(value); return chart.local(measurement_, value);
} else { } else {
const T& value = expression_.value(x); const T& value = expression_.value(x);
return measurement_.localCoordinates(value); return chart.local(measurement_, value);
} }
} }
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const { virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
// Only linearize if the factor is active // Only linearize if the factor is active
if (!active(x)) if (!active(x))
return boost::shared_ptr<JacobianFactor>(); return boost::shared_ptr<JacobianFactor>();
@ -128,7 +131,7 @@ public:
// Evaluate error to get Jacobians and RHS vector b // Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, map); // <<< Reverse AD happens here ! T value = expression_.value(x, map); // <<< Reverse AD happens here !
Ab(size()).col(0) = -measurement_.localCoordinates(value); Ab(size()).col(0) = -chart.local(measurement_, value);
// Whiten the corresponding system, Ab already contains RHS // Whiten the corresponding system, Ab already contains RHS
Vector dummy(Dim); Vector dummy(Dim);