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

resize is slightly more efficient

release/4.3a0
dellaert 2014-10-06 12:54:40 +02:00
parent 0ed96dda33
commit 63d87e6497
2 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/Cal3_S2.cpp
View File

@ -75,10 +75,14 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal)
*Dcal = (Matrix(2, 5) << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0);
if (Dp)
*Dp = (Matrix(2, 2) << fx_, s_, 0.000, fy_);
if (Dcal) {
Dcal->resize(2,5);
*Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
}
if (Dp) {
Dp->resize(2,2);
*Dp << fx_, s_, 0.0, fy_;
}
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}

3
gtsam/geometry/PinholeCamera.h
View File

@ -278,7 +278,8 @@ public:
double d = 1.0 / P.z();
const double u = P.x() * d, v = P.y() * d;
if (Dpoint) {
*Dpoint = (Matrix(2, 3) << d, 0.0, -u * d, 0.0, d, -v * d);
Dpoint->resize(2,3);
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
}
return Point2(u, v);
}