Now use new FixedRef type in all methods that I used fixed-sized matrices in to develop Expressions. All copy/paste bloat is now gone, and boost::none arguments are back.
dellaert
parent
49932cf0f3
commit
1e4905ef04
|
|
@ -64,21 +64,9 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
|
|||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Bundler::uncalibrate(const Point2& p) const {
|
||||
// r = x^2 + y^2;
|
||||
// g = (1 + k(1)*r + k(2)*r^2);
|
||||
// pi(:,i) = g * pn(:,i)
|
||||
const double x = p.x(), y = p.y();
|
||||
const double r = x * x + y * y;
|
||||
const double g = 1. + (k1_ + k2_ * r) * r;
|
||||
const double u = g * x, v = g * y;
|
||||
return Point2(u0_ + f_ * u, v0_ + f_ * v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
|
||||
boost::optional<Matrix23&> Dcal, boost::optional<Matrix2&> Dp) const {
|
||||
FixedRef<2, 3> Dcal, FixedRef<2, 2> Dp) const {
|
||||
// r = x^2 + y^2;
|
||||
// g = (1 + k(1)*r + k(2)*r^2);
|
||||
// pi(:,i) = g * pn(:,i)
|
||||
|
|
@ -103,35 +91,6 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
|
|||
return Point2(u0_ + f_ * u, v0_ + f_ * v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
|
||||
boost::optional<Matrix&> Dcal, boost::optional<Matrix&> Dp) const {
|
||||
// r = x^2 + y^2;
|
||||
// g = (1 + k(1)*r + k(2)*r^2);
|
||||
// pi(:,i) = g * pn(:,i)
|
||||
const double x = p.x(), y = p.y();
|
||||
const double r = x * x + y * y;
|
||||
const double g = 1. + (k1_ + k2_ * r) * r;
|
||||
const double u = g * x, v = g * y;
|
||||
|
||||
// Derivatives make use of intermediate variables above
|
||||
if (Dcal) {
|
||||
double rx = r * x, ry = r * y;
|
||||
Dcal->resize(2, 3);
|
||||
*Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
|
||||
}
|
||||
|
||||
if (Dp) {
|
||||
const double a = 2. * (k1_ + 2. * k2_ * r);
|
||||
const double axx = a * x * x, axy = a * x * y, ayy = a * y * y;
|
||||
Dp->resize(2,2);
|
||||
*Dp << g + axx, axy, axy, g + ayy;
|
||||
*Dp *= f_;
|
||||
}
|
||||
|
||||
return Point2(u0_ + f_ * u, v0_ + f_ * v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
|
||||
// Copied from Cal3DS2 :-(
|
||||
|
|
|
|||
|
|
@ -106,31 +106,14 @@ public:
|
|||
|
||||
|
||||
/**
|
||||
* convert intrinsic coordinates xy to image coordinates uv
|
||||
* @param p point in intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p) const;
|
||||
|
||||
/**
|
||||
* Version of uncalibrate with fixed derivatives
|
||||
* Version of uncalibrate with derivatives
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p, boost::optional<Matrix23&> Dcal,
|
||||
boost::optional<Matrix2&> Dp) const;
|
||||
|
||||
/**
|
||||
* Version of uncalibrate with dynamic derivatives
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
|
||||
boost::optional<Matrix&> Dp) const;
|
||||
Point2 uncalibrate(const Point2& p, FixedRef<2, 3> Dcal = boost::none,
|
||||
FixedRef<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// Conver a pixel coordinate to ideal coordinate
|
||||
Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
|
||||
|
|
|
|||
|
|
@ -91,8 +91,7 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
|
|||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix29&> H1,
|
||||
boost::optional<Matrix2&> H2) const {
|
||||
FixedRef<2,9> H1, FixedRef<2,2> H2) const {
|
||||
|
||||
// rr = x^2 + y^2;
|
||||
// g = (1 + k(1)*rr + k(2)*rr^2);
|
||||
|
|
@ -126,26 +125,6 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
|
|||
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
|
||||
if (H1 || H2) {
|
||||
Matrix29 D1;
|
||||
Matrix2 D2;
|
||||
Point2 pu = uncalibrate(p, D1, D2);
|
||||
if (H1)
|
||||
*H1 = D1;
|
||||
if (H2)
|
||||
*H2 = D2;
|
||||
return pu;
|
||||
|
||||
} else {
|
||||
return uncalibrate(p);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
|
||||
// Use the following fixed point iteration to invert the radial distortion.
|
||||
|
|
|
|||
|
|
@ -113,14 +113,9 @@ public:
|
|||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in (distorted) image coordinates
|
||||
*/
|
||||
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix29&> Dcal = boost::none,
|
||||
boost::optional<Matrix2&> Dp = boost::none) const ;
|
||||
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> Dcal,
|
||||
boost::optional<Matrix&> Dp) const ;
|
||||
FixedRef<2,9> Dcal = boost::none,
|
||||
FixedRef<2,2> Dp = boost::none) const ;
|
||||
|
||||
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
|
||||
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
|
||||
|
|
|
|||
|
|
@ -23,77 +23,9 @@
|
|||
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Eigen::Ref like class that cane take either a fixed size or dynamic
|
||||
* Eigen matrix. In the latter case, the dynamic matrix will be resized.
|
||||
* Finally, the default constructor acts like boost::none.
|
||||
*/
|
||||
template<int Rows, int Cols>
|
||||
class FixedRef {
|
||||
|
||||
public:
|
||||
|
||||
/// Fixed size type
|
||||
typedef Eigen::Matrix<double, Rows, Cols> Fixed;
|
||||
|
||||
private:
|
||||
|
||||
bool empty_;
|
||||
Eigen::Map<Fixed> map_;
|
||||
|
||||
// Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
|
||||
// to make map_ usurp the memory of the fixed size matrix
|
||||
void usurp(double* data) {
|
||||
new (&map_) Eigen::Map<Fixed>(data);
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
/// Defdault constructo acts like boost::none
|
||||
FixedRef() :
|
||||
empty_(true), map_(NULL) {
|
||||
}
|
||||
|
||||
/// Defdault constructo acts like boost::none
|
||||
FixedRef(boost::none_t none) :
|
||||
empty_(true), map_(NULL) {
|
||||
}
|
||||
|
||||
/// Constructor that will usurp data of a fixed size matrix
|
||||
FixedRef(Fixed& fixed) :
|
||||
empty_(false), map_(NULL) {
|
||||
usurp(fixed.data());
|
||||
}
|
||||
|
||||
/// Constructor that will resize a dynamic matrix
|
||||
FixedRef(Matrix& dynamic) :
|
||||
empty_(false), map_(NULL) {
|
||||
}
|
||||
|
||||
/// Constructor compatible with old-style
|
||||
FixedRef(const boost::optional<Matrix&> optional) :
|
||||
empty_(!optional), map_(NULL) {
|
||||
if (optional) {
|
||||
optional->resize(Rows, Cols);
|
||||
usurp(optional->data());
|
||||
}
|
||||
}
|
||||
|
||||
/// Return true is allocated, false if default constructor was used
|
||||
operator bool() const {
|
||||
return !empty_;
|
||||
}
|
||||
|
||||
/// De-reference, like boost optional
|
||||
Eigen::Map<Fixed>& operator* () {
|
||||
return map_;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief The most common 5DOF 3D->2D calibration
|
||||
* @addtogroup geometry
|
||||
|
|
@ -216,8 +148,8 @@ public:
|
|||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p, FixedRef<2,5> Dcal = FixedRef<2,5>(),
|
||||
FixedRef<2,2> Dp = FixedRef<2,2>()) const;
|
||||
Point2 uncalibrate(const Point2& p, FixedRef<2,5> Dcal = boost::none,
|
||||
FixedRef<2,2> Dp = boost::none) const;
|
||||
|
||||
/**
|
||||
* convert image coordinates uv to intrinsic coordinates xy
|
||||
|
|
|
|||
|
|
@ -274,8 +274,8 @@ public:
|
|||
* @param P A point in camera coordinates
|
||||
* @param Dpoint is the 2*3 Jacobian w.r.t. P
|
||||
*/
|
||||
inline static Point2 project_to_camera(const Point3& P,
|
||||
boost::optional<Matrix23&> Dpoint = boost::none) {
|
||||
static Point2 project_to_camera(const Point3& P, //
|
||||
FixedRef<2, 3> Dpoint = boost::none) {
|
||||
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
|
||||
if (P.z() <= 0)
|
||||
throw CheiralityException();
|
||||
|
|
@ -294,20 +294,6 @@ public:
|
|||
return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
|
||||
}
|
||||
|
||||
/** project a point from world coordinate to the image
|
||||
* @param pw is a point in world coordinates
|
||||
*/
|
||||
inline Point2 project(const Point3& pw) const {
|
||||
|
||||
// Transform to camera coordinates and check cheirality
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
|
||||
// Project to normalized image coordinates
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
|
||||
return K_.uncalibrate(pn);
|
||||
}
|
||||
|
||||
typedef Eigen::Matrix<double,2,DimK> Matrix2K;
|
||||
|
||||
/** project a point from world coordinate to the image
|
||||
|
|
@ -317,10 +303,10 @@ public:
|
|||
* @param Dcal is the Jacobian w.r.t. calibration
|
||||
*/
|
||||
inline Point2 project(
|
||||
const Point3& pw, //
|
||||
boost::optional<Matrix26&> Dpose,
|
||||
boost::optional<Matrix23&> Dpoint,
|
||||
boost::optional<Matrix2K&> Dcal) const {
|
||||
const Point3& pw,
|
||||
FixedRef<2,6> Dpose = boost::none,
|
||||
FixedRef<2,3> Dpoint = boost::none,
|
||||
FixedRef<2,DimK> Dcal = boost::none) const {
|
||||
|
||||
// Transform to camera coordinates and check cheirality
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
|
|
@ -341,45 +327,6 @@ public:
|
|||
if (Dpoint)
|
||||
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
|
||||
return pi;
|
||||
} else
|
||||
return K_.uncalibrate(pn, Dcal, boost::none);
|
||||
}
|
||||
|
||||
/** project a point from world coordinate to the image
|
||||
* @param pw is a point in world coordinates
|
||||
* @param Dpose is the Jacobian w.r.t. pose3
|
||||
* @param Dpoint is the Jacobian w.r.t. point3
|
||||
* @param Dcal is the Jacobian w.r.t. calibration
|
||||
*/
|
||||
inline Point2 project(
|
||||
const Point3& pw, //
|
||||
boost::optional<Matrix&> Dpose,
|
||||
boost::optional<Matrix&> Dpoint,
|
||||
boost::optional<Matrix&> Dcal) const {
|
||||
|
||||
// Transform to camera coordinates and check cheirality
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
|
||||
// Project to normalized image coordinates
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
|
||||
if (Dpose || Dpoint) {
|
||||
const double z = pc.z(), d = 1.0 / z;
|
||||
|
||||
// uncalibration
|
||||
Matrix2 Dpi_pn;
|
||||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
||||
|
||||
// chain the Jacobian matrices
|
||||
if (Dpose) {
|
||||
Dpose->resize(2, 6);
|
||||
calculateDpose(pn, d, Dpi_pn, *Dpose);
|
||||
}
|
||||
if (Dpoint) {
|
||||
Dpoint->resize(2, 3);
|
||||
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
|
||||
}
|
||||
return pi;
|
||||
} else
|
||||
return K_.uncalibrate(pn, Dcal);
|
||||
}
|
||||
|
|
@ -391,10 +338,10 @@ public:
|
|||
* @param Dcal is the Jacobian w.r.t. calibration
|
||||
*/
|
||||
inline Point2 projectPointAtInfinity(
|
||||
const Point3& pw, //
|
||||
boost::optional<Matrix&> Dpose = boost::none,
|
||||
boost::optional<Matrix&> Dpoint = boost::none,
|
||||
boost::optional<Matrix&> Dcal = boost::none) const {
|
||||
const Point3& pw,
|
||||
FixedRef<2,6> Dpose = boost::none,
|
||||
FixedRef<2,2> Dpoint = boost::none,
|
||||
FixedRef<2,DimK> Dcal = boost::none) const {
|
||||
|
||||
if (!Dpose && !Dpoint && !Dcal) {
|
||||
const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
|
||||
|
|
@ -403,11 +350,11 @@ public:
|
|||
}
|
||||
|
||||
// world to camera coordinate
|
||||
Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */;
|
||||
Matrix3 Dpc_rot, Dpc_point;
|
||||
const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
|
||||
|
||||
Matrix Dpc_pose = Matrix::Zero(3, 6);
|
||||
Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
|
||||
Matrix36 Dpc_pose; Dpc_pose.setZero();
|
||||
Dpc_pose.leftCols<3>() = Dpc_rot;
|
||||
|
||||
// camera to normalized image coordinate
|
||||
Matrix23 Dpn_pc; // 2*3
|
||||
|
|
@ -418,34 +365,22 @@ public:
|
|||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
||||
|
||||
// chain the Jacobian matrices
|
||||
const Matrix Dpi_pc = Dpi_pn * Dpn_pc;
|
||||
const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
|
||||
if (Dpose)
|
||||
*Dpose = Dpi_pc * Dpc_pose;
|
||||
if (Dpoint)
|
||||
*Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 2, 2); // only 2dof are important for the point (direction-only)
|
||||
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only)
|
||||
return pi;
|
||||
}
|
||||
|
||||
typedef Eigen::Matrix<double,2,6+DimK> Matrix2K6;
|
||||
|
||||
/** project a point from world coordinate to the image
|
||||
* @param pw is a point in the world coordinate
|
||||
*/
|
||||
Point2 project2(const Point3& pw) const {
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
return K_.uncalibrate(pn);
|
||||
}
|
||||
|
||||
/** project a point from world coordinate to the image, fixed Jacobians
|
||||
* @param pw is a point in the world coordinate
|
||||
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
|
||||
* @param Dpoint is the Jacobian w.r.t. point3
|
||||
*/
|
||||
Point2 project2(
|
||||
const Point3& pw, //
|
||||
boost::optional<Matrix2K6&> Dcamera,
|
||||
boost::optional<Matrix23&> Dpoint) const {
|
||||
Point2 project2(const Point3& pw, //
|
||||
FixedRef<2, 6 + DimK> Dcamera = boost::none,
|
||||
FixedRef<2, 3> Dpoint = boost::none) const {
|
||||
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
|
|
@ -461,8 +396,8 @@ public:
|
|||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
||||
|
||||
if (Dcamera) { // TODO why does leftCols<6>() not compile ??
|
||||
calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols(6));
|
||||
Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
|
||||
calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
|
||||
(*Dcamera).rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
|
||||
}
|
||||
if (Dpoint) {
|
||||
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
|
||||
|
|
@ -471,40 +406,6 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
/** project a point from world coordinate to the image
|
||||
* @param pw is a point in the world coordinate
|
||||
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
|
||||
* @param Dpoint is the Jacobian w.r.t. point3
|
||||
*/
|
||||
Point2 project2(const Point3& pw, //
|
||||
boost::optional<Matrix&> Dcamera, boost::optional<Matrix&> Dpoint) const {
|
||||
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
|
||||
if (!Dcamera && !Dpoint) {
|
||||
return K_.uncalibrate(pn);
|
||||
} else {
|
||||
const double z = pc.z(), d = 1.0 / z;
|
||||
|
||||
// uncalibration
|
||||
Matrix2K Dcal;
|
||||
Matrix2 Dpi_pn;
|
||||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
||||
|
||||
if (Dcamera) {
|
||||
Dcamera->resize(2, this->dim());
|
||||
calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>());
|
||||
Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
|
||||
}
|
||||
if (Dpoint) {
|
||||
Dpoint->resize(2, 3);
|
||||
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
|
||||
}
|
||||
return pi;
|
||||
}
|
||||
}
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
|
||||
inline Point3 backproject(const Point2& p, double depth) const {
|
||||
const Point2 pn = K_.calibrate(p);
|
||||
|
|
|
|||
|
|
@ -94,26 +94,8 @@ double Point3::dot(const Point3 &q) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point3::norm() const {
|
||||
return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point3::norm(boost::optional<Matrix&> H) const {
|
||||
double r = norm();
|
||||
if (H) {
|
||||
H->resize(1,3);
|
||||
if (fabs(r) > 1e-10)
|
||||
*H << x_ / r, y_ / r, z_ / r;
|
||||
else
|
||||
*H << 1, 1, 1; // really infinity, why 1 ?
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Point3::norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const {
|
||||
double r = norm();
|
||||
double Point3::norm(FixedRef<1,3> H) const {
|
||||
double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_);
|
||||
if (H) {
|
||||
if (fabs(r) > 1e-10)
|
||||
*H << x_ / r, y_ / r, z_ / r;
|
||||
|
|
|
|||
|
|
@ -180,14 +180,8 @@ namespace gtsam {
|
|||
return (p2 - *this).norm();
|
||||
}
|
||||
|
||||
/** Distance of the point from the origin */
|
||||
double norm() const;
|
||||
|
||||
/** Distance of the point from the origin, with Jacobian */
|
||||
double norm(boost::optional<Matrix&> H) const;
|
||||
|
||||
/** Distance of the point from the origin, with Jacobian */
|
||||
double norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const;
|
||||
double norm(FixedRef<1,3> H = boost::none) const;
|
||||
|
||||
/** normalize, with optional Jacobian */
|
||||
Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
|
||||
|
|
|
|||
|
|
@ -123,17 +123,10 @@ Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
|
|||
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SE(2) section
|
||||
Point2 Pose2::transform_to(const Point2& point) const {
|
||||
Point2 d = point - t_;
|
||||
return r_.unrotate(d);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SE(2) section
|
||||
Point2 Pose2::transform_to(const Point2& point,
|
||||
boost::optional<Matrix23&> H1, boost::optional<Matrix2&> H2) const {
|
||||
FixedRef<2, 3> H1, FixedRef<2, 2> H2) const {
|
||||
Point2 d = point - t_;
|
||||
Point2 q = r_.unrotate(d);
|
||||
if (!H1 && !H2) return q;
|
||||
|
|
@ -144,20 +137,6 @@ Point2 Pose2::transform_to(const Point2& point,
|
|||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SE(2) section
|
||||
Point2 Pose2::transform_to(const Point2& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
Point2 d = point - t_;
|
||||
Point2 q = r_.unrotate(d);
|
||||
if (!H1 && !H2) return q;
|
||||
if (H1) *H1 = (Matrix(2, 3) <<
|
||||
-1.0, 0.0, q.y(),
|
||||
0.0, -1.0, -q.x()).finished();
|
||||
if (H2) *H2 = r_.transpose();
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SE(2) section
|
||||
Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
|
||||
|
|
@ -171,7 +150,7 @@ Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
|
|||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SE(2) section
|
||||
Point2 Pose2::transform_from(const Point2& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
FixedRef<2, 3> H1, FixedRef<2, 2> H2) const {
|
||||
const Point2 q = r_ * p;
|
||||
if (H1 || H2) {
|
||||
const Matrix R = r_.matrix();
|
||||
|
|
|
|||
|
|
@ -190,23 +190,15 @@ public:
|
|||
/// @name Group Action on Point2
|
||||
/// @{
|
||||
|
||||
/** Return point coordinates in pose coordinate frame */
|
||||
Point2 transform_to(const Point2& point) const;
|
||||
|
||||
/** Return point coordinates in pose coordinate frame */
|
||||
Point2 transform_to(const Point2& point,
|
||||
boost::optional<Matrix23&> H1,
|
||||
boost::optional<Matrix2&> H2) const;
|
||||
|
||||
/** Return point coordinates in pose coordinate frame */
|
||||
Point2 transform_to(const Point2& point,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const;
|
||||
FixedRef<2, 3> H1 = boost::none,
|
||||
FixedRef<2, 2> H2 = boost::none) const;
|
||||
|
||||
/** Return point coordinates in global frame */
|
||||
Point2 transform_from(const Point2& point,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const;
|
||||
FixedRef<2, 3> H1 = boost::none,
|
||||
FixedRef<2, 2> H2 = boost::none) const;
|
||||
|
||||
/** syntactic sugar for transform_from */
|
||||
inline Point2 operator*(const Point2& point) const { return transform_from(point);}
|
||||
|
|
|
|||
|
|
@ -254,13 +254,8 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_to(const Point3& p) const {
|
||||
return R_.unrotate(p - t_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
|
||||
boost::optional<Matrix3&> Dpoint) const {
|
||||
Point3 Pose3::transform_to(const Point3& p, FixedRef<3,6> Dpose,
|
||||
FixedRef<3,3> Dpoint) const {
|
||||
// Only get transpose once, to avoid multiple allocations,
|
||||
// as well as multiple conversions in the Quaternion case
|
||||
const Matrix3 Rt = R_.transpose();
|
||||
|
|
@ -277,24 +272,6 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
|
|||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
|
||||
boost::optional<Matrix&> Dpoint) const {
|
||||
const Matrix3 Rt = R_.transpose();
|
||||
const Point3 q(Rt*(p - t_).vector());
|
||||
if (Dpose) {
|
||||
const double wx = q.x(), wy = q.y(), wz = q.z();
|
||||
Dpose->resize(3, 6);
|
||||
(*Dpose) <<
|
||||
0.0, -wz, +wy,-1.0, 0.0, 0.0,
|
||||
+wz, 0.0, -wx, 0.0,-1.0, 0.0,
|
||||
-wy, +wx, 0.0, 0.0, 0.0,-1.0;
|
||||
}
|
||||
if (Dpoint)
|
||||
*Dpoint = Rt;
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
|
|
|
|||
|
|
@ -249,13 +249,9 @@ public:
|
|||
* @param Dpoint optional 3*3 Jacobian wrpt point
|
||||
* @return point in Pose coordinates
|
||||
*/
|
||||
Point3 transform_to(const Point3& p) const;
|
||||
|
||||
Point3 transform_to(const Point3& p,
|
||||
boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const;
|
||||
|
||||
Point3 transform_to(const Point3& p,
|
||||
boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const;
|
||||
FixedRef<3,6> Dpose = boost::none,
|
||||
FixedRef<3,3> Dpoint = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
|
|||
|
|
@ -218,15 +218,8 @@ namespace gtsam {
|
|||
Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
|
||||
|
||||
/// Compose two rotations i.e., R= (*this) * R2
|
||||
Rot3 compose(const Rot3& R2) const;
|
||||
|
||||
/// Compose two rotations i.e., R= (*this) * R2
|
||||
Rot3 compose(const Rot3& R2, boost::optional<Matrix3&> H1,
|
||||
boost::optional<Matrix3&> H2) const;
|
||||
|
||||
/// Compose two rotations i.e., R= (*this) * R2
|
||||
Rot3 compose(const Rot3& R2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const;
|
||||
Rot3 compose(const Rot3& R2, FixedRef<3, 3> H1 = boost::none,
|
||||
FixedRef<3, 3> H2 = boost::none) const;
|
||||
|
||||
/** compose two rotations */
|
||||
Rot3 operator*(const Rot3& R2) const;
|
||||
|
|
|
|||
|
|
@ -142,23 +142,11 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::compose (const Rot3& R2) const {
|
||||
return *this * R2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::compose (const Rot3& R2,
|
||||
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
|
||||
if (H1) *H1 = R2.transpose();
|
||||
if (H2) *H2 = I3;
|
||||
return *this * R2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::compose (const Rot3& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = R2.transpose();
|
||||
if (H2) *H2 = I3;
|
||||
Rot3 Rot3::compose(const Rot3& R2, FixedRef<3, 3> H1, FixedRef<3, 3> H2) const {
|
||||
if (H1)
|
||||
*H1 = R2.transpose();
|
||||
if (H2)
|
||||
*H2 = I3;
|
||||
return *this * R2;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue