Reformatted with new style file, renamed some derivatives to Dcal, Dpose, Dpoint etc.
Frank Dellaert
parent
ca9caf6a66
commit
752a9877c5
|
|
@ -58,26 +58,37 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
|
||||
if (fabs(fx_ - K.fx_) > tol) return false;
|
||||
if (fabs(fy_ - K.fy_) > tol) return false;
|
||||
if (fabs(s_ - K.s_) > tol) return false;
|
||||
if (fabs(u0_ - K.u0_) > tol) return false;
|
||||
if (fabs(v0_ - K.v0_) > tol) return false;
|
||||
if (fabs(fx_ - K.fx_) > tol)
|
||||
return false;
|
||||
if (fabs(fy_ - K.fy_) > tol)
|
||||
return false;
|
||||
if (fabs(s_ - K.s_) > tol)
|
||||
return false;
|
||||
if (fabs(u0_ - K.u0_) > tol)
|
||||
return false;
|
||||
if (fabs(v0_ - K.v0_) > tol)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
|
||||
boost::optional<Matrix&> Dp) const {
|
||||
const double x = p.x(), y = p.y();
|
||||
if (H1)
|
||||
*H1 = Matrix_(2, 5,
|
||||
x, 0.0, y, 1.0, 0.0,
|
||||
0.0, y, 0.0, 0.0, 1.0);
|
||||
if (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_);
|
||||
if (Dcal)
|
||||
*Dcal = Matrix_(2, 5, x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0);
|
||||
if (Dp)
|
||||
*Dp = Matrix_(2, 2, fx_, s_, 0.000, fy_);
|
||||
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3_S2::calibrate(const Point2& p) const {
|
||||
const double u = p.x(), v = p.y();
|
||||
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)),
|
||||
(1 / fy_) * (v - v0_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -53,8 +53,9 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/// constructor from vector
|
||||
Cal3_S2(const Vector &d): fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)){}
|
||||
|
||||
Cal3_S2(const Vector &d) :
|
||||
fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {
|
||||
}
|
||||
|
||||
/**
|
||||
* Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
|
||||
|
|
@ -86,19 +87,29 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// focal length x
|
||||
inline double fx() const { return fx_;}
|
||||
inline double fx() const {
|
||||
return fx_;
|
||||
}
|
||||
|
||||
/// focal length y
|
||||
inline double fy() const { return fy_;}
|
||||
inline double fy() const {
|
||||
return fy_;
|
||||
}
|
||||
|
||||
/// skew
|
||||
inline double skew() const { return s_;}
|
||||
inline double skew() const {
|
||||
return s_;
|
||||
}
|
||||
|
||||
/// image center in x
|
||||
inline double px() const { return u0_;}
|
||||
inline double px() const {
|
||||
return u0_;
|
||||
}
|
||||
|
||||
/// image center in y
|
||||
inline double py() const { return v0_;}
|
||||
inline double py() const {
|
||||
return v0_;
|
||||
}
|
||||
|
||||
/// return the principal point
|
||||
Point2 principalPoint() const {
|
||||
|
|
@ -121,25 +132,26 @@ namespace gtsam {
|
|||
/// return inverted calibration matrix inv(K)
|
||||
Matrix matrix_inverse() const {
|
||||
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
|
||||
return Matrix_(3, 3,
|
||||
1.0/fx_, -s_/fxy, (sv0-fyu0)/fxy,
|
||||
0.0, 1.0/fy_, -v0_/fy_,
|
||||
0.0, 0.0, 1.0);
|
||||
return Matrix_(3, 3, 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
|
||||
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* convert intrinsic coordinates xy to image coordinates uv
|
||||
* with optional derivatives
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const;
|
||||
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal =
|
||||
boost::none, boost::optional<Matrix&> Dp = boost::none) const;
|
||||
|
||||
/// convert image coordinates uv to intrinsic coordinates xy
|
||||
Point2 calibrate(const Point2& p) const {
|
||||
const double u = p.x(), v = p.y();
|
||||
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)),
|
||||
(1 / fy_) * (v - v0_));
|
||||
}
|
||||
/**
|
||||
* convert image coordinates uv to intrinsic coordinates xy
|
||||
* @param p point in image coordinates
|
||||
* @return point in intrinsic coordinates
|
||||
*/
|
||||
Point2 calibrate(const Point2& p) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
|
|
@ -175,7 +187,8 @@ namespace gtsam {
|
|||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("Cal3_S2",
|
||||
ar
|
||||
& boost::serialization::make_nvp("Cal3_S2",
|
||||
boost::serialization::base_object<Value>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(fx_);
|
||||
ar & BOOST_SERIALIZATION_NVP(fy_);
|
||||
|
|
|
|||
|
|
@ -37,8 +37,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Pose3::Pose3(const Pose2& pose2) :
|
||||
R_(Rot3::rodriguez(0, 0, pose2.theta())),
|
||||
t_(Point3(pose2.x(), pose2.y(), 0)) {
|
||||
R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
|
||||
Point3(pose2.x(), pose2.y(), 0)) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -65,11 +65,13 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) {
|
||||
Vector Pose3::adjoint(const Vector& xi, const Vector& y,
|
||||
boost::optional<Matrix&> H) {
|
||||
if (H) {
|
||||
*H = zeros(6, 6);
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
Vector dxi = zero(6); dxi(i) = 1.0;
|
||||
Vector dxi = zero(6);
|
||||
dxi(i) = 1.0;
|
||||
Matrix Gi = adjointMap(dxi);
|
||||
(*H).col(i) = Gi * y;
|
||||
}
|
||||
|
|
@ -78,11 +80,13 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) {
|
||||
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
|
||||
boost::optional<Matrix&> H) {
|
||||
if (H) {
|
||||
*H = zeros(6, 6);
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
Vector dxi = zero(6); dxi(i) = 1.0;
|
||||
Vector dxi = zero(6);
|
||||
dxi(i) = 1.0;
|
||||
Matrix GTi = adjointMap(dxi).transpose();
|
||||
(*H).col(i) = GTi * y;
|
||||
}
|
||||
|
|
@ -94,7 +98,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
|
||||
// Bernoulli numbers, from Wikipedia
|
||||
static const Vector B = Vector_(9, 1.0, -1.0/2.0, 1./6., 0.0, -1.0/30.0, 0.0, 1.0/42.0, 0.0, -1.0/30);
|
||||
static const Vector B = Vector_(9, 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
|
||||
0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
|
||||
static const int N = 5; // order of approximation
|
||||
Matrix res = I6;
|
||||
Matrix6 ad_i = I6;
|
||||
|
|
@ -131,8 +136,7 @@ namespace gtsam {
|
|||
if (theta < 1e-10) {
|
||||
static const Rot3 I;
|
||||
return Pose3(I, v);
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
Point3 n(w / theta); // axis unit vector
|
||||
Rot3 R = Rot3::rodriguez(n.vector(), theta);
|
||||
double vn = n.dot(v); // translation parallel to n
|
||||
|
|
@ -150,8 +154,7 @@ namespace gtsam {
|
|||
Vector6 log;
|
||||
log << w, T;
|
||||
return log;
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
Matrix3 W = skewSymmetric(w / t);
|
||||
// Formula from Agrawal06iros, equation (14)
|
||||
// simplified with Mathematica, and multiplying in T to avoid matrix math
|
||||
|
|
@ -192,7 +195,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// different versions of localCoordinates
|
||||
Vector6 Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const {
|
||||
Vector6 Pose3::localCoordinates(const Pose3& T,
|
||||
Pose3::CoordinatesMode mode) const {
|
||||
if (mode == Pose3::EXPMAP) {
|
||||
// Lie group logarithm map, exact inverse of exponential map
|
||||
return Logmap(between(T));
|
||||
|
|
@ -236,43 +240,48 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_from(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) {
|
||||
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
|
||||
boost::optional<Matrix&> Dpoint) const {
|
||||
if (Dpose) {
|
||||
const Matrix R = R_.matrix();
|
||||
Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
H1->resize(3,6);
|
||||
(*H1) << DR, R;
|
||||
Dpose->resize(3, 6);
|
||||
(*Dpose) << DR, R;
|
||||
}
|
||||
if (H2) *H2 = R_.matrix();
|
||||
if (Dpoint)
|
||||
*Dpoint = R_.matrix();
|
||||
return R_ * p + t_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_to(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
|
||||
boost::optional<Matrix&> Dpoint) const {
|
||||
const Point3 result = R_.unrotate(p - t_);
|
||||
if (H1) {
|
||||
if (Dpose) {
|
||||
const Point3& q = result;
|
||||
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
|
||||
H1->resize(3,6);
|
||||
(*H1) << DR, _I3;
|
||||
Dpose->resize(3, 6);
|
||||
(*Dpose) << DR, _I3;
|
||||
}
|
||||
if (H2) *H2 = R_.transpose();
|
||||
if (Dpoint)
|
||||
*Dpoint = R_.transpose();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::compose(const Pose3& p2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = p2.inverse().AdjointMap();
|
||||
if (H2) *H2 = I6;
|
||||
Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
if (H1)
|
||||
*H1 = p2.inverse().AdjointMap();
|
||||
if (H2)
|
||||
*H2 = I6;
|
||||
return (*this) * p2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
|
||||
if (H1) *H1 = -AdjointMap();
|
||||
if (H1)
|
||||
*H1 = -AdjointMap();
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt, Rt * (-t_));
|
||||
}
|
||||
|
|
@ -282,28 +291,32 @@ namespace gtsam {
|
|||
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
Pose3 result = inverse() * p2;
|
||||
if (H1) *H1 = -result.inverse().AdjointMap();
|
||||
if (H2) *H2 = I6;
|
||||
if (H1)
|
||||
*H1 = -result.inverse().AdjointMap();
|
||||
if (H2)
|
||||
*H2 = I6;
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose3::range(const Point3& point,
|
||||
boost::optional<Matrix&> H1,
|
||||
double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
if (!H1 && !H2) return transform_to(point).norm();
|
||||
if (!H1 && !H2)
|
||||
return transform_to(point).norm();
|
||||
Point3 d = transform_to(point, H1, H2);
|
||||
double x = d.x(), y = d.y(), z = d.z(),
|
||||
d2 = x * x + y * y + z * z, n = sqrt(d2);
|
||||
double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
|
||||
d2);
|
||||
Matrix D_result_d = Matrix_(1, 3, x / n, y / n, z / n);
|
||||
if (H1) *H1 = D_result_d * (*H1);
|
||||
if (H2) *H2 = D_result_d * (*H2);
|
||||
if (H1)
|
||||
*H1 = D_result_d * (*H1);
|
||||
if (H2)
|
||||
*H2 = D_result_d * (*H2);
|
||||
return n;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose3::range(const Pose3& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
double Pose3::range(const Pose3& point, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
double r = range(point.translation(), H1, H2);
|
||||
if (H2) {
|
||||
Matrix H2_ = *H2 * point.rotation().matrix();
|
||||
|
|
@ -316,7 +329,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
|
||||
const size_t n = pairs.size();
|
||||
if (n<3) return boost::none; // we need at least three pairs
|
||||
if (n < 3)
|
||||
return boost::none; // we need at least three pairs
|
||||
|
||||
// calculate centroids
|
||||
Vector cp = zero(3), cq = zero(3);
|
||||
|
|
@ -325,7 +339,8 @@ namespace gtsam {
|
|||
cq += pair.second.vector();
|
||||
}
|
||||
double f = 1.0 / n;
|
||||
cp *= f; cq *= f;
|
||||
cp *= f;
|
||||
cq *= f;
|
||||
|
||||
// Add to form H matrix
|
||||
Matrix H = zeros(3, 3);
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@
|
|||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/config.h>
|
||||
|
|
@ -32,7 +31,8 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class Pose2; // forward declare
|
||||
class Pose2;
|
||||
// forward declare
|
||||
|
||||
/**
|
||||
* A 3D pose (R,t) : (Rot3,Point3)
|
||||
|
|
@ -57,21 +57,27 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/** Default constructor is origin */
|
||||
Pose3() {}
|
||||
Pose3() {
|
||||
}
|
||||
|
||||
/** Copy constructor */
|
||||
Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {}
|
||||
Pose3(const Pose3& pose) :
|
||||
R_(pose.R_), t_(pose.t_) {
|
||||
}
|
||||
|
||||
/** Construct from R,t */
|
||||
Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {}
|
||||
Pose3(const Rot3& R, const Point3& t) :
|
||||
R_(R), t_(t) {
|
||||
}
|
||||
|
||||
/** Construct from Pose2 */
|
||||
explicit Pose3(const Pose2& pose2);
|
||||
|
||||
/** Constructor from 4*4 matrix */
|
||||
Pose3(const Matrix &T) :
|
||||
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
|
||||
T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {}
|
||||
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1),
|
||||
T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
|
|
@ -88,14 +94,15 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
static Pose3 identity() { return Pose3(); }
|
||||
static Pose3 identity() {
|
||||
return Pose3();
|
||||
}
|
||||
|
||||
/// inverse transformation with derivatives
|
||||
Pose3 inverse(boost::optional<Matrix&> H1 = boost::none) const;
|
||||
|
||||
///compose this transformation onto another (first *this and then p2)
|
||||
Pose3 compose(const Pose3& p2,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
Pose3 compose(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/// compose syntactic sugar
|
||||
|
|
@ -107,8 +114,7 @@ namespace gtsam {
|
|||
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||
* as well as optionally the derivatives
|
||||
*/
|
||||
Pose3 between(const Pose3& p2,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
Pose3 between(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/// @}
|
||||
|
|
@ -124,16 +130,21 @@ namespace gtsam {
|
|||
};
|
||||
|
||||
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
|
||||
static size_t Dim() { return dimension; }
|
||||
static size_t Dim() {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/// Dimensionality of the tangent space = 6 DOF
|
||||
size_t dim() const { return dimension; }
|
||||
size_t dim() const {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map
|
||||
Pose3 retractFirstOrder(const Vector& d) const;
|
||||
|
||||
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose
|
||||
Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
|
||||
Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode =
|
||||
POSE3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
|
||||
Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
|
@ -218,16 +229,28 @@ namespace gtsam {
|
|||
/// @name Group Action on Point3
|
||||
/// @{
|
||||
|
||||
/** receives the 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 Dpose optional 3*6 Jacobian wrpt this pose
|
||||
* @param Dpoint optional 3*3 Jacobian wrpt point
|
||||
* @return point in world coordinates
|
||||
*/
|
||||
Point3 transform_from(const Point3& p,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;
|
||||
|
||||
/** syntactic sugar for transform_from */
|
||||
inline Point3 operator*(const Point3& p) const { return transform_from(p); }
|
||||
|
||||
/** receives the 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 Dpose optional 3*6 Jacobian wrpt this pose
|
||||
* @param Dpoint optional 3*3 Jacobian wrpt point
|
||||
* @return point in Pose coordinates
|
||||
*/
|
||||
Point3 transform_to(const Point3& p,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
|
|||
Loading…
Reference in New Issue