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

works on gcc 4.8

release/4.3a0
Natesh Srinivasan 2014-12-05 11:08:13 -05:00
parent 02075b7575
commit f5db91a56f
2 changed files with 15 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/geometry/PinholeCamera.h
View File

@ -397,16 +397,20 @@ public:
double range( double range(
const PinholeCamera<CalibrationB>& camera, // const PinholeCamera<CalibrationB>& camera, //
OptionalJacobian<1, DimC> Dcamera = boost::none, OptionalJacobian<1, DimC> Dcamera = boost::none,
OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother = // OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
boost::optional<Matrix&> Dother =
boost::none) const { boost::none) const {
Matrix16 Dpose_, Dpose2; Matrix16 Dcamera_, Dother_;
double result = pose_.range(camera.pose(), Dcamera ? &Dpose_ : 0, double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0,
Dother ? &Dpose2 : 0); Dother ? &Dother_ : 0);
if (Dcamera) if (Dcamera) {
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero(); Dcamera->resize(1, 6 + DimK);
*Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
}
if (Dother) { if (Dother) {
Dother->resize(1, 6+traits::dimension<CalibrationB>::value);
Dother->setZero(); Dother->setZero();
Dother->block(0, 0, 1, 6) = Dpose2; Dother->block(0, 0, 1, 6) = Dother_;
} }
return result; return result;
} }

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

@ -229,9 +229,8 @@ static double range0(const Camera& camera, const Point3& point) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
typedef Eigen::Matrix<double,1,11> Matrix1_11;
TEST( PinholeCamera, range0) { TEST( PinholeCamera, range0) {
Matrix1_11 D1; Matrix13 D2; Matrix D1; Matrix D2;
double result = camera.range(point1, D1, D2); double result = camera.range(point1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
@ -248,7 +247,7 @@ static double range1(const Camera& camera, const Pose3& pose) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( PinholeCamera, range1) { TEST( PinholeCamera, range1) {
Matrix1_11 D1; Matrix16 D2; Matrix D1; Matrix D2;
double result = camera.range(pose1, D1, D2); double result = camera.range(pose1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1);
Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1); Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1);
@ -267,7 +266,7 @@ static double range2(const Camera& camera, const Camera2& camera2) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( PinholeCamera, range2) { TEST( PinholeCamera, range2) {
Matrix1_11 D1; Matrix19 D2; Matrix D1; Matrix D2;
double result = camera.range<Cal3Bundler>(camera2, D1, D2); double result = camera.range<Cal3Bundler>(camera2, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2);
Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2); Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2);
@ -284,7 +283,7 @@ static double range3(const Camera& camera, const CalibratedCamera& camera3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( PinholeCamera, range3) { TEST( PinholeCamera, range3) {
Matrix1_11 D1; Matrix16 D2; Matrix D1; Matrix D2;
double result = camera.range(camera3, D1, D2); double result = camera.range(camera3, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3);
Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3); Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);