From 0ceeb4bd47358fc74daed5606519c47244d534b9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Feb 2016 20:11:01 -0800 Subject: [PATCH] A set of geometry changes from the FixedValues branch, unrelated to that PR --- gtsam/geometry/CameraSet.h | 12 ++++------- gtsam/geometry/PinholeCamera.h | 22 +++++++++++++++++++++ gtsam/geometry/tests/testPinholeCamera.cpp | 15 ++++++++++++++ gtsam_unstable/geometry/Event.cpp | 7 +------ gtsam_unstable/geometry/Event.h | 6 ++---- gtsam_unstable/geometry/tests/testEvent.cpp | 6 +++--- 6 files changed, 47 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 3208c6555..899e6227c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -110,20 +110,16 @@ public: std::vector z(m); // Allocate derivatives - if (E) - E->resize(ZDim * m, N); - if (Fs) - Fs->resize(m); + if (E) E->resize(ZDim * m, N); + if (Fs) Fs->resize(m); // Project and fill derivatives for (size_t i = 0; i < m; i++) { MatrixZD Fi; Eigen::Matrix Ei; z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0); - if (Fs) - (*Fs)[i] = Fi; - if (E) - E->block(ZDim * i, 0) = Ei; + if (Fs) (*Fs)[i] = Fi; + if (E) E->block(ZDim * i, 0) = Ei; } return z; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 6177dec95..a5707ce89 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -32,6 +32,14 @@ namespace gtsam { template class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { +public: + + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef Point2 Measurement; + private: typedef PinholeBaseK Base; ///< base class has 3D pose as private member @@ -98,6 +106,20 @@ public: return PinholeCamera(Base::LookatPose(eye, target, upVector), K); } + // Create PinholeCamera, with derivatives + static PinholeCamera Create(const Pose3& pose, const Calibration &K, + OptionalJacobian H1 = boost::none, // + OptionalJacobian H2 = boost::none) { + typedef Eigen::Matrix MatrixK6; + if (H1) + *H1 << I_6x6, MatrixK6::Zero(); + typedef Eigen::Matrix Matrix6K; + typedef Eigen::Matrix MatrixK; + if (H2) + *H2 << Matrix6K::Zero(), MatrixK::Identity(); + return PinholeCamera(pose,K); + } + /// @} /// @name Advanced Constructors /// @{ diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 7293d4235..b2579c0d9 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -57,6 +57,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 f = // + boost::bind(Camera::Create,_1,_2,boost::none,boost::none); + Matrix numericalH1 = numericalDerivative21(f,pose,K); + EXPECT(assert_equal(numericalH1, actualH1, 1e-9)); + Matrix numericalH2 = numericalDerivative22(f,pose,K); + EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); +} + //****************************************************************************** TEST(PinholeCamera, Pose) { diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp index 68d4d60ce..dda2c32e6 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/gtsam_unstable/geometry/Event.cpp @@ -21,9 +21,4 @@ namespace gtsam { -const double Event::Speed = 330; -const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished(); - -} - - +} //\ namespace gtsam diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 3c622924a..dd362c7f4 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -33,10 +33,6 @@ class Event { public: enum { dimension = 4 }; - /// Speed of sound - static const double Speed; - static const Matrix14 JacobianZ; - /// Default Constructor Event() : time_(0) { @@ -57,6 +53,7 @@ public: // TODO we really have to think of a better way to do linear arguments double height(OptionalJacobian<1,4> H = boost::none) const { + static const Matrix14 JacobianZ = (Matrix14() << 0,0,0,1).finished(); if (H) *H = JacobianZ; return location_.z(); } @@ -87,6 +84,7 @@ public: double toa(const Point3& microphone, // OptionalJacobian<1, 4> H1 = boost::none, // OptionalJacobian<1, 3> H2 = boost::none) const { + static const double Speed = 330; Matrix13 D1, D2; double distance = location_.distance(microphone, D1, D2); if (H1) diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index 433ca7e7f..0842e2146 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -45,13 +45,13 @@ TEST( Event, Constructor ) { //***************************************************************************** TEST( Event, Toa1 ) { Event event(0, 1, 0, 0); - double expected = 1 / Event::Speed; + double expected = 1. / 330; EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); } //***************************************************************************** TEST( Event, Toa2 ) { - double expectedTOA = timeOfEvent + 1 / Event::Speed; + double expectedTOA = timeOfEvent + 1. / 330; EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); } @@ -79,7 +79,7 @@ TEST( Event, Expression ) { Values values; values.insert(key, exampleEvent); - double expectedTOA = timeOfEvent + 1 / Event::Speed; + double expectedTOA = timeOfEvent + 1. / 330; EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); }