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

Prefer localCoordinates over logmap

release/4.3a0
Milo Knowles 2021-03-21 20:46:26 -04:00
parent 43c9f2ba26
commit 4b29c0370d
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -111,15 +111,14 @@ namespace gtsam {
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const override { Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const override {
if (H) { if (H) {
Matrix H_logmap; Matrix H_logmap;
T::Logmap(p, H_logmap); p.localCoordinates(T::identity(), H_logmap);
(*H) = Matrix::Zero(indices_.size(), T::dimension); (*H) = Matrix::Zero(indices_.size(), T::dimension);
for (size_t i = 0; i < indices_.size(); ++i) { for (size_t i = 0; i < indices_.size(); ++i) {
(*H).row(i) = H_logmap.row(indices_.at(i)); (*H).row(i) = H_logmap.row(indices_.at(i));
} }
} }
// FIXME: this was originally the generic retraction - may not produce same results. // Compute the tangent vector representation of T and select relevant parameters.
// Compute the tangent vector representation of T. const Vector& full_logmap = p.localCoordinates(T::identity());
const Vector& full_logmap = T::Logmap(p);
Vector partial_logmap = Vector::Zero(T::dimension); Vector partial_logmap = Vector::Zero(T::dimension);
for (size_t i = 0; i < indices_.size(); ++i) { for (size_t i = 0; i < indices_.size(); ++i) {
partial_logmap(i) = full_logmap(indices_.at(i)); partial_logmap(i) = full_logmap(indices_.at(i));