Merged in feature/geometry_tidbits (pull request #216)

A set of geometry changes from the FixedValues branch, unrelated to that PR

gtsam/geometry/CameraSet.h

@@ -110,20 +110,16 @@ public:
     std::vector<Z> 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<double, ZDim, N> Ei;
       z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
-      if (Fs)
-        (*Fs)[i] = Fi;
-      if (E)
-        E->block<ZDim, N>(ZDim * i, 0) = Ei;
+      if (Fs) (*Fs)[i] = Fi;
+      if (E) E->block<ZDim, N>(ZDim * i, 0) = Ei;
     }
 
     return z;

gtsam/geometry/PinholeCamera.h

@@ -32,6 +32,14 @@ namespace gtsam {
 template<typename Calibration>
 class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
 
+public:
+
+  /**
+   *  Some classes template on either PinholeCamera or StereoCamera,
+   *  and this typedef informs those classes what "project" returns.
+   */
+  typedef Point2 Measurement;
+
 private:
 
   typedef PinholeBaseK<Calibration> 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<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
   /// @{

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<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) {
 

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

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)

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);
 }