Merge remote-tracking branch 'origin/feature/BAD_FixedJacobians' into feature/BAD

Conflicts:
	gtsam_unstable/nonlinear/tests/testBADFactor.cpp
release/4.3a0
dellaert 2014-10-07 20:59:09 +02:00
commit d38bcf1805
34 changed files with 698 additions and 340 deletions

View File

@ -48,7 +48,7 @@ public:
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
SimpleCamera camera(pose, *K_); SimpleCamera camera(pose, *K_);
Point2 reprojectionError(camera.project(P_, H) - p_); Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_);
return reprojectionError.vector(); return reprojectionError.vector();
} }
}; };

View File

@ -37,10 +37,28 @@ namespace gtsam {
typedef Eigen::MatrixXd Matrix; typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor; typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
typedef Eigen::Matrix2d Matrix2;
typedef Eigen::Matrix3d Matrix3; typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Matrix4d Matrix4; typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix<double,5,5> Matrix5;
typedef Eigen::Matrix<double,6,6> Matrix6; typedef Eigen::Matrix<double,6,6> Matrix6;
typedef Eigen::Matrix<double,2,3> Matrix23;
typedef Eigen::Matrix<double,2,4> Matrix24;
typedef Eigen::Matrix<double,2,5> Matrix25;
typedef Eigen::Matrix<double,2,6> Matrix26;
typedef Eigen::Matrix<double,2,7> Matrix27;
typedef Eigen::Matrix<double,2,8> Matrix28;
typedef Eigen::Matrix<double,2,9> Matrix29;
typedef Eigen::Matrix<double,3,2> Matrix32;
typedef Eigen::Matrix<double,3,4> Matrix34;
typedef Eigen::Matrix<double,3,5> Matrix35;
typedef Eigen::Matrix<double,3,6> Matrix36;
typedef Eigen::Matrix<double,3,7> Matrix37;
typedef Eigen::Matrix<double,3,8> Matrix38;
typedef Eigen::Matrix<double,3,9> Matrix39;
// Matrix expressions for accessing parts of matrices // Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix; typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix; typedef Eigen::Block<const Matrix> ConstSubMatrix;

View File

@ -36,7 +36,12 @@ typedef Eigen::VectorXd Vector;
// Commonly used fixed size vectors // Commonly used fixed size vectors
typedef Eigen::Vector2d Vector2; typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3; typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix<double, 4, 1> Vector4;
typedef Eigen::Matrix<double, 5, 1> Vector5;
typedef Eigen::Matrix<double, 6, 1> Vector6; typedef Eigen::Matrix<double, 6, 1> Vector6;
typedef Eigen::Matrix<double, 7, 1> Vector7;
typedef Eigen::Matrix<double, 8, 1> Vector8;
typedef Eigen::Matrix<double, 9, 1> Vector9;
typedef Eigen::VectorBlock<Vector> SubVector; typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector; typedef Eigen::VectorBlock<const Vector> ConstSubVector;

View File

@ -36,6 +36,8 @@ private:
double u0_, v0_; ///< image center, not a parameter to be optimized but a constant double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
public: public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 3;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{

View File

@ -53,23 +53,23 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx, static Matrix29 D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny, double yy, double xy, double rr, double r4, double pnx, double pny,
const Eigen::Matrix<double, 2, 2>& DK) { const Matrix2& DK) {
Eigen::Matrix<double, 2, 5> DR1; Matrix25 DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 4> DR2; Matrix24 DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy; y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D; Matrix29 D;
D << DR1, DK * DR2; D << DR1, DK * DR2;
return D; return D;
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr, static Matrix2 D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2, double g, double k1, double k2, double p1, double p2,
const Eigen::Matrix<double, 2, 2>& DK) { const Matrix2& DK) {
const double drdx = 2. * x; const double drdx = 2. * x;
const double drdy = 2. * y; const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@ -82,7 +82,7 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
const double dDydx = 2. * p2 * y + p1 * drdx; const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Eigen::Matrix<double, 2, 2> DR; Matrix2 DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy; y * dgdx + dDydx, g + y * dgdy + dDydy;
@ -110,7 +110,7 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
const double pnx = g * x + dx; const double pnx = g * x + dx;
const double pny = g * y + dy; const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK; Matrix2 DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_; if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration // Derivative for calibration
@ -161,7 +161,7 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
const double rr = xx + yy; const double rr = xx + yy;
const double r4 = rr * rr; const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4); const double g = (1 + k1_ * rr + k2_ * r4);
Eigen::Matrix<double, 2, 2> DK; Matrix2 DK;
DK << fx_, s_, 0.0, fy_; DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
} }
@ -176,7 +176,7 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx; const double pnx = g * x + dx;
const double pny = g * y + dy; const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK; Matrix2 DK;
DK << fx_, s_, 0.0, fy_; DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
} }

View File

@ -46,6 +46,9 @@ protected:
double p1_, p2_ ; // tangential distortion double p1_, p2_ ; // tangential distortion
public: public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 9;
Matrix K() const ; Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ; Vector vector() const ;

View File

@ -50,8 +50,9 @@ private:
double xi_; // mirror parameter double xi_; // mirror parameter
public: public:
//Matrix K() const ; /// dimension of the variable - used to autodetect sizes
//Eigen::Vector4d k() const { return Base::k(); } static const size_t dimension = 10;
Vector vector() const ; Vector vector() const ;
/// @name Standard Constructors /// @name Standard Constructors

View File

@ -86,6 +86,21 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
} }
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
boost::optional<Matrix2&> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
if (Dp) *Dp << fx_, s_, 0.0, fy_;
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p) const {
const double x = p.x(), y = p.y();
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3_S2::calibrate(const Point2& p) const { Point2 Cal3_S2::calibrate(const Point2& p) const {
const double u = p.x(), v = p.y(); const double u = p.x(), v = p.y();

View File

@ -36,6 +36,8 @@ private:
double fx_, fy_, s_, u0_, v0_; double fx_, fy_, s_, u0_, v0_;
public: public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 5;
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
@ -144,12 +146,29 @@ public:
/** /**
* convert intrinsic coordinates xy to image coordinates uv * convert intrinsic coordinates xy to image coordinates uv
* @param p point in intrinsic coordinates * @param p point in intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p) const;
/**
* convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
* @param p point in intrinsic coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates * @return point in image coordinates
*/ */
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal = Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
boost::none, boost::optional<Matrix&> Dp = boost::none) const; boost::optional<Matrix2&> Dp) const;
/**
* convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves
* @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&> Dcal,
boost::optional<Matrix&> Dp) const;
/** /**
* convert image coordinates uv to intrinsic coordinates xy * convert image coordinates uv to intrinsic coordinates xy
@ -181,12 +200,12 @@ public:
/// return DOF, dimensionality of tangent space /// return DOF, dimensionality of tangent space
inline size_t dim() const { inline size_t dim() const {
return 5; return dimension;
} }
/// return DOF, dimensionality of tangent space /// return DOF, dimensionality of tangent space
static size_t Dim() { static size_t Dim() {
return 5; return dimension;
} }
/// Given 5-dim tangent vector, create new calibration /// Given 5-dim tangent vector, create new calibration

View File

@ -270,17 +270,15 @@ public:
* @param Dpoint is the 2*3 Jacobian w.r.t. P * @param Dpoint is the 2*3 Jacobian w.r.t. P
*/ */
inline static Point2 project_to_camera(const Point3& P, inline static Point2 project_to_camera(const Point3& P,
boost::optional<Matrix&> Dpoint = boost::none) { boost::optional<Matrix23&> Dpoint = boost::none) {
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (P.z() <= 0) if (P.z() <= 0)
throw CheiralityException(); throw CheiralityException();
#endif #endif
double d = 1.0 / P.z(); double d = 1.0 / P.z();
const double u = P.x() * d, v = P.y() * d; const double u = P.x() * d, v = P.y() * d;
if (Dpoint) { if (Dpoint)
Dpoint->resize(2,3);
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
}
return Point2(u, v); return Point2(u, v);
} }
@ -291,6 +289,22 @@ public:
return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); 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,Calibration::dimension> Matrix2K;
/** project a point from world coordinate to the image /** project a point from world coordinate to the image
* @param pw is a point in world coordinates * @param pw is a point in world coordinates
* @param Dpose is the Jacobian w.r.t. pose3 * @param Dpose is the Jacobian w.r.t. pose3
@ -299,9 +313,44 @@ public:
*/ */
inline Point2 project( inline Point2 project(
const Point3& pw, // const Point3& pw, //
boost::optional<Matrix&> Dpose = boost::none, boost::optional<Matrix26&> Dpose,
boost::optional<Matrix&> Dpoint = boost::none, boost::optional<Matrix23&> Dpoint,
boost::optional<Matrix&> Dcal = boost::none) const { boost::optional<Matrix2K&> 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)
calculateDpose(pn, d, Dpi_pn, *Dpose);
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 // Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw); const Point3 pc = pose_.transform_to(pw);
@ -327,7 +376,7 @@ public:
} }
return pi; return pi;
} else } else
return K_.uncalibrate(pn, Dcal); return K_.uncalibrate(pn, Dcal, boost::none);
} }
/** project a point at infinity from world coordinate to the image /** project a point at infinity from world coordinate to the image
@ -356,7 +405,7 @@ public:
Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
// camera to normalized image coordinate // camera to normalized image coordinate
Matrix Dpn_pc; // 2*3 Matrix23 Dpn_pc; // 2*3
const Point2 pn = project_to_camera(pc, Dpn_pc); const Point2 pn = project_to_camera(pc, Dpn_pc);
// uncalibration // uncalibration
@ -391,12 +440,12 @@ public:
const double z = pc.z(), d = 1.0 / z; const double z = pc.z(), d = 1.0 / z;
// uncalibration // uncalibration
Matrix Dcal, Dpi_pn(2, 2); Matrix Dcal, Dpi_pn(2,2);
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
if (Dcamera) { if (Dcamera) {
Dcamera->resize(2, this->dim()); Dcamera->resize(2, this->dim());
calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>()); calculateDpose(pn, d, Dpi_pn.block<2,2>(0,0), Dcamera->leftCols<6>());
Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
} }
if (Dpoint) { if (Dpoint) {
@ -518,16 +567,16 @@ private:
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
*/ */
template<typename Derived> template<typename Derived>
static void calculateDpose(const Point2& pn, double d, const Matrix& Dpi_pn, static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn,
Eigen::MatrixBase<Derived> const & Dpose) { Eigen::MatrixBase<Derived> const & Dpose) {
// optimized version of derivatives, see CalibratedCamera.nb // optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y(); const double u = pn.x(), v = pn.y();
double uv = u * v, uu = u * u, vv = v * v; double uv = u * v, uu = u * u, vv = v * v;
Eigen::Matrix<double, 2, 6> Dpn_pose; Matrix26 Dpn_pose;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
assert(Dpose.rows()==2 && Dpose.cols()==6); assert(Dpose.rows()==2 && Dpose.cols()==6);
const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = // const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
Dpi_pn.block<2, 2>(0, 0) * Dpn_pose; Dpi_pn * Dpn_pose;
} }
/** /**
@ -539,18 +588,18 @@ private:
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
*/ */
template<typename Derived> template<typename Derived>
static void calculateDpoint(const Point2& pn, double d, const Matrix& R, static void calculateDpoint(const Point2& pn, double d, const Matrix3& R,
const Matrix& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) { const Matrix2& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
// optimized version of derivatives, see CalibratedCamera.nb // optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y(); const double u = pn.x(), v = pn.y();
Eigen::Matrix<double, 2, 3> Dpn_point; Matrix23 Dpn_point;
Dpn_point << // Dpn_point << //
R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
/**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
Dpn_point *= d; Dpn_point *= d;
assert(Dpoint.rows()==2 && Dpoint.cols()==3); assert(Dpoint.rows()==2 && Dpoint.cols()==3);
const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = // const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
Dpi_pn.block<2, 2>(0, 0) * Dpn_point; Dpi_pn * Dpn_point;
} }
/// @} /// @}

View File

@ -123,6 +123,27 @@ Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); 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 {
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x();
if (H2) *H2 = r_.transpose();
return q;
}
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SE(2) section // see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point, Point2 Pose2::transform_to(const Point2& point,
@ -161,6 +182,62 @@ Point2 Pose2::transform_from(const Point2& p,
return q + t_; return q + t_;
} }
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2) const {
// get cosines and sines from rotation matrices
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
// t = R1' * (t2-t1)
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
return Pose2(R,t);
}
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const {
// get cosines and sines from rotation matrices
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
// t = R1' * (t2-t1)
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0;
}
if (H2) *H2 = I3;
return Pose2(R,t);
}
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1, Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {

View File

@ -123,10 +123,19 @@ public:
/** /**
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame
*/ */
Pose2 between(const Pose2& p2, Pose2 between(const Pose2& p2) const;
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
/// @} /// @}
/// @name Manifold /// @name Manifold
@ -182,10 +191,18 @@ public:
/// @name Group Action on Point2 /// @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 */ /** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point, Point2 transform_to(const Point2& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix23&> H1,
boost::optional<Matrix&> H2=boost::none) const; 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;
/** Return point coordinates in global frame */ /** Return point coordinates in global frame */
Point2 transform_from(const Point2& point, Point2 transform_from(const Point2& point,

View File

@ -254,16 +254,36 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose, Point3 Pose3::transform_to(const Point3& p) const {
boost::optional<Matrix&> Dpoint) const { return R_.unrotate(p - t_);
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
boost::optional<Matrix3&> Dpoint) 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
Matrix3 Rt(R_.transpose()); Matrix3 Rt(R_.transpose());
const Point3 q(Rt*(p - t_).vector()); const Point3 q(Rt*(p - t_).vector());
if (Dpose) { if (Dpose) {
double wx = q.x(); const double wx = q.x(), wy = q.y(), wz = q.z();
double wy = q.y(); (*Dpose) <<
double wz = q.z(); 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;
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) 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->resize(3, 6);
(*Dpose) << (*Dpose) <<
0.0, -wz, +wy,-1.0, 0.0, 0.0, 0.0, -wz, +wy,-1.0, 0.0, 0.0,

View File

@ -250,8 +250,13 @@ public:
* @param Dpoint optional 3*3 Jacobian wrpt point * @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in Pose coordinates * @return point in Pose coordinates
*/ */
Point3 transform_to(const Point3& p) const;
Point3 transform_to(const Point3& p, Point3 transform_to(const Point3& p,
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const; boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const;
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface

View File

@ -97,13 +97,33 @@ Unit3 Rot3::operator*(const Unit3& p) const {
return rotate(p); return rotate(p);
} }
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const {
Matrix3 Rt(transpose());
Point3 q(Rt * p.vector()); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1)
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
if (H2)
*H2 = Rt;
return q;
}
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SO(3) section // see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p, Point3 Rot3::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point3 q(transpose()*p.vector()); // q = Rt*p Matrix3 Rt(transpose());
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); Point3 q(Rt * p.vector()); // q = Rt*p
if (H2) *H2 = transpose(); const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1) {
H1->resize(3,3);
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
}
if (H2)
*H2 = Rt;
return q; return q;
} }

View File

@ -219,8 +219,15 @@ namespace gtsam {
Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const; Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/// Compose two rotations i.e., R= (*this) * R2 /// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, Rot3 compose(const Rot3& R2) const;
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) 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;
/** compose two rotations */ /** compose two rotations */
Rot3 operator*(const Rot3& R2) const; Rot3 operator*(const Rot3& R2) const;
@ -328,11 +335,16 @@ namespace gtsam {
/// rotate point from rotated coordinate frame to world = R*p /// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const; Point3 operator*(const Point3& p) const;
/** /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
* rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ Point3 unrotate(const Point3& p) const;
*/
Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none, /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
boost::optional<Matrix&> H2 = boost::none) const; Point3 unrotate(const Point3& p, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
/// @} /// @}
/// @name Group Action on Unit3 /// @name Group Action on Unit3

View File

@ -143,6 +143,19 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
-swy + C02, swx + C12, c + C22); -swy + C02, swx + C12, c + C22);
} }
/* ************************************************************************* */
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, Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -301,6 +314,12 @@ Quaternion Rot3::toQuaternion() const {
return Quaternion(rot_); return Quaternion(rot_);
} }
/* ************************************************************************* */
Point3 Rot3::unrotate(const Point3& p) const {
// Eigen expression
return Point3(rot_.transpose()*p.vector()); // q = Rt*p
}
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

View File

@ -82,6 +82,19 @@ namespace gtsam {
Rot3 Rot3::rodriguez(const Vector& w, double theta) { Rot3 Rot3::rodriguez(const Vector& w, double theta) {
return Quaternion(Eigen::AngleAxisd(theta, w)); } return Quaternion(Eigen::AngleAxisd(theta, w)); }
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2) const {
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
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 Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2, Rot3 Rot3::compose(const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -159,6 +172,11 @@ namespace gtsam {
Quaternion Rot3::toQuaternion() const { return quaternion_; } Quaternion Rot3::toQuaternion() const { return quaternion_; }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::unrotate(const Point3& p) const {
return Point3(transpose()*p.vector()); // q = Rt*p
}
/* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

View File

@ -115,7 +115,7 @@ public:
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 = Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const { boost::none) const {
try { try {
Point2 error(camera_.project(point, boost::none, H2) - measured_); Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_);
return error.vector(); return error.vector();
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
if (H2) if (H2)
@ -155,7 +155,7 @@ public:
// Would be even better if we could pass blocks to project // Would be even better if we could pass blocks to project
const Point3& point = x.at<Point3>(key()); const Point3& point = x.at<Point3>(key());
b = -(camera_.project(point, boost::none, A) - measured_).vector(); b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector();
if (noiseModel_) if (noiseModel_)
this->noiseModel_->WhitenSystem(A, b); this->noiseModel_->WhitenSystem(A, b);

View File

@ -125,7 +125,7 @@ static Point2 project2(const Pose3& pose, const Point3& point) {
TEST( SimpleCamera, Dproject_point_pose) TEST( SimpleCamera, Dproject_point_pose)
{ {
Matrix Dpose, Dpoint; Matrix Dpose, Dpoint;
Point2 result = camera.project(point1, Dpose, Dpoint); Point2 result = camera.project(point1, Dpose, Dpoint, boost::none);
Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); Matrix numerical_pose = numericalDerivative21(project2, pose1, point1);
Matrix numerical_point = numericalDerivative22(project2, pose1, point1); Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
CHECK(assert_equal(result, Point2(-100, 100) )); CHECK(assert_equal(result, Point2(-100, 100) ));

View File

@ -53,7 +53,7 @@ public:
static Point3 unrotate(const Rot2& R, const Point3& p, static Point3 unrotate(const Rot2& R, const Point3& p,
boost::optional<Matrix&> HR = boost::none) { boost::optional<Matrix&> HR = boost::none) {
Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR); Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none);
if (HR) { if (HR) {
// assign to temporary first to avoid error in Win-Debug mode // assign to temporary first to avoid error in Win-Debug mode
Matrix H = HR->col(2); Matrix H = HR->col(2);
@ -106,7 +106,7 @@ public:
Vector evaluateError(const Rot3& nRb, Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRbÕ * nM + b // measured bM = nRbÕ * nM + b
Point3 hx = nRb.unrotate(nM_, H) + bias_; Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
return (hx - measured_).vector(); return (hx - measured_).vector();
} }
}; };

View File

@ -174,12 +174,14 @@ public:
} else { } else {
// Calculate derivatives. TODO if slow: optimize with Mathematica // Calculate derivatives. TODO if slow: optimize with Mathematica
// 3*2 3*3 3*3 2*3 // 3*2 3*3 3*3
Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2; Matrix D_1T2_dir, DdP2_rot, DP2_point;
Point3 _1T2 = E.direction().point3(D_1T2_dir); Point3 _1T2 = E.direction().point3(D_1T2_dir);
Point3 d1T2 = d * _1T2; Point3 d1T2 = d * _1T2;
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
Matrix23 Dpn_dP2;
pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
if (DE) { if (DE) {

View File

@ -29,7 +29,6 @@ public:
protected: protected:
typedef Eigen::Matrix<double, 2, D> Matrix2D; ///< type of an F block typedef Eigen::Matrix<double, 2, D> Matrix2D; ///< type of an F block
typedef Eigen::Matrix<double, 2, 3> Matrix23;
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
@ -203,7 +202,7 @@ public:
// F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
// static const Eigen::Matrix<double, 2, 2> I2 = eye(2); // static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
// Eigen::Matrix<double, 2, 2> Q = // // Matrix2 Q = //
// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
// blocks[j] = Fj.transpose() * Q * Fj; // blocks[j] = Fj.transpose() * Q * Fj;
} }

View File

@ -132,17 +132,17 @@ namespace gtsam {
if(H1) { if(H1) {
gtsam::Matrix H0; gtsam::Matrix H0;
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_); PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_); Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
*H1 = *H1 * H0; *H1 = *H1 * H0;
return reprojectionError.vector(); return reprojectionError.vector();
} else { } else {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_); PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_); Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
return reprojectionError.vector(); return reprojectionError.vector();
} }
} else { } else {
PinholeCamera<CALIBRATION> camera(pose, *K_); PinholeCamera<CALIBRATION> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_); Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
return reprojectionError.vector(); return reprojectionError.vector();
} }
} catch( CheiralityException& e) { } catch( CheiralityException& e) {

View File

@ -125,7 +125,7 @@ public:
Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1); Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1);
Matrix D_gravityBody_gk; Matrix D_gravityBody_gk;
Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk); Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, boost::none);
Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;

View File

@ -104,7 +104,7 @@ public:
} }
else { else {
gtsam::Matrix J2; gtsam::Matrix J2;
gtsam::Point2 uv= camera.project(landmark,H1, J2); gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none);
if (H1) { if (H1) {
*H1 = (*H1) * gtsam::eye(6); *H1 = (*H1) * gtsam::eye(6);
} }

View File

@ -22,6 +22,7 @@
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
namespace gtsam { namespace gtsam {
@ -134,16 +135,25 @@ public:
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
template<class T>
struct JacobianTrace { struct JacobianTrace {
T t; virtual ~JacobianTrace() {
T value() const {
return t;
} }
virtual void reverseAD(JacobianMap& jacobians) const = 0; virtual void reverseAD(JacobianMap& jacobians) const = 0;
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0;
// template<class JacobianFT>
// void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const {
}; };
typedef JacobianTrace* TracePtr;
//template <class Derived>
//struct TypedTrace {
// virtual void reverseAD(JacobianMap& jacobians) const = 0;
// virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0;
//// template<class JacobianFT>
//// void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const {
//};
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/** /**
* Expression node. The superclass for objects that do the heavy lifting * Expression node. The superclass for objects that do the heavy lifting
@ -175,8 +185,7 @@ public:
virtual Augmented<T> forward(const Values& values) const = 0; virtual Augmented<T> forward(const Values& values) const = 0;
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual T traceExecution(const Values& values, TracePtr& p) const = 0;
const Values& values) const = 0;
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -217,7 +226,7 @@ public:
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> { struct Trace: public JacobianTrace {
/// If the expression is just a constant, we do nothing /// If the expression is just a constant, we do nothing
virtual void reverseAD(JacobianMap& jacobians) const { virtual void reverseAD(JacobianMap& jacobians) const {
} }
@ -227,11 +236,10 @@ public:
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual T traceExecution(const Values& values, TracePtr& p) const {
const Values& values) const { Trace* trace = new Trace();
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); p = static_cast<TracePtr>(trace);
trace->t = constant_; return constant_;
return trace;
} }
}; };
@ -270,12 +278,11 @@ public:
/// Return value and derivatives /// Return value and derivatives
virtual Augmented<T> forward(const Values& values) const { virtual Augmented<T> forward(const Values& values) const {
T t = value(values); return Augmented<T>(values.at<T>(key_), key_);
return Augmented<T>(t, key_);
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> { struct Trace: public JacobianTrace {
Key key; Key key;
/// If the expression is just a leaf, we just insert an identity matrix /// If the expression is just a leaf, we just insert an identity matrix
virtual void reverseAD(JacobianMap& jacobians) const { virtual void reverseAD(JacobianMap& jacobians) const {
@ -293,12 +300,11 @@ public:
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual T traceExecution(const Values& values, TracePtr& p) const {
const Values& values) const { Trace* trace = new Trace();
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); p = static_cast<TracePtr>(trace);
trace->t = value(values);
trace->key = key_; trace->key = key_;
return trace; return values.at<T>(key_);
} }
}; };
@ -310,7 +316,8 @@ class UnaryExpression: public ExpressionNode<T> {
public: public:
typedef boost::function<T(const A&, boost::optional<Matrix&>)> Function; typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
typedef boost::function<T(const A&, boost::optional<JacobianTA&>)> Function;
private: private:
@ -344,33 +351,35 @@ public:
virtual Augmented<T> forward(const Values& values) const { virtual Augmented<T> forward(const Values& values) const {
using boost::none; using boost::none;
Augmented<A> argument = this->expressionA_->forward(values); Augmented<A> argument = this->expressionA_->forward(values);
Matrix dTdA; JacobianTA dTdA;
T t = function_(argument.value(), T t = function_(argument.value(),
argument.constant() ? none : boost::optional<Matrix&>(dTdA)); argument.constant() ? none : boost::optional<JacobianTA&>(dTdA));
return Augmented<T>(t, dTdA, argument.jacobians()); return Augmented<T>(t, dTdA, argument.jacobians());
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> { struct Trace: public JacobianTrace {
boost::shared_ptr<JacobianTrace<A> > trace1; TracePtr trace;
Matrix dTdA; JacobianTA dTdA;
virtual ~Trace() {
delete trace;
}
/// Start the reverse AD process /// Start the reverse AD process
virtual void reverseAD(JacobianMap& jacobians) const { virtual void reverseAD(JacobianMap& jacobians) const {
trace1->reverseAD(dTdA, jacobians); trace->reverseAD(dTdA, jacobians);
} }
/// Given df/dT, multiply in dT/dA and continue reverse AD process /// Given df/dT, multiply in dT/dA and continue reverse AD process
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
trace1->reverseAD(dFdT * dTdA, jacobians); trace->reverseAD(dFdT * dTdA, jacobians);
} }
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual T traceExecution(const Values& values, TracePtr& p) const {
const Values& values) const { Trace* trace = new Trace();
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); p = static_cast<TracePtr>(trace);
trace->trace1 = this->expressionA_->traceExecution(values); A a = this->expressionA_->traceExecution(values,trace->trace);
trace->t = function_(trace->trace1->value(), trace->dTdA); return function_(a, trace->dTdA);
return trace;
} }
}; };
@ -382,9 +391,11 @@ class BinaryExpression: public ExpressionNode<T> {
public: public:
typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA1;
typedef Eigen::Matrix<double, T::dimension, A2::dimension> JacobianTA2;
typedef boost::function< typedef boost::function<
T(const A1&, const A2&, boost::optional<Matrix&>, T(const A1&, const A2&, boost::optional<JacobianTA1&>,
boost::optional<Matrix&>)> Function; boost::optional<JacobianTA2&>)> Function;
private: private:
@ -426,18 +437,23 @@ public:
using boost::none; using boost::none;
Augmented<A1> a1 = this->expressionA1_->forward(values); Augmented<A1> a1 = this->expressionA1_->forward(values);
Augmented<A2> a2 = this->expressionA2_->forward(values); Augmented<A2> a2 = this->expressionA2_->forward(values);
Matrix dTdA1, dTdA2; JacobianTA1 dTdA1;
JacobianTA2 dTdA2;
T t = function_(a1.value(), a2.value(), T t = function_(a1.value(), a2.value(),
a1.constant() ? none : boost::optional<Matrix&>(dTdA1), a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1),
a2.constant() ? none : boost::optional<Matrix&>(dTdA2)); a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2));
return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians()); return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians());
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> { struct Trace: public JacobianTrace {
boost::shared_ptr<JacobianTrace<A1> > trace1; TracePtr trace1, trace2;
boost::shared_ptr<JacobianTrace<A2> > trace2; JacobianTA1 dTdA1;
Matrix dTdA1, dTdA2; JacobianTA2 dTdA2;
virtual ~Trace() {
delete trace1;
delete trace2;
}
/// Start the reverse AD process /// Start the reverse AD process
virtual void reverseAD(JacobianMap& jacobians) const { virtual void reverseAD(JacobianMap& jacobians) const {
trace1->reverseAD(dTdA1, jacobians); trace1->reverseAD(dTdA1, jacobians);
@ -451,14 +467,12 @@ public:
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual T traceExecution(const Values& values, TracePtr& p) const {
const Values& values) const { Trace* trace = new Trace();
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); p = static_cast<TracePtr>(trace);
trace->trace1 = this->expressionA1_->traceExecution(values); A1 a1 = this->expressionA1_->traceExecution(values,trace->trace1);
trace->trace2 = this->expressionA2_->traceExecution(values); A2 a2 = this->expressionA2_->traceExecution(values,trace->trace2);
trace->t = function_(trace->trace1->value(), trace->trace2->value(), return function_(a1, a2, trace->dTdA1, trace->dTdA2);
trace->dTdA1, trace->dTdA2);
return trace;
} }
}; };
@ -471,9 +485,12 @@ class TernaryExpression: public ExpressionNode<T> {
public: public:
typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA1;
typedef Eigen::Matrix<double, T::dimension, A2::dimension> JacobianTA2;
typedef Eigen::Matrix<double, T::dimension, A3::dimension> JacobianTA3;
typedef boost::function< typedef boost::function<
T(const A1&, const A2&, const A3&, boost::optional<Matrix&>, T(const A1&, const A2&, const A3&, boost::optional<JacobianTA1&>,
boost::optional<Matrix&>, boost::optional<Matrix&>)> Function; boost::optional<JacobianTA2&>, boost::optional<JacobianTA3&>)> Function;
private: private:
@ -523,21 +540,28 @@ public:
Augmented<A1> a1 = this->expressionA1_->forward(values); Augmented<A1> a1 = this->expressionA1_->forward(values);
Augmented<A2> a2 = this->expressionA2_->forward(values); Augmented<A2> a2 = this->expressionA2_->forward(values);
Augmented<A3> a3 = this->expressionA3_->forward(values); Augmented<A3> a3 = this->expressionA3_->forward(values);
Matrix dTdA1, dTdA2, dTdA3; JacobianTA1 dTdA1;
JacobianTA2 dTdA2;
JacobianTA3 dTdA3;
T t = function_(a1.value(), a2.value(), a3.value(), T t = function_(a1.value(), a2.value(), a3.value(),
a1.constant() ? none : boost::optional<Matrix&>(dTdA1), a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1),
a2.constant() ? none : boost::optional<Matrix&>(dTdA2), a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2),
a3.constant() ? none : boost::optional<Matrix&>(dTdA3)); a3.constant() ? none : boost::optional<JacobianTA3&>(dTdA3));
return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians(), dTdA3, return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians(), dTdA3,
a3.jacobians()); a3.jacobians());
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> { struct Trace: public JacobianTrace {
boost::shared_ptr<JacobianTrace<A1> > trace1; TracePtr trace1, trace2, trace3;
boost::shared_ptr<JacobianTrace<A2> > trace2; JacobianTA1 dTdA1;
boost::shared_ptr<JacobianTrace<A3> > trace3; JacobianTA2 dTdA2;
Matrix dTdA1, dTdA2, dTdA3; JacobianTA3 dTdA3;
virtual ~Trace() {
delete trace1;
delete trace2;
delete trace3;
}
/// Start the reverse AD process /// Start the reverse AD process
virtual void reverseAD(JacobianMap& jacobians) const { virtual void reverseAD(JacobianMap& jacobians) const {
trace1->reverseAD(dTdA1, jacobians); trace1->reverseAD(dTdA1, jacobians);
@ -553,15 +577,13 @@ public:
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual T traceExecution(const Values& values, TracePtr& p) const {
const Values& values) const { Trace* trace = new Trace();
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); p = static_cast<TracePtr>(trace);
trace->trace1 = this->expressionA1_->traceExecution(values); A1 a1 = this->expressionA1_->traceExecution(values,trace->trace1);
trace->trace2 = this->expressionA2_->traceExecution(values); A2 a2 = this->expressionA2_->traceExecution(values,trace->trace2);
trace->trace3 = this->expressionA3_->traceExecution(values); A3 a3 = this->expressionA3_->traceExecution(values,trace->trace3);
trace->t = function_(trace->trace1->value(), trace->trace2->value(), return function_(a1, a2, a3, trace->dTdA1, trace->dTdA2, trace->dTdA3);
trace->trace3->value(), trace->dTdA1, trace->dTdA2, trace->dTdA3);
return trace;
} }
}; };

View File

@ -76,8 +76,10 @@ public:
/// Construct a unary method expression /// Construct a unary method expression
template<typename A1, typename A2> template<typename A1, typename A2>
Expression(const Expression<A1>& expression1, Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, boost::optional<Matrix&>, T (A1::*method)(const A2&,
boost::optional<Matrix&>) const, const Expression<A2>& expression2) { boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA1&>,
boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA2&>) const,
const Expression<A2>& expression2) {
root_.reset( root_.reset(
new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4), new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
expression1, expression2)); expression1, expression2));
@ -94,9 +96,11 @@ public:
/// Construct a ternary function expression /// Construct a ternary function expression
template<typename A1, typename A2, typename A3> template<typename A1, typename A2, typename A3>
Expression(typename TernaryExpression<T, A1, A2, A3>::Function function, Expression(typename TernaryExpression<T, A1, A2, A3>::Function function,
const Expression<A1>& expression1, const Expression<A2>& expression2, const Expression<A3>& expression3) { const Expression<A1>& expression1, const Expression<A2>& expression2,
const Expression<A3>& expression3) {
root_.reset( root_.reset(
new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2, expression3)); new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2,
expression3));
} }
/// Return keys that play in this expression /// Return keys that play in this expression
@ -113,9 +117,11 @@ public:
Augmented<T> augmented(const Values& values) const { Augmented<T> augmented(const Values& values) const {
#define REVERSE_AD #define REVERSE_AD
#ifdef REVERSE_AD #ifdef REVERSE_AD
boost::shared_ptr<JacobianTrace<T> > trace(root_->traceExecution(values)); TracePtr trace;
Augmented<T> augmented(trace->value()); T value = root_->traceExecution(values,trace);
Augmented<T> augmented(value);
trace->reverseAD(augmented.jacobians()); trace->reverseAD(augmented.jacobians());
delete trace;
return augmented; return augmented;
#else #else
return root_->forward(values); return root_->forward(values);
@ -132,8 +138,9 @@ public:
template<class T> template<class T>
struct apply_compose { struct apply_compose {
typedef T result_type; typedef T result_type;
T operator()(const T& x, const T& y, boost::optional<Matrix&> H1, typedef Eigen::Matrix<double, T::dimension, T::dimension> Jacobian;
boost::optional<Matrix&> H2) const { T operator()(const T& x, const T& y, boost::optional<Jacobian&> H1,
boost::optional<Jacobian&> H2) const {
return x.compose(y, H1, H2); return x.compose(y, H1, H2);
} }
}; };

View File

@ -58,8 +58,8 @@ TEST(BADFactor, test) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Unary(Binary(Leaf,Leaf)) // Unary(Binary(Leaf,Leaf))
TEST(BADFactor, test1) { TEST(BADFactor, test1) {
// Create some values // Create some values
Values values; Values values;
@ -82,11 +82,11 @@ TEST(BADFactor, test) {
EXPECT_LONGS_EQUAL(2, f2.dim()); EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values); boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9)); EXPECT( assert_equal(*expected, *gf2, 1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Binary(Leaf,Unary(Binary(Leaf,Leaf))) // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
TEST(BADFactor, test2) { TEST(BADFactor, test2) {
// Create some values // Create some values
Values values; Values values;
@ -123,11 +123,20 @@ TEST(BADFactor, test) {
EXPECT_LONGS_EQUAL(2, f2.dim()); EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values); boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9)); EXPECT( assert_equal(*expected, *gf2, 1e-9));
}
/* ************************************************************************* */ TernaryExpression<Point2, Pose3, Point3, Cal3_S2>::Function fff = project6;
TEST(BADFactor, compose1) { // Try ternary version
BADFactor<Point2> f3(model, measured, project3(x, p, K));
EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f3.dim());
boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
EXPECT( assert_equal(*expected, *gf3, 1e-9));
}
/* ************************************************************************* */
TEST(BADFactor, compose1) {
// Create expression // Create expression
Rot3_ R1(1), R2(2); Rot3_ R1(1), R2(2);
@ -153,11 +162,11 @@ TEST(BADFactor, test) {
boost::shared_ptr<JacobianFactor> jf = // boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf); boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9)); EXPECT( assert_equal(expected, *jf,1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test compose with arguments referring to the same rotation // Test compose with arguments referring to the same rotation
TEST(BADFactor, compose2) { TEST(BADFactor, compose2) {
// Create expression // Create expression
Rot3_ R1(1), R2(1); Rot3_ R1(1), R2(1);
@ -182,11 +191,11 @@ TEST(BADFactor, test) {
boost::shared_ptr<JacobianFactor> jf = // boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf); boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9)); EXPECT( assert_equal(expected, *jf,1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test compose with one arguments referring to a constant same rotation // Test compose with one arguments referring to a constant same rotation
TEST(BADFactor, compose3) { TEST(BADFactor, compose3) {
// Create expression // Create expression
Rot3_ R1(Rot3::identity()), R2(3); Rot3_ R1(Rot3::identity()), R2(3);
@ -211,13 +220,13 @@ TEST(BADFactor, test) {
boost::shared_ptr<JacobianFactor> jf = // boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf); boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9)); EXPECT( assert_equal(expected, *jf,1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test compose with three arguments // Test compose with three arguments
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
boost::optional<Matrix&> H3) { boost::optional<Matrix3&> H3) {
// return dummy derivatives (not correct, but that's ok for testing here) // return dummy derivatives (not correct, but that's ok for testing here)
if (H1) if (H1)
*H1 = eye(3); *H1 = eye(3);
@ -226,9 +235,9 @@ TEST(BADFactor, test) {
if (H3) if (H3)
*H3 = eye(3); *H3 = eye(3);
return R1 * (R2 * R3); return R1 * (R2 * R3);
} }
TEST(BADFactor, composeTernary) { TEST(BADFactor, composeTernary) {
// Create expression // Create expression
Rot3_ A(1), B(2), C(3); Rot3_ A(1), B(2), C(3);
@ -257,9 +266,9 @@ TEST(BADFactor, test) {
boost::shared_ptr<JacobianFactor> jf = // boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf); boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9)); EXPECT( assert_equal(expected, *jf,1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);

View File

@ -32,12 +32,12 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
template<class CAL> template<class CAL>
Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix&> Dcal, Point2 uncalibrate(const CAL& K, const Point2& p,
boost::optional<Matrix&> Dp) { boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {
return K.uncalibrate(p, Dcal, Dp); return K.uncalibrate(p, Dcal, Dp);
} }
static const Rot3 someR = Rot3::RzRyRx(1,2,3); static const Rot3 someR = Rot3::RzRyRx(1, 2, 3);
/* ************************************************************************* */ /* ************************************************************************* */
@ -55,7 +55,7 @@ TEST(Expression, constant) {
TEST(Expression, leaf) { TEST(Expression, leaf) {
Expression<Rot3> R(100); Expression<Rot3> R(100);
Values values; Values values;
values.insert(100,someR); values.insert(100, someR);
Augmented<Rot3> a = R.augmented(values); Augmented<Rot3> a = R.augmented(values);
EXPECT(assert_equal(someR, a.value())); EXPECT(assert_equal(someR, a.value()));
JacobianMap expected; JacobianMap expected;
@ -76,7 +76,6 @@ TEST(Expression, leaf) {
// expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50)); // expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50));
// EXPECT(assert_equal(expected.at(67),a.jacobians().at(67))); // EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Expression, test) { TEST(Expression, test) {
@ -149,8 +148,8 @@ TEST(Expression, compose3) {
/* ************************************************************************* */ /* ************************************************************************* */
// Test with ternary function // Test with ternary function
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
boost::optional<Matrix&> H3) { boost::optional<Matrix3&> H3) {
// return dummy derivatives (not correct, but that's ok for testing here) // return dummy derivatives (not correct, but that's ok for testing here)
if (H1) if (H1)
*H1 = eye(3); *H1 = eye(3);

View File

@ -127,13 +127,13 @@ namespace gtsam {
if(H1 || H2 || H3) { if(H1 || H2 || H3) {
gtsam::Matrix H0, H02; gtsam::Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_); PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
Point2 reprojectionError(camera.project(point, H1, H3) - measured_); Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
*H2 = *H1 * H02; *H2 = *H1 * H02;
*H1 = *H1 * H0; *H1 = *H1 * H0;
return reprojectionError.vector(); return reprojectionError.vector();
} else { } else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_); PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
Point2 reprojectionError(camera.project(point, H1, H3) - measured_); Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
return reprojectionError.vector(); return reprojectionError.vector();
} }
} catch( CheiralityException& e) { } catch( CheiralityException& e) {

View File

@ -10,6 +10,7 @@
#include <gtsam_unstable/nonlinear/Expression.h> #include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <boost/bind.hpp>
namespace gtsam { namespace gtsam {
@ -48,6 +49,16 @@ Point2_ project(const Point3_& p_cam) {
return Point2_(PinholeCamera<Cal3_S2>::project_to_camera, p_cam); return Point2_(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
} }
Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
boost::optional<Matrix26&> Dpose, boost::optional<Matrix23&> Dpoint,
boost::optional<Matrix25&> Dcal) {
return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
}
Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) {
return Point2_(project6, x, p, K);
}
template<class CAL> template<class CAL>
Point2_ uncalibrate(const Expression<CAL>& K, const Point2_& xy_hat) { Point2_ uncalibrate(const Expression<CAL>& K, const Point2_& xy_hat) {
return Point2_(K, &CAL::uncalibrate, xy_hat); return Point2_(K, &CAL::uncalibrate, xy_hat);

View File

@ -26,7 +26,6 @@
#include <time.h> #include <time.h>
#include <iostream> #include <iostream>
#include <iomanip> // std::setprecision #include <iomanip> // std::setprecision
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -47,9 +46,9 @@ void time(const NonlinearFactor& f, const Values& values) {
boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2()); boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());
Point2 myProject(const Pose3& pose, const Point3& point, Point2 myProject(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { boost::optional<Matrix26&> H1, boost::optional<Matrix23&> H2) {
PinholeCamera<Cal3_S2> camera(pose, *fixedK); PinholeCamera<Cal3_S2> camera(pose, *fixedK);
return camera.project(point, H1, H2); return camera.project(point, H1, H2, boost::none);
} }
int main() { int main() {
@ -74,37 +73,43 @@ int main() {
// Dedicated factor // Dedicated factor
// Oct 3, 2014, Macbook Air // Oct 3, 2014, Macbook Air
// 4.2 musecs/call // 4.2 musecs/call
GeneralSFMFactor2<Cal3_S2> oldFactor2(z, model, 1, 2, 3); GeneralSFMFactor2<Cal3_S2> f1(z, model, 1, 2, 3);
time(oldFactor2, values); time(f1, values);
// BADFactor // BADFactor
// Oct 3, 2014, Macbook Air // Oct 3, 2014, Macbook Air
// 20.3 musecs/call // 20.3 musecs/call
BADFactor<Point2> newFactor2(model, z, BADFactor<Point2> f2(model, z,
uncalibrate(K, project(transform_to(x, p)))); uncalibrate(K, project(transform_to(x, p))));
time(newFactor2, values); time(f2, values);
// BADFactor ternary
// Oct 3, 2014, Macbook Air
// 20.3 musecs/call
BADFactor<Point2> f3(model, z, project3(x, p, K));
time(f3, values);
// CALIBRATED // CALIBRATED
// Dedicated factor // Dedicated factor
// Oct 3, 2014, Macbook Air // Oct 3, 2014, Macbook Air
// 3.4 musecs/call // 3.4 musecs/call
GenericProjectionFactor<Pose3, Point3> oldFactor1(z, model, 1, 2, fixedK); GenericProjectionFactor<Pose3, Point3> g1(z, model, 1, 2, fixedK);
time(oldFactor1, values); time(g1, values);
// BADFactor // BADFactor
// Oct 3, 2014, Macbook Air // Oct 3, 2014, Macbook Air
// 16.0 musecs/call // 16.0 musecs/call
BADFactor<Point2> newFactor1(model, z, BADFactor<Point2> g2(model, z,
uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p)))); uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p))));
time(newFactor1, values); time(g2, values);
// BADFactor, optimized // BADFactor, optimized
// Oct 3, 2014, Macbook Air // Oct 3, 2014, Macbook Air
// 9.0 musecs/call // 9.0 musecs/call
typedef PinholeCamera<Cal3_S2> Camera; typedef PinholeCamera<Cal3_S2> Camera;
typedef Expression<Camera> Camera_; typedef Expression<Camera> Camera_;
BADFactor<Point2> newFactor3(model, z, Point2_(myProject, x, p)); BADFactor<Point2> g3(model, z, Point2_(myProject, x, p));
time(newFactor3, values); time(g3, values);
return 0; return 0;
} }

View File

@ -25,7 +25,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
static const int n = 500000; static const int n = 1000000;
void time(const NonlinearFactor& f, const Values& values) { void time(const NonlinearFactor& f, const Values& values) {
long timeLog = clock(); long timeLog = clock();
@ -58,9 +58,13 @@ int main() {
// BADFactor // BADFactor
// Oct 3, 2014, Macbook Air // Oct 3, 2014, Macbook Air
// 20.3 musecs/call // 20.3 musecs/call
BADFactor<Point2> newFactor2(model, z, #define TERNARY
uncalibrate(K, project(transform_to(x, p)))); #ifdef TERNARY
time(newFactor2, values); BADFactor<Point2> f(model, z, project3(x, p, K));
#else
BADFactor<Point2> f(model, z, uncalibrate(K, project(transform_to(x, p))));
#endif
time(f, values);
return 0; return 0;
} }