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; 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 { OptionalJacobian<3, 3> Hpoint) const {
// Only get matrix once, to avoid multiple allocations, // Only get matrix once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case // as well as multiple conversions in the Quaternion case
const Matrix3 R = R_.matrix(); const Matrix3 R = R_.matrix();
if (Hself) { 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; Hself->rightCols<3>() = R;
} }
if (Hpoint) { if (Hpoint) {
*Hpoint = R; *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 { OptionalJacobian<3, 3> Hpoint) const {
// Only get transpose once, to avoid multiple allocations, // Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case // as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose(); const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_)); const Point3 q(Rt*(point - t_));
if (Hself) { if (Hself) {
const double wx = q.x(), wy = q.y(), wz = q.z(); const double wx = q.x(), wy = q.y(), wz = q.z();
(*Hself) << (*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 * @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 Hself optional 3*6 Jacobian wrpt this pose
* @param Hpoint optional 3*3 Jacobian wrpt point * @param Hpoint optional 3*3 Jacobian wrpt point
* @return point in world coordinates * @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; boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
/** syntactic sugar for transformFrom */ /** syntactic sugar for transformFrom */
inline Point3 operator*(const Point3& p) const { inline Point3 operator*(const Point3& point) const {
return transformFrom(p); return transformFrom(point);
} }
/** /**
* @brief takes point in world coordinates and transforms it to Pose coordinates * @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 Hself optional 3*6 Jacobian wrpt this pose
* @param Hpoint optional 3*3 Jacobian wrpt point * @param Hpoint optional 3*3 Jacobian wrpt point
* @return point in Pose coordinates * @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; boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
/// @} /// @}
@ -252,12 +252,20 @@ public:
/** convert to 4*4 matrix */ /** convert to 4*4 matrix */
Matrix4 matrix() const; 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, * Assuming self == wTa, takes a pose wTb in world coordinates
OptionalJacobian<6, 6> Hpose = boost::none) const; * 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 * Calculate range to a landmark