Merge remote-tracking branch 'origin/feature/BAD_FixedJacobians' into feature/BAD
Conflicts: gtsam_unstable/nonlinear/tests/testBADFactor.cpprelease/4.3a0
commit
d38bcf1805
|
@ -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();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
||||||
/// @{
|
/// @{
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
|
@ -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 ;
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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();
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
|
@ -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 {
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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) ));
|
||||||
|
|
|
@ -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();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
|
@ -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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue