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

Fixed conventions for Jacobians

release/4.3a0
Frank dellaert 2020-07-21 11:04:36 -04:00
parent da2d1f3dd4
commit 2605e10c27
3 changed files with 122 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
GTSAM-Concepts.md
View File

@ -72,9 +72,9 @@ A Lie group is both a manifold *and* a group. Hence, a LIE_GROUP type should imp
However, we now also need to be able to evaluate the derivatives of compose and inverse. However, we now also need to be able to evaluate the derivatives of compose and inverse.
Hence, we have the following extra valid static functions defined in the struct `gtsam::traits<T>`: Hence, we have the following extra valid static functions defined in the struct `gtsam::traits<T>`:
* `r = traits<T>::Compose(p,q,Hq,Hp)` * `r = traits<T>::Compose(p,q,Hp,Hq)`
* `q = traits<T>::Inverse(p,Hp)` * `q = traits<T>::Inverse(p,Hp)`
* `r = traits<T>::Between(p,q,Hq,H2p)` * `r = traits<T>::Between(p,q,Hp,Hq)`
where above the *H* arguments stand for optional Jacobian arguments. where above the *H* arguments stand for optional Jacobian arguments.
That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor). That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor).

158
gtsam/geometry/Pose3.cpp
View File

@ -19,8 +19,10 @@
#include <gtsam/geometry/concepts.h> #include <gtsam/geometry/concepts.h>
#include <gtsam/base/concepts.h> #include <gtsam/base/concepts.h>
#include <iostream>
#include <cmath> #include <cmath>
#include <iostream>
#include <limits>
#include <string>
using namespace std; using namespace std;
@ -36,10 +38,10 @@ Pose3::Pose3(const Pose2& pose2) :
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> H1, Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR,
OptionalJacobian<6, 3> H2) { OptionalJacobian<6, 3> Ht) {
if (H1) *H1 << I_3x3, Z_3x3; if (HR) *HR << I_3x3, Z_3x3;
if (H2) *H2 << Z_3x3, R.transpose(); if (Ht) *Ht << Z_3x3, R.transpose();
return Pose3(R, t); return Pose3(R, t);
} }
@ -72,15 +74,15 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6,6> H) { OptionalJacobian<6, 6> Hxi) {
if (H) { if (Hxi) {
H->setZero(); Hxi->setZero();
for (int i = 0; i < 6; ++i) { for (int i = 0; i < 6; ++i) {
Vector6 dxi; Vector6 dxi;
dxi.setZero(); dxi.setZero();
dxi(i) = 1.0; dxi(i) = 1.0;
Matrix6 Gi = adjointMap(dxi); Matrix6 Gi = adjointMap(dxi);
H->col(i) = Gi * y; Hxi->col(i) = Gi * y;
} }
} }
return adjointMap(xi) * y; return adjointMap(xi) * y;
@ -88,15 +90,15 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6,6> H) { OptionalJacobian<6, 6> Hxi) {
if (H) { if (Hxi) {
H->setZero(); Hxi->setZero();
for (int i = 0; i < 6; ++i) { for (int i = 0; i < 6; ++i) {
Vector6 dxi; Vector6 dxi;
dxi.setZero(); dxi.setZero();
dxi(i) = 1.0; dxi(i) = 1.0;
Matrix6 GTi = adjointMap(dxi).transpose(); Matrix6 GTi = adjointMap(dxi).transpose();
H->col(i) = GTi * y; Hxi->col(i) = GTi * y;
} }
} }
return adjointMap(xi).transpose() * y; return adjointMap(xi).transpose() * y;
@ -116,8 +118,8 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */ /** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
if (H) *H = ExpmapDerivative(xi); if (Hxi) *Hxi = ExpmapDerivative(xi);
// get angular velocity omega and translational velocity v from twist xi // get angular velocity omega and translational velocity v from twist xi
Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
@ -125,8 +127,8 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
Rot3 R = Rot3::Expmap(omega); Rot3 R = Rot3::Expmap(omega);
double theta2 = omega.dot(omega); double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) { if (theta2 > std::numeric_limits<double>::epsilon()) {
Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
Vector3 omega_cross_v = omega.cross(v); // points towards axis Vector3 omega_cross_v = omega.cross(v); // points towards axis
Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
return Pose3(R, t); return Pose3(R, t);
} else { } else {
@ -135,10 +137,10 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) {
if (H) *H = LogmapDerivative(p); if (Hpose) *Hpose = LogmapDerivative(pose);
const Vector3 w = Rot3::Logmap(p.rotation()); const Vector3 w = Rot3::Logmap(pose.rotation());
const Vector3 T = p.translation(); const Vector3 T = pose.translation();
const double t = w.norm(); const double t = w.norm();
if (t < 1e-10) { if (t < 1e-10) {
Vector6 log; Vector6 log;
@ -158,33 +160,33 @@ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) { Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) {
#ifdef GTSAM_POSE3_EXPMAP #ifdef GTSAM_POSE3_EXPMAP
return Expmap(xi, H); return Expmap(xi, Hxi);
#else #else
Matrix3 DR; Matrix3 DR;
Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0); Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : 0);
if (H) { if (Hxi) {
*H = I_6x6; *Hxi = I_6x6;
H->topLeftCorner<3,3>() = DR; Hxi->topLeftCorner<3, 3>() = DR;
} }
return Pose3(R, Point3(xi.tail<3>())); return Pose3(R, Point3(xi.tail<3>()));
#endif #endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
#ifdef GTSAM_POSE3_EXPMAP #ifdef GTSAM_POSE3_EXPMAP
return Logmap(T, H); return Logmap(pose, Hpose);
#else #else
Matrix3 DR; Matrix3 DR;
Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0); Vector3 omega = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : 0);
if (H) { if (Hpose) {
*H = I_6x6; *Hpose = I_6x6;
H->topLeftCorner<3,3>() = DR; Hpose->topLeftCorner<3, 3>() = DR;
} }
Vector6 xi; Vector6 xi;
xi << omega, T.translation(); xi << omega, pose.translation();
return xi; return xi;
#endif #endif
} }
@ -261,16 +263,16 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {
if (H) *H << Z_3x3, rotation().matrix(); if (Hself) *Hself << Z_3x3, rotation().matrix();
return t_; return t_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
const Rot3& Pose3::rotation(OptionalJacobian<3, 6> H) const { const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const {
if (H) { if (Hself) {
*H << I_3x3, Z_3x3; *Hself << I_3x3, Z_3x3;
} }
return R_; return R_;
} }
@ -299,101 +301,101 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
#endif #endif
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> H1, Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
OptionalJacobian<6, 6> H2) const { OptionalJacobian<6, 6> HwTb) const {
if (H1) *H1 = -wTb.inverse().AdjointMap() * AdjointMap(); if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap();
if (H2) *H2 = I_6x6; if (HwTb) *HwTb = I_6x6;
const Pose3& wTa = *this; const Pose3& wTa = *this;
return wTa.inverse() * wTb; return wTa.inverse() * wTb;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transformFrom(const Point3& p, OptionalJacobian<3,6> Dpose, Point3 Pose3::transformFrom(const Point3& p, OptionalJacobian<3, 6> Hself,
OptionalJacobian<3,3> Dpoint) 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 (Dpose) { if (Hself) {
Dpose->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z()); Hself->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z());
Dpose->rightCols<3>() = R; Hself->rightCols<3>() = R;
} }
if (Dpoint) { if (Hpoint) {
*Dpoint = R; *Hpoint = R;
} }
return R_ * p + t_; return R_ * p + t_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transformTo(const Point3& p, OptionalJacobian<3,6> Dpose, Point3 Pose3::transformTo(const Point3& p, OptionalJacobian<3, 6> Hself,
OptionalJacobian<3,3> Dpoint) 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*(p - t_));
if (Dpose) { if (Hself) {
const double wx = q.x(), wy = q.y(), wz = q.z(); const double wx = q.x(), wy = q.y(), wz = q.z();
(*Dpose) << (*Hself) <<
0.0, -wz, +wy,-1.0, 0.0, 0.0, 0.0, -wz, +wy,-1.0, 0.0, 0.0,
+wz, 0.0, -wx, 0.0,-1.0, 0.0, +wz, 0.0, -wx, 0.0,-1.0, 0.0,
-wy, +wx, 0.0, 0.0, 0.0,-1.0; -wy, +wx, 0.0, 0.0, 0.0,-1.0;
} }
if (Dpoint) { if (Hpoint) {
*Dpoint = Rt; *Hpoint = Rt;
} }
return q; return q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself,
OptionalJacobian<1, 3> H2) const { OptionalJacobian<1, 3> Hpoint) const {
Matrix36 D_local_pose; Matrix36 D_local_pose;
Matrix3 D_local_point; Matrix3 D_local_point;
Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0);
if (!H1 && !H2) { if (!Hself && !Hpoint) {
return local.norm(); return local.norm();
} else { } else {
Matrix13 D_r_local; Matrix13 D_r_local;
const double r = norm3(local, D_r_local); const double r = norm3(local, D_r_local);
if (H1) *H1 = D_r_local * D_local_pose; if (Hself) *Hself = D_r_local * D_local_pose;
if (H2) *H2 = D_r_local * D_local_point; if (Hpoint) *Hpoint = D_r_local * D_local_point;
return r; return r;
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1, double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself,
OptionalJacobian<1, 6> H2) const { OptionalJacobian<1, 6> Hpose) const {
Matrix13 D_local_point; Matrix13 D_local_point;
double r = range(pose.translation(), H1, H2 ? &D_local_point : 0); double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0);
if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); if (Hpose) *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
return r; return r;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> Hself,
OptionalJacobian<2, 3> H2) const { OptionalJacobian<2, 3> Hpoint) const {
Matrix36 D_local_pose; Matrix36 D_local_pose;
Matrix3 D_local_point; Matrix3 D_local_point;
Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0);
if (!H1 && !H2) { if (!Hself && !Hpoint) {
return Unit3(local); return Unit3(local);
} else { } else {
Matrix23 D_b_local; Matrix23 D_b_local;
Unit3 b = Unit3::FromPoint3(local, D_b_local); Unit3 b = Unit3::FromPoint3(local, D_b_local);
if (H1) *H1 = D_b_local * D_local_pose; if (Hself) *Hself = D_b_local * D_local_pose;
if (H2) *H2 = D_b_local * D_local_point; if (Hpoint) *Hpoint = D_b_local * D_local_point;
return b; return b;
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> H1, Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
OptionalJacobian<2, 6> H2) const { OptionalJacobian<2, 6> Hpose) const {
if (H2) { if (Hpose) {
H2->setZero(); Hpose->setZero();
return bearing(pose.translation(), H1, H2.cols<3>(3)); return bearing(pose.translation(), Hself, Hpose.cols<3>(3));
} }
return bearing(pose.translation(), H1, boost::none); return bearing(pose.translation(), Hself, boost::none);
} }
/* ************************************************************************* */ /* ************************************************************************* */

80
gtsam/geometry/Pose3.h
View File

@ -75,8 +75,8 @@ public:
/// Named constructor with derivatives /// Named constructor with derivatives
static Pose3 Create(const Rot3& R, const Point3& t, static Pose3 Create(const Rot3& R, const Point3& t,
OptionalJacobian<6, 3> H1 = boost::none, OptionalJacobian<6, 3> HR = boost::none,
OptionalJacobian<6, 3> H2 = boost::none); OptionalJacobian<6, 3> Ht = boost::none);
/** /**
* Create Pose3 by aligning two point pairs * Create Pose3 by aligning two point pairs
@ -117,10 +117,10 @@ public:
/// @{ /// @{
/// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> H = boost::none); static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none);
/// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none); static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
/** /**
* Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame
@ -157,7 +157,7 @@ public:
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/ */
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, static Vector6 adjoint(const Vector6 &xi, const Vector6 &y,
OptionalJacobian<6, 6> = boost::none); OptionalJacobian<6, 6> Hxi = boost::none);
// temporary fix for wrappers until case issue is resolved // temporary fix for wrappers until case issue is resolved
static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
@ -167,7 +167,7 @@ public:
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
*/ */
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> H = boost::none); OptionalJacobian<6, 6> Hxi = boost::none);
/// Derivative of Expmap /// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi); static Matrix6 ExpmapDerivative(const Vector6& xi);
@ -177,8 +177,8 @@ public:
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct ChartAtOrigin { struct ChartAtOrigin {
static Pose3 Retract(const Vector6& v, ChartJacobian H = boost::none); static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none);
static Vector6 Local(const Pose3& r, ChartJacobian H = boost::none); static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
}; };
using LieGroup<Pose3, 6>::inverse; // version with derivative using LieGroup<Pose3, 6>::inverse; // version with derivative
@ -202,12 +202,12 @@ 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 p point in Pose coordinates
* @param Dpose optional 3*6 Jacobian wrpt this pose * @param Hself optional 3*6 Jacobian wrpt this pose
* @param Dpoint 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> Dpose = Point3 transformFrom(const Point3& p, OptionalJacobian<3, 6> Hself =
boost::none, OptionalJacobian<3, 3> Dpoint = 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& p) const {
@ -217,22 +217,22 @@ public:
/** /**
* @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 p point in world coordinates
* @param Dpose optional 3*6 Jacobian wrpt this pose * @param Hself optional 3*6 Jacobian wrpt this pose
* @param Dpoint 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> Dpose = Point3 transformTo(const Point3& p, OptionalJacobian<3, 6> Hself =
boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/// get rotation /// get rotation
const Rot3& rotation(OptionalJacobian<3, 6> H = boost::none) const; const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const;
/// get translation /// get translation
const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const; const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const;
/// get x /// get x
double x() const { double x() const {
@ -256,32 +256,32 @@ public:
Pose3 transformPoseFrom(const Pose3& pose) const; Pose3 transformPoseFrom(const Pose3& pose) const;
/** receives a pose in world coordinates and transforms it to local coordinates */ /** receives a pose in world coordinates and transforms it to local coordinates */
Pose3 transformPoseTo(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none, Pose3 transformPoseTo(const Pose3& pose, OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const; OptionalJacobian<6, 6> Hpose = boost::none) const;
/** /**
* Calculate range to a landmark * Calculate range to a landmark
* @param point 3D location of landmark * @param point 3D location of landmark
* @return range (double) * @return range (double)
*/ */
double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const; OptionalJacobian<1, 3> Hpoint = boost::none) const;
/** /**
* Calculate range to another pose * Calculate range to another pose
* @param pose Other SO(3) pose * @param pose Other SO(3) pose
* @return range (double) * @return range (double)
*/ */
double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) const; OptionalJacobian<1, 6> Hpose = boost::none) const;
/** /**
* Calculate bearing to a landmark * Calculate bearing to a landmark
* @param point 3D location of landmark * @param point 3D location of landmark
* @return bearing (Unit3) * @return bearing (Unit3)
*/ */
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none, Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none,
OptionalJacobian<2, 3> H2 = boost::none) const; OptionalJacobian<2, 3> Hpoint = boost::none) const;
/** /**
* Calculate bearing to another pose * Calculate bearing to another pose
@ -289,8 +289,8 @@ public:
* information is ignored. * information is ignored.
* @return bearing (Unit3) * @return bearing (Unit3)
*/ */
Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> H1 = boost::none, Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none,
OptionalJacobian<2, 6> H2 = boost::none) const; OptionalJacobian<2, 6> Hpose = boost::none) const;
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
@ -321,20 +321,20 @@ public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated /// @name Deprecated
/// @{ /// @{
Point3 transform_from(const Point3& p, Point3 transform_from(const Point3& point,
OptionalJacobian<3, 6> Dpose = boost::none, OptionalJacobian<3, 6> Hself = boost::none,
OptionalJacobian<3, 3> Dpoint = boost::none) const { OptionalJacobian<3, 3> Hpoint = boost::none) const {
return transformFrom(p, Dpose, Dpoint); return transformFrom(point, Hself, Hpoint);
} }
Point3 transform_to(const Point3& p, Point3 transform_to(const Point3& point,
OptionalJacobian<3, 6> Dpose = boost::none, OptionalJacobian<3, 6> Hself = boost::none,
OptionalJacobian<3, 3> Dpoint = boost::none) const { OptionalJacobian<3, 3> Hpoint = boost::none) const {
return transformTo(p, Dpose, Dpoint); return transformTo(point, Hself, Hpoint);
} }
Pose3 transform_pose_to(const Pose3& pose, Pose3 transform_pose_to(const Pose3& pose,
OptionalJacobian<6, 6> H1 = boost::none, OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const { OptionalJacobian<6, 6> Hpose = boost::none) const {
return transformPoseTo(pose, H1, H2); return transformPoseTo(pose, Hself, Hpose);
} }
/** /**
* @deprecated: this function is neither here not there. */ * @deprecated: this function is neither here not there. */