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

add logmap expression

release/4.3a0
yetongumich 2021-10-20 14:27:42 -04:00
parent 9e032e4511
commit e0425f3548
2 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/slam/expressions.h
View File

@ -138,4 +138,21 @@ Point2_ uncalibrate(const Expression<CALIBRATION>& K, const Point2_& xy_hat) {
return Point2_(K, &CALIBRATION::uncalibrate, xy_hat);
}
/// logmap
// TODO(dellaert): Should work but fails because of a type deduction conflict.
// template <typename T>
// gtsam::Expression<typename gtsam::traits<T>::TangentVector> logmap(
// const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
// return gtsam::Expression<typename gtsam::traits<T>::TangentVector>(
// x1, &T::logmap, x2);
// }
template <typename T>
gtsam::Expression<typename gtsam::traits<T>::TangentVector> logmap(
const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
return Expression<typename gtsam::traits<T>::TangentVector>(
gtsam::traits<T>::Logmap, between(x1, x2));
}
} // \namespace gtsam

6
gtsam/slam/tests/testSlamExpressions.cpp
View File

@ -58,6 +58,12 @@ TEST(SlamExpressions, unrotate) {
const Point3_ q_ = unrotate(R_, p_);
}
TEST(SlamExpressions, logmap) {
Pose3_ T1_(0);
Pose3_ T2_(1);
const Vector6_ l = logmap(T1_, T2_);
}
/* ************************************************************************* */
int main() {
TestResult tr;