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

Reformatted with new style file, renamed some derivatives to Dcal, Dpose, Dpoint etc.

release/4.3a0
Frank Dellaert 2013-10-12 05:13:36 +00:00
parent ca9caf6a66
commit 752a9877c5
4 changed files with 648 additions and 586 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/geometry/Cal3_S2.cpp
View File

@ -58,26 +58,37 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
if (fabs(fx_ - K.fx_) > tol) return false; if (fabs(fx_ - K.fx_) > tol)
if (fabs(fy_ - K.fy_) > tol) return false; return false;
if (fabs(s_ - K.s_) > tol) return false; if (fabs(fy_ - K.fy_) > tol)
if (fabs(u0_ - K.u0_) > tol) return false; return false;
if (fabs(v0_ - K.v0_) > 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; return true;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1, Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> Dp) const {
const double x = p.x(), y = p.y(); const double x = p.x(), y = p.y();
if (H1) if (Dcal)
*H1 = Matrix_(2, 5, *Dcal = Matrix_(2, 5, x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0);
x, 0.0, y, 1.0, 0.0, if (Dp)
0.0, y, 0.0, 0.0, 1.0); *Dp = Matrix_(2, 2, fx_, s_, 0.000, fy_);
if (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_);
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); 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 } // namespace gtsam

55
gtsam/geometry/Cal3_S2.h
View File

@ -53,8 +53,9 @@ namespace gtsam {
} }
/// constructor from vector /// 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 * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
@ -86,19 +87,29 @@ namespace gtsam {
/// @{ /// @{
/// focal length x /// focal length x
inline double fx() const { return fx_;} inline double fx() const {
return fx_;
}
/// focal length y /// focal length y
inline double fy() const { return fy_;} inline double fy() const {
return fy_;
}
/// skew /// skew
inline double skew() const { return s_;} inline double skew() const {
return s_;
}
/// image center in x /// image center in x
inline double px() const { return u0_;} inline double px() const {
return u0_;
}
/// image center in y /// image center in y
inline double py() const { return v0_;} inline double py() const {
return v0_;
}
/// return the principal point /// return the principal point
Point2 principalPoint() const { Point2 principalPoint() const {
@ -121,25 +132,26 @@ namespace gtsam {
/// return inverted calibration matrix inv(K) /// return inverted calibration matrix inv(K)
Matrix matrix_inverse() const { Matrix matrix_inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_; const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
return Matrix_(3, 3, return Matrix_(3, 3, 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
1.0/fx_, -s_/fxy, (sv0-fyu0)/fxy, 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0);
0.0, 1.0/fy_, -v0_/fy_,
0.0, 0.0, 1.0);
} }
/** /**
* convert intrinsic coordinates xy to image coordinates uv * 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 = Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal =
boost::none, boost::optional<Matrix&> H2 = boost::none) const; boost::none, boost::optional<Matrix&> Dp = boost::none) const;
/// convert image coordinates uv to intrinsic coordinates xy /**
Point2 calibrate(const Point2& p) const { * convert image coordinates uv to intrinsic coordinates xy
const double u = p.x(), v = p.y(); * @param p point in image coordinates
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), * @return point in intrinsic coordinates
(1 / fy_) * (v - v0_)); */
} Point2 calibrate(const Point2& p) const;
/// @} /// @}
/// @name Manifold /// @name Manifold
@ -175,7 +187,8 @@ namespace gtsam {
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { 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)); boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(fy_);

99
gtsam/geometry/Pose3.cpp
View File

@ -37,8 +37,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) : Pose3::Pose3(const Pose2& pose2) :
R_(Rot3::rodriguez(0, 0, pose2.theta())), R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
t_(Point3(pose2.x(), pose2.y(), 0)) { 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) { if (H) {
*H = zeros(6, 6); *H = zeros(6, 6);
for (int i = 0; i < 6; ++i) { 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); Matrix Gi = adjointMap(dxi);
(*H).col(i) = Gi * y; (*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) { if (H) {
*H = zeros(6, 6); *H = zeros(6, 6);
for (int i = 0; i < 6; ++i) { 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(); Matrix GTi = adjointMap(dxi).transpose();
(*H).col(i) = GTi * y; (*H).col(i) = GTi * y;
} }
@ -94,7 +98,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix6 Pose3::dExpInv_exp(const Vector& xi) { Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
// Bernoulli numbers, from Wikipedia // 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 static const int N = 5; // order of approximation
Matrix res = I6; Matrix res = I6;
Matrix6 ad_i = I6; Matrix6 ad_i = I6;
@ -131,8 +136,7 @@ namespace gtsam {
if (theta < 1e-10) { if (theta < 1e-10) {
static const Rot3 I; static const Rot3 I;
return Pose3(I, v); return Pose3(I, v);
} } else {
else {
Point3 n(w / theta); // axis unit vector Point3 n(w / theta); // axis unit vector
Rot3 R = Rot3::rodriguez(n.vector(), theta); Rot3 R = Rot3::rodriguez(n.vector(), theta);
double vn = n.dot(v); // translation parallel to n double vn = n.dot(v); // translation parallel to n
@ -150,8 +154,7 @@ namespace gtsam {
Vector6 log; Vector6 log;
log << w, T; log << w, T;
return log; return log;
} } else {
else {
Matrix3 W = skewSymmetric(w / t); Matrix3 W = skewSymmetric(w / t);
// Formula from Agrawal06iros, equation (14) // Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in T to avoid matrix math // simplified with Mathematica, and multiplying in T to avoid matrix math
@ -192,7 +195,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// different versions of localCoordinates // 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) { if (mode == Pose3::EXPMAP) {
// Lie group logarithm map, exact inverse of exponential map // Lie group logarithm map, exact inverse of exponential map
return Logmap(between(T)); return Logmap(between(T));
@ -236,43 +240,48 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p, Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> Dpoint) const {
if (H1) { if (Dpose) {
const Matrix R = R_.matrix(); const Matrix R = R_.matrix();
Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
H1->resize(3,6); Dpose->resize(3, 6);
(*H1) << DR, R; (*Dpose) << DR, R;
} }
if (H2) *H2 = R_.matrix(); if (Dpoint)
*Dpoint = R_.matrix();
return R_ * p + t_; return R_ * p + t_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> Dpoint) const {
const Point3 result = R_.unrotate(p - t_); const Point3 result = R_.unrotate(p - t_);
if (H1) { if (Dpose) {
const Point3& q = result; const Point3& q = result;
Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
H1->resize(3,6); Dpose->resize(3, 6);
(*H1) << DR, _I3; (*Dpose) << DR, _I3;
} }
if (H2) *H2 = R_.transpose(); if (Dpoint)
*Dpoint = R_.transpose();
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
if (H1) *H1 = p2.inverse().AdjointMap(); if (H1)
if (H2) *H2 = I6; *H1 = p2.inverse().AdjointMap();
if (H2)
*H2 = I6;
return (*this) * p2; return (*this) * p2;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const { Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -AdjointMap(); if (H1)
*H1 = -AdjointMap();
Rot3 Rt = R_.inverse(); Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt * (-t_)); return Pose3(Rt, Rt * (-t_));
} }
@ -282,28 +291,32 @@ namespace gtsam {
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1, Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
Pose3 result = inverse() * p2; Pose3 result = inverse() * p2;
if (H1) *H1 = -result.inverse().AdjointMap(); if (H1)
if (H2) *H2 = I6; *H1 = -result.inverse().AdjointMap();
if (H2)
*H2 = I6;
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Pose3::range(const Point3& point, double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { 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); Point3 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), z = d.z(), double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
d2 = x * x + y * y + z * z, n = sqrt(d2); d2);
Matrix D_result_d = Matrix_(1, 3, x / n, y / n, z / n); Matrix D_result_d = Matrix_(1, 3, x / n, y / n, z / n);
if (H1) *H1 = D_result_d * (*H1); if (H1)
if (H2) *H2 = D_result_d * (*H2); *H1 = D_result_d * (*H1);
if (H2)
*H2 = D_result_d * (*H2);
return n; return n;
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Pose3::range(const Pose3& point, double Pose3::range(const Pose3& point, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
double r = range(point.translation(), H1, H2); double r = range(point.translation(), H1, H2);
if (H2) { if (H2) {
Matrix H2_ = *H2 * point.rotation().matrix(); Matrix H2_ = *H2 * point.rotation().matrix();
@ -316,7 +329,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) { boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
const size_t n = pairs.size(); 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 // calculate centroids
Vector cp = zero(3), cq = zero(3); Vector cp = zero(3), cq = zero(3);
@ -325,7 +339,8 @@ namespace gtsam {
cq += pair.second.vector(); cq += pair.second.vector();
} }
double f = 1.0 / n; double f = 1.0 / n;
cp *= f; cq *= f; cp *= f;
cq *= f;
// Add to form H matrix // Add to form H matrix
Matrix H = zeros(3, 3); Matrix H = zeros(3, 3);

61
gtsam/geometry/Pose3.h
View File

@ -15,7 +15,6 @@
*/ */
// \callgraph // \callgraph
#pragma once #pragma once
#include <gtsam/config.h> #include <gtsam/config.h>
@ -32,7 +31,8 @@
namespace gtsam { namespace gtsam {
class Pose2; // forward declare class Pose2;
// forward declare
/** /**
* A 3D pose (R,t) : (Rot3,Point3) * A 3D pose (R,t) : (Rot3,Point3)
@ -57,21 +57,27 @@ namespace gtsam {
/// @{ /// @{
/** Default constructor is origin */ /** Default constructor is origin */
Pose3() {} Pose3() {
}
/** Copy constructor */ /** 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 */ /** 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 */ /** Construct from Pose2 */
explicit Pose3(const Pose2& pose2); explicit Pose3(const Pose2& pose2);
/** Constructor from 4*4 matrix */ /** Constructor from 4*4 matrix */
Pose3(const Matrix &T) : 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), 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, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
}
/// @} /// @}
/// @name Testable /// @name Testable
@ -88,14 +94,15 @@ namespace gtsam {
/// @{ /// @{
/// identity for group operation /// identity for group operation
static Pose3 identity() { return Pose3(); } static Pose3 identity() {
return Pose3();
}
/// inverse transformation with derivatives /// inverse transformation with derivatives
Pose3 inverse(boost::optional<Matrix&> H1 = boost::none) const; Pose3 inverse(boost::optional<Matrix&> H1 = boost::none) const;
///compose this transformation onto another (first *this and then p2) ///compose this transformation onto another (first *this and then p2)
Pose3 compose(const Pose3& p2, Pose3 compose(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2 = boost::none) const; boost::optional<Matrix&> H2 = boost::none) const;
/// compose syntactic sugar /// compose syntactic sugar
@ -107,8 +114,7 @@ namespace gtsam {
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives * as well as optionally the derivatives
*/ */
Pose3 between(const Pose3& p2, Pose3 between(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2 = boost::none) const; boost::optional<Matrix&> H2 = boost::none) const;
/// @} /// @}
@ -124,16 +130,21 @@ namespace gtsam {
}; };
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes /// 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 /// 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 /// 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; 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 /// 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 /// 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; Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const;
@ -218,16 +229,28 @@ namespace gtsam {
/// @name Group Action on Point3 /// @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, 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 */ /** syntactic sugar for transform_from */
inline Point3 operator*(const Point3& p) const { return transform_from(p); } 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, 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 /// @name Standard Interface