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

BackprojectFromCamera and some small fixes from Skydio

release/4.3a0
Frank Dellaert 2018-05-11 15:13:14 -07:00
parent cc25ece055
commit ae86bf0271
10 changed files with 348 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/base/cholesky.cpp
View File

@ -144,8 +144,9 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) {
// Check last diagonal element - Eigen does not check it
if (nFrontal >= 2) {
int exp2, exp1;
(void)frexp(R(topleft + nFrontal - 2, topleft + nFrontal - 2), &exp2);
(void)frexp(R(topleft + nFrontal - 1, topleft + nFrontal - 1), &exp1);
// NOTE(gareth): R is already the size of A, so we don't need to add topleft here.
(void)frexp(R(nFrontal - 2, nFrontal - 2), &exp2);
(void)frexp(R(nFrontal - 1, nFrontal - 1), &exp1);
return (exp2 - exp1 < underconstrainedExponentDifference);
} else if (nFrontal == 1) {
int exp1;

172
gtsam/base/numericalDerivative.h
View File

@ -298,7 +298,7 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*
/**
* Compute numerical derivative in argument 1 of 4-argument function
* @param h ternary function yielding m-vector
* @param h quartic function yielding m-vector
* @param x1 n-dimensional first argument value
* @param x2 second argument value
* @param x3 third argument value
@ -317,9 +317,15 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4), x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative41<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
}
/**
* Compute numerical derivative in argument 2 of 4-argument function
* @param h ternary function yielding m-vector
* @param h quartic function yielding m-vector
* @param x1 first argument value
* @param x2 n-dimensional second argument value
* @param x3 third argument value
@ -338,9 +344,15 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4), x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative42<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
}
/**
* Compute numerical derivative in argument 3 of 4-argument function
* @param h ternary function yielding m-vector
* @param h quartic function yielding m-vector
* @param x1 first argument value
* @param x2 second argument value
* @param x3 n-dimensional third argument value
@ -359,9 +371,15 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4), x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative43<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
}
/**
* Compute numerical derivative in argument 4 of 4-argument function
* @param h ternary function yielding m-vector
* @param h quartic function yielding m-vector
* @param x1 first argument value
* @param x2 second argument value
* @param x3 third argument value
@ -380,6 +398,152 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1), x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative44<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
}
/**
* Compute numerical derivative in argument 1 of 5-argument function
* @param h quintic function yielding m-vector
* @param x1 n-dimensional first argument value
* @param x2 second argument value
* @param x3 third argument value
* @param x4 fourth argument value
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4, x5), x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative51<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
}
/**
* Compute numerical derivative in argument 2 of 5-argument function
* @param h quintic function yielding m-vector
* @param x1 n-dimensional first argument value
* @param x2 second argument value
* @param x3 third argument value
* @param x4 fourth argument value
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4, x5), x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
}
/**
* Compute numerical derivative in argument 3 of 5-argument function
* @param h quintic function yielding m-vector
* @param x1 n-dimensional first argument value
* @param x2 second argument value
* @param x3 third argument value
* @param x4 fourth argument value
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4, x5), x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative53<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
}
/**
* Compute numerical derivative in argument 4 of 5-argument function
* @param h quintic function yielding m-vector
* @param x1 n-dimensional first argument value
* @param x2 second argument value
* @param x3 third argument value
* @param x4 fourth argument value
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1, x5), x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
}
/**
* Compute numerical derivative in argument 5 of 5-argument function
* @param h quintic function yielding m-vector
* @param x1 n-dimensional first argument value
* @param x2 second argument value
* @param x3 third argument value
* @param x4 fourth argument value
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5>(boost::bind(h, x1, x2, x3, x4, _1), x5, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
}
/**
* Compute numerical Hessian matrix. Requires a single-argument Lie->scalar
* function. This is implemented simply as the derivative of the gradient.

4
gtsam/base/serializationTestHelpers.h
View File

@ -24,6 +24,9 @@
#include <gtsam/base/serialization.h>
#include <boost/serialization/serialization.hpp>
// whether to print the serialized text to stdout
const bool verbose = false;
@ -142,4 +145,3 @@ bool equalsDereferencedBinary(const T& input = T()) {
} // \namespace serializationTestHelpers
} // \namespace gtsam

4
gtsam/base/tests/testSerializationBase.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -11,7 +11,7 @@
/**
* @file testSerializationBase.cpp
* @brief
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/

15
gtsam/geometry/CalibratedCamera.cpp
View File

@ -142,8 +142,11 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
Matrix23 Dpc_rot;
Matrix2 Dpc_point;
const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0,
Dpose ? &Dpc_point : 0);
Dpoint ? &Dpc_point : 0);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (pc.unitVector().z() <= 0)
throw CheiralityException();
#endif
// camera to normalized image coordinate
Matrix2 Dpn_pc;
const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0);
@ -161,8 +164,12 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
return pn;
}
/* ************************************************************************* */
Point3 PinholeBase::backproject_from_camera(const Point2& p,
const double depth) {
Point3 PinholeBase::BackprojectFromCamera(const Point2& p,
const double depth, OptionalJacobian<3, 2> Dpoint, OptionalJacobian<3, 1> Ddepth) {
if (Dpoint)
*Dpoint << depth, 0, 0, depth, 0, 0;
if (Ddepth)
*Ddepth << p.x(), p.y(), 1;
return Point3(p.x() * depth, p.y() * depth, depth);
}

29
gtsam/geometry/CalibratedCamera.h
View File

@ -201,7 +201,9 @@ public:
OptionalJacobian<2, 2> Dpoint = boost::none) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
static Point3 backproject_from_camera(const Point2& p, const double depth);
static Point3 BackprojectFromCamera(const Point2& p, const double depth,
OptionalJacobian<3, 2> Dpoint = boost::none,
OptionalJacobian<3, 1> Ddepth = boost::none);
/// @}
/// @name Advanced interface
@ -337,8 +339,28 @@ public:
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& pn, double depth) const {
return pose().transform_from(backproject_from_camera(pn, depth));
Point3 backproject(const Point2& pn, double depth,
OptionalJacobian<3, 6> Dresult_dpose = boost::none,
OptionalJacobian<3, 2> Dresult_dp = boost::none,
OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const {
Matrix32 Dpoint_dpn;
Matrix31 Dpoint_ddepth;
const Point3 point = BackprojectFromCamera(pn, depth,
Dresult_dp ? &Dpoint_dpn : 0,
Dresult_ddepth ? &Dpoint_ddepth : 0);
Matrix33 Dresult_dpoint;
const Point3 result = pose().transform_from(point, Dresult_dpose,
(Dresult_ddepth ||
Dresult_dp) ? &Dresult_dpoint : 0);
if (Dresult_dp)
*Dresult_dp = Dresult_dpoint * Dpoint_dpn;
if (Dresult_ddepth)
*Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
return result;
}
/**
@ -404,4 +426,3 @@ template <typename T>
struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
} // namespace gtsam

59
gtsam/geometry/PinholePose.h
View File

@ -85,25 +85,9 @@ public:
return pn;
}
/** project a point from world coordinate to the image
* @param pw is a point in the world coordinates
*/
Point2 project(const Point3& pw) const {
const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates
return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
}
/** project a point from world coordinate to the image
* @param pw is a point at infinity in the world coordinates
*/
Point2 project(const Unit3& pw) const {
const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame
const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates
return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
}
/** Templated projection of a point (possibly at infinity) from world coordinate to the image
* @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates
* @param pw is a 3D point or a Unit3 (point at infinity) 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
@ -131,25 +115,51 @@ public:
}
/// project a 3D point from world coordinates into the image
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose,
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
/// project a point at infinity from world coordinates into the image
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& p, double depth) const {
const Point2 pn = calibration().calibrate(p);
return pose().transform_from(backproject_from_camera(pn, depth));
Point3 backproject(const Point2& p, double depth,
OptionalJacobian<3, 6> Dresult_dpose = boost::none,
OptionalJacobian<3, 2> Dresult_dp = boost::none,
OptionalJacobian<3, 1> Dresult_ddepth = boost::none,
OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const {
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
Matrix2K Dpn_dcal;
Matrix22 Dpn_dp;
const Point2 pn = calibration().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0,
Dresult_dp ? &Dpn_dp : 0);
Matrix32 Dpoint_dpn;
Matrix31 Dpoint_ddepth;
const Point3 point = BackprojectFromCamera(pn, depth,
(Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0,
Dresult_ddepth ? &Dpoint_ddepth : 0);
Matrix33 Dresult_dpoint;
const Point3 result = pose().transform_from(point, Dresult_dpose,
(Dresult_ddepth ||
Dresult_dp ||
Dresult_dcal) ? &Dresult_dpoint : 0);
if (Dresult_dcal)
*Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal; // (3x3)*(3x2)*(2xDimK)
if (Dresult_dp)
*Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp; // (3x3)*(3x2)*(2x2)
if (Dresult_ddepth)
*Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; // (3x3)*(3x1)
return result;
}
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
Unit3 backprojectPointAtInfinity(const Point2& p) const {
const Point2 pn = calibration().calibrate(p);
@ -302,6 +312,11 @@ public:
Base(v), K_(new CALIBRATION(K)) {
}
// Init from Pose3 and calibration
PinholePose(const Pose3 &pose, const Vector &K) :
Base(pose), K_(new CALIBRATION(K)) {
}
/// @}
/// @name Testable
/// @{

48
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -113,7 +113,7 @@ static Point2 Project2(const Unit3& point) {
return PinholeBase::Project(point);
}
Unit3 pointAtInfinity(0, 0, 1000);
Unit3 pointAtInfinity(0, 0, -1000);
TEST( CalibratedCamera, DProjectInfinity) {
Matrix test1;
CalibratedCamera::Project(pointAtInfinity, test1);
@ -127,6 +127,7 @@ static Point2 project2(const CalibratedCamera& camera, const Point3& point) {
return camera.project(point);
}
TEST( CalibratedCamera, Dproject_point_pose)
{
Matrix Dpose, Dpoint;
@ -173,7 +174,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity)
// Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject_point_pose2_infinity)
{
static const Pose3 pose(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
static const CalibratedCamera camera(pose);
Matrix Dpose, Dpoint;
camera.project2(pointAtInfinity, Dpose, Dpoint);
@ -183,7 +184,48 @@ TEST( CalibratedCamera, Dproject_point_pose2_infinity)
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
static Point3 BackprojectFromCamera(const CalibratedCamera& camera, const Point2& point,
const double& depth) {
return camera.BackprojectFromCamera(point, depth);
}
TEST( CalibratedCamera, DBackprojectFromCamera)
{
static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
static const CalibratedCamera camera(pose);
static const double depth = 5.4;
static const Point2 point(10.1, 50.3);
Matrix Dpoint, Ddepth;
camera.BackprojectFromCamera(point, depth, Dpoint, Ddepth);
Matrix numerical_point = numericalDerivative32(BackprojectFromCamera, camera, point, depth);
Matrix numerical_depth = numericalDerivative33(BackprojectFromCamera, camera, point, depth);
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
CHECK(assert_equal(numerical_depth, Ddepth, 1e-7));
}
static Point3 backproject(const Pose3& pose, const Point2& point, const double& depth) {
return CalibratedCamera(pose).backproject(point, depth);
}
TEST( PinholePose, Dbackproject)
{
Matrix36 Dpose;
Matrix31 Ddepth;
Matrix32 Dpoint;
const Point2 point(-100, 100);
const double depth(10);
static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
static const CalibratedCamera camera(pose);
camera.backproject(point, depth, Dpose, Dpoint, Ddepth);
Matrix expectedDpose = numericalDerivative31(backproject, pose, point, depth);
Matrix expectedDpoint = numericalDerivative32(backproject, pose, point, depth);
Matrix expectedDdepth = numericalDerivative33(backproject, pose,point, depth);
EXPECT(assert_equal(expectedDpose, Dpose, 1e-7));
EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
EXPECT(assert_equal(expectedDdepth, Ddepth, 1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

28
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -191,7 +191,7 @@ static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity,
/* ************************************************************************* */
TEST( PinholePose, DprojectAtInfinity2)
{
Unit3 pointAtInfinity(0,0,1000);
Unit3 pointAtInfinity(0,0,-1000);
Matrix Dpose, Dpoint;
Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
Matrix expectedDcamera = numericalDerivative31(project, pose, pointAtInfinity, K);
@ -201,6 +201,32 @@ TEST( PinholePose, DprojectAtInfinity2)
EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
}
static Point3 backproject(const Pose3& pose, const Cal3_S2& cal,
const Point2& p, const double& depth) {
return Camera(pose, cal.vector()).backproject(p, depth);
}
TEST( PinholePose, Dbackproject)
{
Matrix36 Dpose;
Matrix31 Ddepth;
Matrix32 Dpoint;
Matrix Dcal;
const Point2 point(-100, 100);
const double depth(10);
camera.backproject(point, depth, Dpose, Dpoint, Ddepth, Dcal);
Matrix expectedDpose = numericalDerivative41(backproject, pose, *K, point, depth);
Matrix expectedDcal = numericalDerivative42(backproject, pose, *K, point, depth);
Matrix expectedDpoint = numericalDerivative43(backproject, pose, *K, point, depth);
Matrix expectedDdepth = numericalDerivative44(backproject, pose, *K, point, depth);
EXPECT(assert_equal(expectedDpose, Dpose, 1e-7));
EXPECT(assert_equal(expectedDcal, Dcal, 1e-7));
EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
EXPECT(assert_equal(expectedDdepth, Ddepth, 1e-7));
}
/* ************************************************************************* */
static double range0(const Camera& camera, const Point3& point) {
return camera.range(point);

47
python/handwritten/geometry/PinholeBaseK.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -36,31 +36,38 @@ struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper<PinholeBaseKCa
}
};
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 2, 4)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 1, 4)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, PinholeBaseKCal3_S2::range, 1, 3)
// Function pointers to desambiguate project() calls
Point2 (PinholeBaseKCal3_S2::*project1) (const Point3 &pw) const = &PinholeBaseKCal3_S2::project;
Point2 (PinholeBaseKCal3_S2::*project2) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
Point2 (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
// Function pointers to disambiguate project() calls
Point2 (PinholeBaseKCal3_S2::*project1) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
Point2 (PinholeBaseKCal3_S2::*project2) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
// function pointers to desambiguate range() calls
// function pointers to disambiguate range() calls
double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range;
double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose) const = &PinholeBaseKCal3_S2::range;
double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range;
void exportPinholeBaseK(){
// wrap projectSafe in a function that returns None or a tuple
// TODO(frank): find out how to return an ndarray instead
object project_safe(const PinholeBaseKCal3_S2& camera, const gtsam::Point3& p) {
auto result = camera.projectSafe(p);
if (result.second)
return make_tuple(result.first.x(), result.first.y());
else
return object();
}
void exportPinholeBaseK() {
class_<PinholeBaseKCal3_S2Callback, boost::noncopyable>("PinholeBaseKCal3_S2", no_init)
.def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy<copy_const_reference>())
.def("project", project1)
.def("project", project2, project_overloads())
.def("project", project3, project_overloads())
.def("backproject", &PinholeBaseKCal3_S2::backproject)
.def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity)
.def("range", range1, range_overloads())
.def("range", range2, range_overloads())
.def("range", range3, range_overloads())
;
}
.def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration),
return_value_policy<copy_const_reference>())
.def("project_safe", make_function(project_safe))
.def("project", project1, project_overloads())
.def("project", project2, project_overloads())
.def("backproject", &PinholeBaseKCal3_S2::backproject)
.def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity)
.def("range", range1, range_overloads())
.def("range", range2, range_overloads())
.def("range", range3, range_overloads());
}