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

Fixing uncalibrate (does not yet compile)

release/4.3a0
dellaert 2014-10-06 15:45:26 +02:00
parent f9695f058e
commit e48b38ca21
4 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/Matrix.h
View File

@ -37,8 +37,10 @@ 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;

9
gtsam/geometry/Cal3_S2.cpp
View File

@ -86,6 +86,15 @@ 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::calibrate(const Point2& p) const {
const double u = p.x(), v = p.y();

9
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
@ -151,6 +153,9 @@ public:
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::none, boost::optional<Matrix2&> Dp = boost::none) const;
/**
* convert image coordinates uv to intrinsic coordinates xy
* @param p point in image coordinates
@ -181,12 +186,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

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

@ -383,6 +383,8 @@ 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;