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

getPose -> pose

release/4.3a0
dellaert 2015-02-21 08:28:00 +01:00
parent eccb0663f3
commit 14ea858e3f
3 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/geometry/PinholeCamera.h
View File

@ -147,17 +147,17 @@ public:
} }
/// return pose /// return pose
inline Pose3& pose() { Pose3& pose() {
return pose_; return pose_;
} }
/// return pose, constant version /// return pose, constant version
inline const Pose3& pose() const { const Pose3& pose() const {
return pose_; return pose_;
} }
/// return pose, with derivative /// return pose, with derivative
inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const { const Pose3& pose(OptionalJacobian<6, dimension> H) const {
if (H) { if (H) {
H->setZero(); H->setZero();
H->block(0, 0, 6, 6) = I_6x6; H->block(0, 0, 6, 6) = I_6x6;
@ -166,12 +166,12 @@ public:
} }
/// return calibration /// return calibration
inline Calibration& calibration() { Calibration& calibration() {
return K_; return K_;
} }
/// return calibration /// return calibration
inline const Calibration& calibration() const { const Calibration& calibration() const {
return K_; return K_;
} }
@ -180,12 +180,12 @@ public:
/// @{ /// @{
/// Manifold dimension /// Manifold dimension
inline size_t dim() const { size_t dim() const {
return dimension; return dimension;
} }
/// Manifold dimension /// Manifold dimension
inline static size_t Dim() { static size_t Dim() {
return dimension; return dimension;
} }
@ -238,7 +238,7 @@ public:
} }
/// Project a point into the image and check depth /// Project a point into the image and check depth
inline std::pair<Point2, bool> projectSafe(const Point3& pw) const { std::pair<Point2, bool> projectSafe(const Point3& pw) const {
const Point3 pc = pose_.transform_to(pw); const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc); const Point2 pn = project_to_camera(pc);
return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
@ -252,7 +252,7 @@ public:
* @param Dpoint is the Jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration * @param Dcal is the Jacobian w.r.t. calibration
*/ */
inline 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, boost::none, OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const { OptionalJacobian<2, DimK> Dcal = boost::none) const {
@ -285,7 +285,7 @@ public:
* @param Dpoint is the Jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration * @param Dcal is the Jacobian w.r.t. calibration
*/ */
inline Point2 projectPointAtInfinity(const Point3& pw, Point2 projectPointAtInfinity(const Point3& pw,
OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const { OptionalJacobian<2, DimK> Dcal = boost::none) const {
@ -356,14 +356,14 @@ public:
} }
/// backproject a 2-dimensional point to a 3-dimensional point at given depth /// backproject a 2-dimensional point to a 3-dimensional point at given depth
inline Point3 backproject(const Point2& p, double depth) const { Point3 backproject(const Point2& p, double depth) const {
const Point2 pn = K_.calibrate(p); const Point2 pn = K_.calibrate(p);
const Point3 pc(pn.x() * depth, pn.y() * depth, depth); const Point3 pc(pn.x() * depth, pn.y() * depth, depth);
return pose_.transform_from(pc); return pose_.transform_from(pc);
} }
/// backproject a 2-dimensional point to a 3-dimensional point at infinity /// backproject a 2-dimensional point to a 3-dimensional point at infinity
inline Point3 backprojectPointAtInfinity(const Point2& p) const { Point3 backprojectPointAtInfinity(const Point2& p) const {
const Point2 pn = K_.calibrate(p); const Point2 pn = K_.calibrate(p);
const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
return pose_.rotation().rotate(pc); return pose_.rotation().rotate(pc);

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

@ -60,11 +60,11 @@ TEST( PinholeCamera, constructor)
TEST(PinholeCamera, Pose) { TEST(PinholeCamera, Pose) {
Matrix actualH; Matrix actualH;
EXPECT(assert_equal(pose, camera.getPose(actualH))); EXPECT(assert_equal(pose, camera.pose(actualH)));
// Check derivative // Check derivative
boost::function<Pose3(Camera)> f = // boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none); boost::bind(&Camera::pose,_1,boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera); Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9)); EXPECT(assert_equal(numericalH, actualH, 1e-9));
} }

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

@ -60,11 +60,11 @@ TEST( PinholePose, constructor)
TEST(PinholePose, Pose) { TEST(PinholePose, Pose) {
Matrix actualH; Matrix actualH;
EXPECT(assert_equal(pose, camera.getPose(actualH))); EXPECT(assert_equal(pose, camera.pose(actualH)));
// Check derivative // Check derivative
boost::function<Pose3(Camera)> f = // boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none); boost::bind(&Camera::pose,_1,boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera); Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9)); EXPECT(assert_equal(numericalH, actualH, 1e-9));
} }