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

Fix Rot3::LocalCoordinates runtime error when using Cayley map

release/4.3a0
Milo Knowles 2021-03-26 17:29:47 -04:00
parent a0ff5e3886
commit 1f12f82e01
1 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -107,16 +107,23 @@ namespace gtsam {
/** Returns a vector of errors for the measured tangent parameters. */ /** Returns a vector of errors for the measured tangent parameters. */
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 {
Eigen::Matrix<double, T::dimension, T::dimension> H_local;
// If the Rot3 Cayley map is used, Rot3::LocalCoordinates will throw a runtime error
// when asked to compute the Jacobian matrix (see Rot3M.cpp).
#ifdef GTSAM_ROT3_EXPMAP
const Vector full_tangent = T::LocalCoordinates(p, H ? &H_local : nullptr);
#else
const Vector full_tangent = T::Logmap(p, H ? &H_local : nullptr);
#endif
if (H) { if (H) {
Matrix H_local;
T::LocalCoordinates(p, H_local);
(*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_local.row(indices_.at(i)); (*H).row(i) = H_local.row(indices_.at(i));
} }
} }
// Compute the tangent vector representation of T and select relevant parameters. // Select relevant parameters from the tangent vector.
const Vector& full_tangent = T::LocalCoordinates(p);
Vector partial_tangent = Vector::Zero(indices_.size()); Vector partial_tangent = Vector::Zero(indices_.size());
for (size_t i = 0; i < indices_.size(); ++i) { for (size_t i = 0; i < indices_.size(); ++i) {
partial_tangent(i) = full_tangent(indices_.at(i)); partial_tangent(i) = full_tangent(indices_.at(i));