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

Better project2 for Point3 and (new) Unit3

release/4.3a0
dellaert 2015-03-05 22:16:47 -08:00
parent 5cc4513ddb
commit 6f36bbf456
2 changed files with 32 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
gtsam/geometry/PinholeCamera.h
View File

@ -200,57 +200,33 @@ public:
typedef Eigen::Matrix<double, 2, DimK> Matrix2K; typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
/** project a point from world coordinate to the image /** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in world coordinates * @param pw is a point in the world coordinate
* @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
*/ */
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera =
OptionalJacobian<2, 3> Dpoint = boost::none, boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
OptionalJacobian<2, DimK> Dcal = boost::none) const { // We just call 3-derivative version in Base
Matrix26 Dpose;
// project to normalized coordinates Eigen::Matrix<double, 2, DimK> Dcal;
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint,
Dcamera ? &Dcal : 0);
// uncalibrate to pixel coordinates if (Dcamera)
Matrix2 Dpi_pn; *Dcamera << Dpose, Dcal;
const Point2 pi = calibration().uncalibrate(pn, Dcal,
Dpose || Dpoint ? &Dpi_pn : 0);
// If needed, apply chain rule
if (Dpose)
*Dpose = Dpi_pn * *Dpose;
if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint;
return pi; return pi;
} }
/** project a point from world coordinate to the image, fixed Jacobians /** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate * @param pw is a point in the world coordinate
*/ */
Point2 project2( Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
const Point3& pw, // boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
OptionalJacobian<2, dimension> Dcamera = boost::none, // We just call 3-derivative version in Base
OptionalJacobian<2, 3> Dpoint = boost::none) const {
// project to normalized coordinates
Matrix26 Dpose; Matrix26 Dpose;
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); Eigen::Matrix<double, 2, DimK> Dcal;
Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint,
// uncalibrate to pixel coordinates Dcamera ? &Dcal : 0);
Matrix2K Dcal;
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0,
Dcamera || Dpoint ? &Dpi_pn : 0);
// If needed, calculate derivatives
if (Dcamera) if (Dcamera)
*Dcamera << Dpi_pn * Dpose, Dcal; *Dcamera << Dpose, Dcal;
if (Dpoint)
*Dpoint = Dpi_pn * (*Dpoint);
return pi; return pi;
} }

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

@ -45,10 +45,10 @@ static const Point3 point2(-0.08, 0.08, 0.0);
static const Point3 point3( 0.08, 0.08, 0.0); static const Point3 point3( 0.08, 0.08, 0.0);
static const Point3 point4( 0.08,-0.08, 0.0); static const Point3 point4( 0.08,-0.08, 0.0);
static const Point3 point1_inf(-0.16,-0.16, -1.0); static const Unit3 point1_inf(-0.16,-0.16, -1.0);
static const Point3 point2_inf(-0.16, 0.16, -1.0); static const Unit3 point2_inf(-0.16, 0.16, -1.0);
static const Point3 point3_inf( 0.16, 0.16, -1.0); static const Unit3 point3_inf( 0.16, 0.16, -1.0);
static const Point3 point4_inf( 0.16,-0.16, -1.0); static const Unit3 point4_inf( 0.16,-0.16, -1.0);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( PinholeCamera, constructor) TEST( PinholeCamera, constructor)
@ -154,9 +154,9 @@ TEST( PinholeCamera, backprojectInfinity2)
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K); Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backprojectPointAtInfinity(Point2()); Unit3 actual = camera.backprojectPointAtInfinity(Point2());
Point3 expected(0., 1., 0.); Unit3 expected(0., 1., 0.);
Point2 x = camera.projectPointAtInfinity(expected); Point2 x = camera.project(expected);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x)); EXPECT(assert_equal(Point2(), x));
@ -169,9 +169,9 @@ TEST( PinholeCamera, backprojectInfinity3)
Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity
Camera camera(Pose3(rot, origin), K); Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backprojectPointAtInfinity(Point2()); Unit3 actual = camera.backprojectPointAtInfinity(Point2());
Point3 expected(0., 0., 1.); Unit3 expected(0., 0., 1.);
Point2 x = camera.projectPointAtInfinity(expected); Point2 x = camera.project(expected);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x)); EXPECT(assert_equal(Point2(), x));
@ -197,17 +197,17 @@ TEST( PinholeCamera, Dproject)
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) { static Point2 projectInfinity3(const Pose3& pose, const Unit3& point3D, const Cal3_S2& cal) {
return Camera(pose,cal).projectPointAtInfinity(point3D); return Camera(pose,cal).project(point3D);
} }
TEST( PinholeCamera, Dproject_Infinity) TEST( PinholeCamera, Dproject_Infinity)
{ {
Matrix Dpose, Dpoint, Dcal; Matrix Dpose, Dpoint, Dcal;
Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 Unit3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1
// test Projection // test Projection
Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); Point2 actual = camera.project(point3D, Dpose, Dpoint, Dcal);
Point2 expected(-5.0, 5.0); Point2 expected(-5.0, 5.0);
EXPECT(assert_equal(actual, expected, 1e-7)); EXPECT(assert_equal(actual, expected, 1e-7));