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

Avoiding mallocs and working with fixed blocks where possible, makes a pretty noticeable difference in the inner linearize loop

release/4.3a0
dellaert 2014-03-01 18:04:16 -05:00
parent 6b215ea8d9
commit e4edaf359c
4 changed files with 115 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/geometry/Cal3Bundler.cpp
View File

@ -78,17 +78,16 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
// Derivatives make use of intermediate variables above
if (Dcal) {
double rx = r * x, ry = r * y;
Eigen::Matrix<double, 2, 3> D;
D << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
*Dcal = D;
Dcal->resize(2, 3);
*Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
}
if (Dp) {
const double a = 2. * (k1_ + 2. * k2_ * r);
const double axx = a * x * x, axy = a * x * y, ayy = a * y * y;
Eigen::Matrix<double, 2, 2> D;
D << g + axx, axy, axy, g + ayy;
*Dp = f_ * D;
Dp->resize(2,2);
*Dp << g + axx, axy, axy, g + ayy;
*Dp *= f_;
}
return Point2(u0_ + f_ * u, v0_ + f_ * v);

117
gtsam/geometry/PinholeCamera.h
View File

@ -271,6 +271,10 @@ public:
*/
inline static Point2 project_to_camera(const Point3& P,
boost::optional<Matrix&> Dpoint = boost::none) {
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (P.z() <= 0)
throw CheiralityException();
#endif
if (Dpoint) {
double d = 1.0 / P.z(), d2 = d * d;
*Dpoint = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
@ -300,40 +304,25 @@ public:
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (pc.z() <= 0)
throw CheiralityException();
#endif
// Project to normalized image coordinates
const Point2 pn = project_to_camera(pc);
if (Dpose || Dpoint) {
// optimized version of derivatives, see CalibratedCamera.nb
const double z = pc.z(), d = 1.0 / z;
const double u = pn.x(), v = pn.y();
Matrix Dpn_pose(2, 6), Dpn_point(2, 3);
if (Dpose) {
double uv = u * v, uu = u * u, vv = v * v;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
}
if (Dpoint) {
const Matrix R(pose_.rotation().matrix());
Dpn_point << //
R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
/**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
Dpn_point *= d;
}
// uncalibration
Matrix Dpi_pn; // 2*2
Matrix Dpi_pn(2, 2);
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
if (Dpose)
*Dpose = Dpi_pn * Dpn_pose;
if (Dpoint)
*Dpoint = Dpi_pn * Dpn_point;
if (Dpose) {
Dpose->resize(2, 6);
calculateDpose(pn, d, Dpi_pn, *Dpose);
}
if (Dpoint) {
Dpoint->resize(2, 3);
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
}
return pi;
} else
return K_.uncalibrate(pn, Dcal);
@ -391,27 +380,29 @@ public:
boost::optional<Matrix&> Dcamera = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
if (!Dcamera && !Dpoint) {
const Point3 pc = pose_.transform_to(pw);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (pc.z() <= 0)
throw CheiralityException();
#endif
const Point2 pn = project_to_camera(pc);
return K_.uncalibrate(pn);
}
} else {
const double z = pc.z(), d = 1.0 / z;
Matrix Dpose, Dp, Dcal;
const Point2 pi = this->project(pw, Dpose, Dp, Dcal);
if (Dcamera) {
*Dcamera = Matrix(2, this->dim());
Dcamera->leftCols(pose().dim()) = Dpose; // Jacobian wrt pose3
Dcamera->rightCols(calibration().dim()) = Dcal; // Jacobian wrt calib
// uncalibration
Matrix Dcal, Dpi_pn(2, 2);
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
if (Dcamera) {
Dcamera->resize(2, this->dim());
calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>());
Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
}
if (Dpoint) {
Dpoint->resize(2, 3);
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
}
return pi;
}
if (Dpoint)
*Dpoint = Dp;
return pi;
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
@ -516,6 +507,50 @@ public:
private:
/**
* Calculate Jacobian with respect to pose
* @param pn projection in normalized coordinates
* @param d disparity (inverse depth)
* @param Dpi_pn derivative of uncalibrate with respect to pn
* @param Dpose Output argument, can be matrix or block, assumed right size !
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
*/
template<typename Derived>
static void calculateDpose(const Point2& pn, double d, const Matrix& Dpi_pn,
Eigen::MatrixBase<Derived> const & Dpose) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
double uv = u * v, uu = u * u, vv = v * v;
Eigen::Matrix<double, 2, 6> Dpn_pose;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
assert(Dpose.rows()==2 && Dpose.cols()==6);
const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
Dpi_pn.block<2, 2>(0, 0) * Dpn_pose;
}
/**
* Calculate Jacobian with respect to point
* @param pn projection in normalized coordinates
* @param d disparity (inverse depth)
* @param Dpi_pn derivative of uncalibrate with respect to pn
* @param Dpoint Output argument, can be matrix or block, assumed right size !
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
*/
template<typename Derived>
static void calculateDpoint(const Point2& pn, double d, const Matrix& R,
const Matrix& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
Eigen::Matrix<double, 2, 3> Dpn_point;
Dpn_point << //
R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
/**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
Dpn_point *= d;
assert(Dpoint.rows()==2 && Dpoint.cols()==3);
const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
Dpi_pn.block<2, 2>(0, 0) * Dpn_point;
}
/// @}
/// @name Advanced Interface
/// @{

5
gtsam/geometry/Rot3.cpp
View File

@ -101,10 +101,9 @@ Sphere2 Rot3::operator*(const Sphere2& p) const {
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Matrix Rt(transpose());
Point3 q(Rt*p.vector()); // q = Rt*p
Point3 q(transpose()*p.vector()); // q = Rt*p
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
if (H2) *H2 = Rt;
if (H2) *H2 = transpose();
return q;
}

47
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -169,27 +169,27 @@ static Point2 project3(const Pose3& pose, const Point3& point, const Cal3_S2& ca
}
/* ************************************************************************* */
static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) {
Point3 point(point2D.x(), point2D.y(), 1.0);
return Camera(pose,cal).projectPointAtInfinity(point);
}
/* ************************************************************************* */
TEST( PinholeCamera, Dproject_point_pose)
TEST( PinholeCamera, Dproject)
{
Matrix Dpose, Dpoint, Dcal;
Point2 result = camera.project(point1, Dpose, Dpoint, Dcal);
Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K);
Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K);
Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K);
CHECK(assert_equal(result, Point2(-100, 100) ));
CHECK(assert_equal(Dpose, numerical_pose, 1e-7));
CHECK(assert_equal(Dpoint, numerical_point,1e-7));
CHECK(assert_equal(Dcal, numerical_cal,1e-7));
CHECK(assert_equal(Point2(-100, 100), result));
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
CHECK(assert_equal(numerical_cal, Dcal, 1e-7));
}
/* ************************************************************************* */
TEST( PinholeCamera, Dproject_point_pose_Infinity)
static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) {
Point3 point(point2D.x(), point2D.y(), 1.0);
return Camera(pose,cal).projectPointAtInfinity(point);
}
/* ************************************************************************* */
TEST( PinholeCamera, Dproject_Infinity)
{
Matrix Dpose, Dpoint, Dcal;
Point2 point2D(-0.08,-0.08);
@ -198,9 +198,26 @@ TEST( PinholeCamera, Dproject_point_pose_Infinity)
Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K);
Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K);
Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K);
CHECK(assert_equal(Dpose, numerical_pose, 1e-7));
CHECK(assert_equal(Dpoint, numerical_point,1e-7));
CHECK(assert_equal(Dcal, numerical_cal,1e-7));
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
CHECK(assert_equal(numerical_cal, Dcal, 1e-7));
}
/* ************************************************************************* */
static Point2 project4(const Camera& camera, const Point3& point) {
return camera.project2(point);
}
/* ************************************************************************* */
TEST( PinholeCamera, Dproject2)
{
Matrix Dcamera, Dpoint;
Point2 result = camera.project2(point1, Dcamera, Dpoint);
Matrix numerical_camera = numericalDerivative21(project4, camera, point1);
Matrix numerical_point = numericalDerivative22(project4, camera, point1);
CHECK(assert_equal(result, Point2(-100, 100) ));
CHECK(assert_equal(numerical_camera, Dcamera, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */