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) {
return origin + d[0];
}
static const int getDimension(double) {
static int getDimension(double) {
return 1;
}
};

13
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -81,6 +81,8 @@ public:
*/
virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
if (H) {
// H should be pre-allocated
assert(H->size()==size());
@ -95,18 +97,19 @@ public:
T value = expression_.value(x, map); // <<< Reverse AD happens here !
// 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);
return measurement_.localCoordinates(value);
return chart.local(measurement_, value);
} else {
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 {
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
// Only linearize if the factor is active
if (!active(x))
return boost::shared_ptr<JacobianFactor>();
@ -128,7 +131,7 @@ public:
// Evaluate error to get Jacobians and RHS vector b
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
Vector dummy(Dim);