Switch to cleaner T::Logmap
parent
74b92efd89
commit
05fad78ce9
|
@ -111,14 +111,14 @@ namespace gtsam {
|
|||
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const override {
|
||||
if (H) {
|
||||
Matrix H_logmap;
|
||||
p.localCoordinates(T::identity(), H_logmap);
|
||||
T::Logmap(p, H_logmap);
|
||||
(*H) = Matrix::Zero(indices_.size(), T::dimension);
|
||||
for (size_t i = 0; i < indices_.size(); ++i) {
|
||||
(*H).row(i) = H_logmap.row(indices_.at(i));
|
||||
}
|
||||
}
|
||||
// Compute the tangent vector representation of T and select relevant parameters.
|
||||
const Vector& full_logmap = p.localCoordinates(T::identity());
|
||||
const Vector& full_logmap = T::Logmap(p);
|
||||
Vector partial_logmap = Vector::Zero(T::dimension);
|
||||
for (size_t i = 0; i < indices_.size(); ++i) {
|
||||
partial_logmap(i) = full_logmap(indices_.at(i));
|
||||
|
|
Loading…
Reference in New Issue