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

Merge pull request #18 from bricerebsamen/brice/fix13-rotation_expression 

Expression wrapper for rotation
release/4.3a0
Frank Dellaert 2019-05-22 16:09:02 -04:00 committed by GitHub
parent 69771f7ed5 56229eda03
commit ef174aabb8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/slam/expressions.h
View File

@ -40,6 +40,16 @@ inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
return Point3_(x, &Pose3::transformFrom, p);
}
namespace internal {
Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {
return pose.rotation(H);
}
} // namespace internal
inline Rot3_ rotation(const Pose3_& pose) {
return Rot3_(internal::rotation, pose);
}
inline Point3_ rotate(const Rot3_& x, const Point3_& p) {
return Point3_(x, &Rot3::rotate, p);
}

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

@ -45,6 +45,19 @@ TEST(SlamExpressions, project2) {
EXPECT_CORRECT_EXPRESSION_JACOBIANS(point2_expr, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(SlamExpressions, rotation) {
Pose3_ T_(0);
const Rot3_ R_ = rotation(T_);
}
/* ************************************************************************* */
TEST(SlamExpressions, unrotate) {
Rot3_ R_(0);
Point3_ p_(1);
const Point3_ q_ = unrotate(R_, p_);
}
/* ************************************************************************* */
int main() {
TestResult tr;