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

Some more name changes and documentation

release/4.3a0
Frank dellaert 2020-07-21 11:37:23 -04:00
parent d672e7eb87
commit 6192f90de5
2 changed files with 27 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/Pose3.cpp
View File

@ -286,9 +286,10 @@ Matrix4 Pose3::matrix() const {
}
/* ************************************************************************* */
Pose3 Pose3::transformPoseFrom(const Pose3& aTb) const {
Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself,
OptionalJacobian<6, 6> HaTb) const {
const Pose3& wTa = *this;
return wTa * aTb;
return wTa.compose(aTb, Hself, HaTb);
}
/* ************************************************************************* */
@ -310,28 +311,28 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
}
/* ************************************************************************* */
Point3 Pose3::transformFrom(const Point3& p, OptionalJacobian<3, 6> Hself,
Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
OptionalJacobian<3, 3> Hpoint) const {
// Only get matrix once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 R = R_.matrix();
if (Hself) {
Hself->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z());
Hself->leftCols<3>() = R * skewSymmetric(-point.x(), -point.y(), -point.z());
Hself->rightCols<3>() = R;
}
if (Hpoint) {
*Hpoint = R;
}
return R_ * p + t_;
return R_ * point + t_;
}
/* ************************************************************************* */
Point3 Pose3::transformTo(const Point3& p, OptionalJacobian<3, 6> Hself,
Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
OptionalJacobian<3, 3> Hpoint) const {
// Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_));
const Point3 q(Rt*(point - t_));
if (Hself) {
const double wx = q.x(), wy = q.y(), wz = q.z();
(*Hself) <<

30
gtsam/geometry/Pose3.h
View File

@ -201,27 +201,27 @@ public:
/**
* @brief takes point in Pose coordinates and transforms it to world coordinates
* @param p point in Pose coordinates
* @param point point in Pose coordinates
* @param Hself optional 3*6 Jacobian wrpt this pose
* @param Hpoint optional 3*3 Jacobian wrpt point
* @return point in world coordinates
*/
Point3 transformFrom(const Point3& p, OptionalJacobian<3, 6> Hself =
Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
/** syntactic sugar for transformFrom */
inline Point3 operator*(const Point3& p) const {
return transformFrom(p);
inline Point3 operator*(const Point3& point) const {
return transformFrom(point);
}
/**
* @brief takes point in world coordinates and transforms it to Pose coordinates
* @param p point in world coordinates
* @param point point in world coordinates
* @param Hself optional 3*6 Jacobian wrpt this pose
* @param Hpoint optional 3*3 Jacobian wrpt point
* @return point in Pose coordinates
*/
Point3 transformTo(const Point3& p, OptionalJacobian<3, 6> Hself =
Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
/// @}
@ -252,12 +252,20 @@ public:
/** convert to 4*4 matrix */
Matrix4 matrix() const;
/** receives a pose in local coordinates and transforms it to world coordinates */
Pose3 transformPoseFrom(const Pose3& pose) const;
/**
* Assuming self == wTa, takes a pose aTb in local coordinates
* and transforms it to world coordinates wTb = wTa * aTb.
* This is identical to compose.
*/
Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> HaTb = boost::none) const;
/** receives a pose in world coordinates and transforms it to local coordinates */
Pose3 transformPoseTo(const Pose3& pose, OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> Hpose = boost::none) const;
/**
* Assuming self == wTa, takes a pose wTb in world coordinates
* and transforms it to local coordinates aTb = inv(wTa) * wTb
*/
Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> HwTb = boost::none) const;
/**
* Calculate range to a landmark