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

Switch to cleaner T::Logmap

release/4.3a0
Milo Knowles 2021-03-26 10:09:05 -04:00
parent 74b92efd89
commit 05fad78ce9
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -111,14 +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;
p.localCoordinates(T::identity(), H_logmap); T::Logmap(p, 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));
} }
} }
// Compute the tangent vector representation of T and select relevant parameters. // 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); 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));