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

Create, with derivatives

release/4.3a0
dellaert 2015-01-22 00:41:02 +01:00
parent 71ae1e5c0a
commit 2bcc86e2ac
2 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/geometry/PinholeCamera.h
View File

@ -108,6 +108,20 @@ public:
return PinholeCamera(pose3, K);
}
// Create PinholeCamera, with derivatives
static PinholeCamera Create(const Pose3& pose, const Calibration &K,
OptionalJacobian<dimension, 6> H1 = boost::none, //
OptionalJacobian<dimension, DimK> H2 = boost::none) {
typedef Eigen::Matrix<double, DimK, 6> MatrixK6;
if (H1)
*H1 << I_6x6, MatrixK6::Zero();
typedef Eigen::Matrix<double, 6, DimK> Matrix6K;
typedef Eigen::Matrix<double, DimK, DimK> MatrixK;
if (H2)
*H2 << Matrix6K::Zero(), MatrixK::Identity();
return PinholeCamera(pose,K);
}
/// @}
/// @name Advanced Constructors
/// @{

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

@ -55,6 +55,21 @@ TEST( PinholeCamera, constructor)
EXPECT(assert_equal( pose, camera.pose()));
}
//******************************************************************************
TEST(PinholeCamera, Create) {
Matrix actualH1, actualH2;
EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2)));
// Check derivative
boost::function<Camera(Pose3,Cal3_S2)> f = //
boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
}
//******************************************************************************
TEST(PinholeCamera, Pose) {