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

reinstated getPose as otherwise expressions can't distinguish overloads

release/4.3a0
dellaert 2015-02-21 13:29:03 +01:00
parent b662c8f644
commit be49d2a118
5 changed files with 8 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/CalibratedCamera.cpp
View File

@ -76,7 +76,7 @@ void PinholeBase::print(const string& s) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
const Pose3& PinholeBase::pose(OptionalJacobian<6, 6> H) const { const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> 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;

2
gtsam/geometry/CalibratedCamera.h
View File

@ -131,7 +131,7 @@ public:
} }
/// return pose, with derivative /// return pose, with derivative
const Pose3& pose(OptionalJacobian<6, 6> H) const; const Pose3& getPose(OptionalJacobian<6, 6> H) const;
/// @} /// @}
/// @name Transformations and measurement functions /// @name Transformations and measurement functions

2
gtsam/geometry/PinholeCamera.h
View File

@ -142,7 +142,7 @@ public:
} }
/// return pose, with derivative /// return pose, with derivative
const Pose3& pose(OptionalJacobian<6, dimension> H) const { const Pose3& getPose(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;

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

@ -61,11 +61,11 @@ TEST( PinholeCamera, constructor)
TEST(PinholeCamera, Pose) { TEST(PinholeCamera, Pose) {
Matrix actualH; Matrix actualH;
EXPECT(assert_equal(pose, camera.pose(actualH))); EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative // Check derivative
boost::function<Pose3(Camera)> f = // boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::pose,_1,boost::none); boost::bind(&Camera::getPose,_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));
} }

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

@ -58,32 +58,18 @@ TEST( PinholePose, constructor)
} }
//****************************************************************************** //******************************************************************************
TEST(PinholePose, Pose) { TEST(PinholeCamera, Pose) {
Matrix actualH; Matrix actualH;
EXPECT(assert_equal(pose, camera.pose(actualH))); EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative // Check derivative
boost::function<Pose3(Camera)> f = // boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::pose,_1,boost::none); boost::bind(&Camera::getPose,_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));
} }
/* ************************************************************************* */
TEST( PinholePose, level2)
{
// Create a level camera, looking in Y-direction
Pose2 pose2(0.4,0.3,M_PI/2.0);
Camera camera = Camera::Level(K, pose2, 0.1);
// expected
Point3 x(1,0,0),y(0,0,-1),z(0,1,0);
Rot3 wRc(x,y,z);
Pose3 expected(wRc,Point3(0.4,0.3,0.1));
EXPECT(assert_equal( camera.pose(), expected));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( PinholePose, lookat) TEST( PinholePose, lookat)
{ {