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

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
parent 38602ebdc4 d7022a21c7
commit d38bcf1805
34 changed files with 698 additions and 340 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/CameraResectioning.cpp
View File

@ -48,7 +48,7 @@ public:
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
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();
}
};

18
gtsam/base/Matrix.h
View File

@ -37,10 +37,28 @@ namespace gtsam {
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
typedef Eigen::Matrix2d Matrix2;
typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix<double,5,5> Matrix5;
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
typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;

5
gtsam/base/Vector.h
View File

@ -36,7 +36,12 @@ typedef Eigen::VectorXd Vector;
// Commonly used fixed size vectors
typedef Eigen::Vector2d Vector2;
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, 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<const Vector> ConstSubVector;

2
gtsam/geometry/Cal3Bundler.h
View File

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

22
gtsam/geometry/Cal3DS2.cpp
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,
const Eigen::Matrix<double, 2, 2>& DK) {
Eigen::Matrix<double, 2, 5> DR1;
const Matrix2& DK) {
Matrix25 DR1;
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, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D;
Matrix29 D;
D << DR1, DK * DR2;
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,
const Eigen::Matrix<double, 2, 2>& DK) {
const Matrix2& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
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 dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Eigen::Matrix<double, 2, 2> DR;
Matrix2 DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
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 pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
Matrix2 DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration
@ -161,7 +161,7 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
Eigen::Matrix<double, 2, 2> DK;
Matrix2 DK;
DK << fx_, s_, 0.0, fy_;
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 pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
Matrix2 DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}

3
gtsam/geometry/Cal3DS2.h
View File

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

5
gtsam/geometry/Cal3Unified.h
View File

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

15
gtsam/geometry/Cal3_S2.cpp
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_);
}
/* ************************************************************************* */
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 {
const double u = p.x(), v = p.y();

27
gtsam/geometry/Cal3_S2.h
View File

@ -36,6 +36,8 @@ private:
double fx_, fy_, s_, u0_, v0_;
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
@ -144,12 +146,29 @@ 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;
/**
* 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 Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal =
boost::none, boost::optional<Matrix&> Dp = boost::none) const;
Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
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
@ -181,12 +200,12 @@ public:
/// return DOF, dimensionality of tangent space
inline size_t dim() const {
return 5;
return dimension;
}
/// return DOF, dimensionality of tangent space
static size_t Dim() {
return 5;
return dimension;
}
/// Given 5-dim tangent vector, create new calibration

85
gtsam/geometry/PinholeCamera.h
View File

@ -270,17 +270,15 @@ public:
* @param Dpoint is the 2*3 Jacobian w.r.t. 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
if (P.z() <= 0)
throw CheiralityException();
#endif
double d = 1.0 / P.z();
const double u = P.x() * d, v = P.y() * d;
if (Dpoint) {
Dpoint->resize(2,3);
if (Dpoint)
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
}
return Point2(u, v);
}
@ -291,6 +289,22 @@ 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,Calibration::dimension> Matrix2K;
/** 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
@ -299,9 +313,44 @@ public:
*/
inline Point2 project(
const Point3& pw, //
boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dpoint = boost::none,
boost::optional<Matrix&> Dcal = boost::none) const {
boost::optional<Matrix26&> Dpose,
boost::optional<Matrix23&> Dpoint,
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
const Point3 pc = pose_.transform_to(pw);
@ -327,7 +376,7 @@ public:
}
return pi;
} else
return K_.uncalibrate(pn, Dcal);
return K_.uncalibrate(pn, Dcal, boost::none);
}
/** 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;
// camera to normalized image coordinate
Matrix Dpn_pc; // 2*3
Matrix23 Dpn_pc; // 2*3
const Point2 pn = project_to_camera(pc, Dpn_pc);
// uncalibration
@ -391,12 +440,12 @@ public:
const double z = pc.z(), d = 1.0 / z;
// uncalibration
Matrix Dcal, Dpi_pn(2, 2);
Matrix Dcal, Dpi_pn(2,2);
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
if (Dcamera) {
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
}
if (Dpoint) {
@ -518,16 +567,16 @@ private:
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
*/
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) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
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;
assert(Dpose.rows()==2 && Dpose.cols()==6);
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
*/
template<typename Derived>
static void calculateDpoint(const Point2& pn, double d, const Matrix& R,
const Matrix& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
static void calculateDpoint(const Point2& pn, double d, const Matrix3& R,
const Matrix2& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
Eigen::Matrix<double, 2, 3> Dpn_point;
Matrix23 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, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
Dpn_point *= d;
assert(Dpoint.rows()==2 && Dpoint.cols()==3);
const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
Dpi_pn.block<2, 2>(0, 0) * Dpn_point;
Dpi_pn * Dpn_point;
}
/// @}

77
gtsam/geometry/Pose2.cpp
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())));
}
/* ************************************************************************* */
// 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
Point2 Pose2::transform_to(const Point2& point,
@ -161,6 +182,62 @@ Point2 Pose2::transform_from(const Point2& p,
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,
boost::optional<Matrix&> H2) const {

27
gtsam/geometry/Pose2.h
View File

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

30
gtsam/geometry/Pose3.cpp
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,
boost::optional<Matrix&> Dpoint) const {
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 {
// Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
Matrix3 Rt(R_.transpose());
const Point3 q(Rt*(p - t_).vector());
if (Dpose) {
double wx = q.x();
double wy = q.y();
double wz = q.z();
const double wx = q.x(), wy = q.y(), wz = q.z();
(*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;
}
/* ************************************************************************* */
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) <<
0.0, -wz, +wy,-1.0, 0.0, 0.0,

7
gtsam/geometry/Pose3.h
View File

@ -250,8 +250,13 @@ 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<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

26
gtsam/geometry/Rot3.cpp
View File

@ -97,13 +97,33 @@ Unit3 Rot3::operator*(const Unit3& p) const {
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
Point3 Rot3::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point3 q(transpose()*p.vector()); // q = Rt*p
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
if (H2) *H2 = transpose();
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->resize(3,3);
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
}
if (H2)
*H2 = Rt;
return q;
}

26
gtsam/geometry/Rot3.h
View File

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

19
gtsam/geometry/Rot3M.cpp
View File

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

18
gtsam/geometry/Rot3Q.cpp
View File

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

4
gtsam/geometry/TriangulationFactor.h
View File

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

2
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -125,7 +125,7 @@ static Point2 project2(const Pose3& pose, const Point3& point) {
TEST( SimpleCamera, Dproject_point_pose)
{
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_point = numericalDerivative22(project2, pose1, point1);
CHECK(assert_equal(result, Point2(-100, 100) ));

4
gtsam/navigation/MagFactor.h
View File

@ -53,7 +53,7 @@ public:
static Point3 unrotate(const Rot2& R, const Point3& p,
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) {
// assign to temporary first to avoid error in Win-Debug mode
Matrix H = HR->col(2);
@ -106,7 +106,7 @@ public:
Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) const {
// 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();
}
};

6
gtsam/slam/EssentialMatrixFactor.h
View File

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

3
gtsam/slam/ImplicitSchurFactor.h
View File

@ -29,7 +29,6 @@ public:
protected:
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 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 :-(
// 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();
// blocks[j] = Fj.transpose() * Q * Fj;
}

6
gtsam/slam/ProjectionFactor.h
View File

@ -132,17 +132,17 @@ namespace gtsam {
if(H1) {
gtsam::Matrix H0;
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;
return reprojectionError.vector();
} else {
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();
}
} else {
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();
}
} catch( CheiralityException& e) {

2
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -125,7 +125,7 @@ public:
Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1);
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 hx = pk - pk_1 - h_*Fu_ - h_*f_ext;

2
gtsam_unstable/geometry/InvDepthCamera3.h
View File

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

164
gtsam_unstable/nonlinear/Expression-inl.h
View File

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

23
gtsam_unstable/nonlinear/Expression.h
View File

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

337
gtsam_unstable/nonlinear/tests/testBADFactor.cpp
View File

@ -58,208 +58,217 @@ TEST(BADFactor, test) {
}
/* ************************************************************************* */
// Unary(Binary(Leaf,Leaf))
TEST(BADFactor, test1) {
// Unary(Binary(Leaf,Leaf))
TEST(BADFactor, test1) {
// Create some values
Values values;
values.insert(1, Pose3());
values.insert(2, Point3(0, 0, 1));
// Create some values
Values values;
values.insert(1, Pose3());
values.insert(2, Point3(0, 0, 1));
// Create old-style factor to create expected value and derivatives
GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2,
boost::make_shared<Cal3_S2>());
double expected_error = old.error(values);
GaussianFactor::shared_ptr expected = old.linearize(values);
// Create old-style factor to create expected value and derivatives
GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2,
boost::make_shared<Cal3_S2>());
double expected_error = old.error(values);
GaussianFactor::shared_ptr expected = old.linearize(values);
// Create leaves
Pose3_ x(1);
Point3_ p(2);
// Create leaves
Pose3_ x(1);
Point3_ p(2);
// Try concise version
BADFactor<Point2> f2(model, measured, project(transform_to(x, p)));
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
}
// Try concise version
BADFactor<Point2> f2(model, measured, project(transform_to(x, p)));
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
}
/* ************************************************************************* */
// Binary(Leaf,Unary(Binary(Leaf,Leaf)))
TEST(BADFactor, test2) {
/* ************************************************************************* */
// Binary(Leaf,Unary(Binary(Leaf,Leaf)))
TEST(BADFactor, test2) {
// Create some values
Values values;
values.insert(1, Pose3());
values.insert(2, Point3(0, 0, 1));
values.insert(3, Cal3_S2());
// Create some values
Values values;
values.insert(1, Pose3());
values.insert(2, Point3(0, 0, 1));
values.insert(3, Cal3_S2());
// Create old-style factor to create expected value and derivatives
GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
double expected_error = old.error(values);
GaussianFactor::shared_ptr expected = old.linearize(values);
// Create old-style factor to create expected value and derivatives
GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
double expected_error = old.error(values);
GaussianFactor::shared_ptr expected = old.linearize(values);
// Create leaves
Pose3_ x(1);
Point3_ p(2);
Cal3_S2_ K(3);
// Create leaves
Pose3_ x(1);
Point3_ p(2);
Cal3_S2_ K(3);
// Create expression tree
Point3_ p_cam(x, &Pose3::transform_to, p);
Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
// Create expression tree
Point3_ p_cam(x, &Pose3::transform_to, p);
Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
// Create factor and check value, dimension, linearization
BADFactor<Point2> f(model, measured, uv_hat);
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
EXPECT( assert_equal(*expected, *gf, 1e-9));
// Create factor and check value, dimension, linearization
BADFactor<Point2> f(model, measured, uv_hat);
EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
EXPECT( assert_equal(*expected, *gf, 1e-9));
// Try concise version
BADFactor<Point2> f2(model, measured,
uncalibrate(K, project(transform_to(x, p))));
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
}
// Try concise version
BADFactor<Point2> f2(model, measured,
uncalibrate(K, project(transform_to(x, p))));
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
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));
}
// Create expression
Rot3_ R1(1), R2(2);
Rot3_ R3 = R1 * R2;
/* ************************************************************************* */
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
TEST(BADFactor, compose1) {
// Create some values
Values values;
values.insert(1, Rot3());
values.insert(2, Rot3());
// Create expression
Rot3_ R1(1), R2(2);
Rot3_ R3 = R1 * R2;
// Check unwhitenedError
std::vector<Matrix> H(2);
Vector actual = f.unwhitenedError(values, H);
EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(eye(3), H[1],1e-9));
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Check linearization
JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
}
// Create some values
Values values;
values.insert(1, Rot3());
values.insert(2, Rot3());
/* ************************************************************************* */
// Test compose with arguments referring to the same rotation
TEST(BADFactor, compose2) {
// Check unwhitenedError
std::vector<Matrix> H(2);
Vector actual = f.unwhitenedError(values, H);
EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(eye(3), H[1],1e-9));
// Create expression
Rot3_ R1(1), R2(1);
Rot3_ R3 = R1 * R2;
// Check linearization
JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
}
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
/* ************************************************************************* */
// Test compose with arguments referring to the same rotation
TEST(BADFactor, compose2) {
// Create some values
Values values;
values.insert(1, Rot3());
// Create expression
Rot3_ R1(1), R2(1);
Rot3_ R3 = R1 * R2;
// Check unwhitenedError
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(2*eye(3), H[0],1e-9));
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Check linearization
JacobianFactor expected(1, 2 * eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
}
// Create some values
Values values;
values.insert(1, Rot3());
/* ************************************************************************* */
// Test compose with one arguments referring to a constant same rotation
TEST(BADFactor, compose3) {
// Check unwhitenedError
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(2*eye(3), H[0],1e-9));
// Create expression
Rot3_ R1(Rot3::identity()), R2(3);
Rot3_ R3 = R1 * R2;
// Check linearization
JacobianFactor expected(1, 2 * eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
}
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
/* ************************************************************************* */
// Test compose with one arguments referring to a constant same rotation
TEST(BADFactor, compose3) {
// Create some values
Values values;
values.insert(3, Rot3());
// Create expression
Rot3_ R1(Rot3::identity()), R2(3);
Rot3_ R3 = R1 * R2;
// Check unwhitenedError
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(eye(3), H[0],1e-9));
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
// Check linearization
JacobianFactor expected(3, eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
}
// Create some values
Values values;
values.insert(3, Rot3());
/* ************************************************************************* */
// Test compose with three arguments
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
if (H1)
*H1 = eye(3);
if (H2)
*H2 = eye(3);
if (H3)
*H3 = eye(3);
return R1 * (R2 * R3);
}
// Check unwhitenedError
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(eye(3), H[0],1e-9));
TEST(BADFactor, composeTernary) {
// Check linearization
JacobianFactor expected(3, eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
}
// Create expression
Rot3_ A(1), B(2), C(3);
Rot3_ ABC(composeThree, A, B, C);
/* ************************************************************************* */
// Test compose with three arguments
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
boost::optional<Matrix3&> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
if (H1)
*H1 = eye(3);
if (H2)
*H2 = eye(3);
if (H3)
*H3 = eye(3);
return R1 * (R2 * R3);
}
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
TEST(BADFactor, composeTernary) {
// Create some values
Values values;
values.insert(1, Rot3());
values.insert(2, Rot3());
values.insert(3, Rot3());
// Create expression
Rot3_ A(1), B(2), C(3);
Rot3_ ABC(composeThree, A, B, C);
// Check unwhitenedError
std::vector<Matrix> H(3);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(3, H.size());
EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(eye(3), H[1],1e-9));
EXPECT( assert_equal(eye(3), H[2],1e-9));
// Create factor
BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
// Check linearization
JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
}
// Create some values
Values values;
values.insert(1, Rot3());
values.insert(2, Rot3());
values.insert(3, Rot3());
/* ************************************************************************* */
// Check unwhitenedError
std::vector<Matrix> H(3);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(3, H.size());
EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(eye(3), H[1],1e-9));
EXPECT( assert_equal(eye(3), H[2],1e-9));
// Check linearization
JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);

13
gtsam_unstable/nonlinear/tests/testExpression.cpp
View File

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

4
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -127,13 +127,13 @@ namespace gtsam {
if(H1 || H2 || H3) {
gtsam::Matrix H0, H02;
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;
*H1 = *H1 * H0;
return reprojectionError.vector();
} else {
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();
}
} catch( CheiralityException& e) {

11
gtsam_unstable/slam/expressions.h
View File

@ -10,6 +10,7 @@
#include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <boost/bind.hpp>
namespace gtsam {
@ -48,6 +49,16 @@ Point2_ project(const Point3_& 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>
Point2_ uncalibrate(const Expression<CAL>& K, const Point2_& xy_hat) {
return Point2_(K, &CAL::uncalibrate, xy_hat);

31
gtsam_unstable/timing/timeCameraExpression.cpp
View File

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

12
gtsam_unstable/timing/timeOneCameraExpression.cpp
View File

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