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

replace Eigen matrix type by gtsam matrix type

release/4.3a0
Jing Dong 2014-11-08 16:09:51 -05:00
parent ad88d4df57
commit 80b7fdd932
4 changed files with 19 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Cal3DS2.cpp
View File

@ -50,8 +50,8 @@ Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
/* ************************************************************************* */
Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Eigen::Matrix<double, 2, 9> H1f;
Eigen::Matrix<double, 2, 2> H2f;
Matrix29 H1f;
Matrix2 H2f;
Point2 u = Base::uncalibrate(p,H1f,H2f);
if (H1)
*H1 = H1f;

22
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -53,23 +53,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& 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;
@ -111,7 +111,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
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
@ -163,7 +163,7 @@ Matrix Cal3DS2_Base::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);
}
@ -178,7 +178,7 @@ Matrix Cal3DS2_Base::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);
}

4
gtsam/geometry/Cal3DS2_Base.h
View File

@ -115,8 +115,8 @@ public:
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Eigen::Matrix<double, 2, 9>&> Dcal = boost::none,
boost::optional<Eigen::Matrix<double, 2, 2>&> Dp = boost::none) const ;
boost::optional<Matrix29&> Dcal = boost::none,
boost::optional<Matrix2&> Dp = boost::none) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;

9
gtsam/geometry/Cal3Unified.cpp
View File

@ -71,17 +71,16 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
// Part2: project NPlane point to pixel plane: use Cal3DS2
Point2 m(x,y);
Eigen::Matrix<double, 2, 9> H1base;
Eigen::Matrix<double, 2, 2> H2base; // jacobians from Base class
Matrix29 H1base;
Matrix2 H2base; // jacobians from Base class
Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
// Inlined derivative for calibration
if (H1) {
// part1
Eigen::Matrix<double, 2, 1> DU;
Vector2 DU, DDS2U;
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
-ys * sqrt_nx * xi_sqrt_nx2;
Eigen::Matrix<double, 2, 1> DDS2U;
DDS2U = H2base * DU;
//*H1 = collect(2, &H1base, &DDS2U);
@ -93,7 +92,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
// part1
const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
const double mid = -(xi * xs*ys) * denom;
Eigen::Matrix<double, 2, 2> DU;
Matrix2 DU;
DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;