stuck on compile issue

gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h

@@ -18,6 +18,9 @@
 #pragma once
 
 #include <gtsam/slam/SmartProjectionFactor.h>
+#include <gtsam/slam/SmartFactorBase.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/CameraSet.h>
 
 namespace gtsam {
 /**
@@ -58,6 +61,8 @@ PinholePose<CALIBRATION> > {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   typedef PinholePose<CALIBRATION> Camera;
+  // typedef CameraSet<Camera> Cameras;
+
   /// shorthand for base class type
   typedef SmartProjectionFactor<Camera> Base;
 
@@ -289,13 +294,12 @@ PinholePose<CALIBRATION> > {
   }
 
   /// linearize and return a Hessianfactor that is an approximation of error(p)
-  boost::shared_ptr<RegularHessianFactor<DimBlock> > createHessianFactor(
+  boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
       const Values& values, const double lambda = 0.0, bool diagonalDamping =
           false) const {
 
-    // we may have multiple cameras sharing the same extrinsic cals, hence the number
+    // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation),
-    // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we
+    // hence the number of unique keys may be smaller than 2 * nrMeasurements
-    // have a body key and an extrinsic calibration key for each measurement)
     size_t nrUniqueKeys =  this->keys_.size();
     size_t nrNonuniqueKeys = 2*world_P_body_key_pairs_.size();
 
@@ -304,37 +308,38 @@ PinholePose<CALIBRATION> > {
     std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
     std::vector<Vector> gs(nrUniqueKeys);
 
-    if (this->measured_.size() != cameras(values).size())
+    if (this->measured_.size() != this->cameras(values).size())
       throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: "
           "measured_.size() inconsistent with input");
 
-    //    // triangulate 3D point at given linearization point
+    std::cout << "got this far.." << std::endl;
-    //    triangulateSafe(cameras(values));
+
-    //
+    // triangulate 3D point at given linearization point
-    //    if (!this->result_) { // failed: return "empty/zero" Hessian
+    this->triangulateSafe(this->cameras(values));
-    //      for (Matrix& m : Gs)
+
-    //        m = Matrix::Zero(DimPose, DimPose);
+    if (!this->result_) { // failed: return "empty/zero" Hessian
-    //      for (Vector& v : gs)
+      for (Matrix& m : Gs)
-    //        v = Vector::Zero(DimPose);
+        m = Matrix::Zero(DimPose, DimPose);
-    //      return boost::make_shared < RegularHessianFactor<DimPose>
+      for (Vector& v : gs)
-    //          > ( this->keys_, Gs, gs, 0.0);
+        v = Vector::Zero(DimPose);
-    //    }
+      return boost::make_shared < RegularHessianFactor<DimPose> > ( this->keys_, Gs, gs, 0.0);
-    //
+    }
-    //    // compute Jacobian given triangulated 3D Point
+
-    //    FBlocks Fs;
+    // compute Jacobian given triangulated 3D Point
-    //    Matrix F, E;
+    FBlocks Fs;
-    //    Vector b;
+    Matrix F, E;
-    //    computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
+    Vector b;
-    //
+    this->computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
-    //    // Whiten using noise model
+
-    //    this->noiseModel_->WhitenSystem(E, b);
+    // Whiten using noise model
-    //    for (size_t i = 0; i < Fs.size(); i++)
+    this->noiseModel_->WhitenSystem(E, b);
-    //      Fs[i] = this->noiseModel_->Whiten(Fs[i]);
+    for (size_t i = 0; i < Fs.size(); i++)
-    //
+      Fs[i] = this->noiseModel_->Whiten(Fs[i]);
-    //    // build augmented Hessian (with last row/column being the information vector)
+
-    //    Matrix3 P;
+    // build augmented Hessian (with last row/column being the information vector)
-    //    This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
+    Matrix3 P;
-    //
+    Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
+
     //    // marginalize point: note - we reuse the standard SchurComplement function
     //    SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b);
 
@@ -409,6 +414,13 @@ PinholePose<CALIBRATION> > {
     //    }
     //    return boost::make_shared < RegularHessianFactor<DimPose>
     //        > ( this->keys_, augmentedHessianUniqueKeys);
+
+    // TO REMOVE:
+    for (Matrix& m : Gs)
+      m = Matrix::Zero(DimPose, DimPose);
+    for (Vector& v : gs)
+      v = Vector::Zero(DimPose);
+    return boost::make_shared < RegularHessianFactor<DimPose> > ( this->keys_, Gs, gs, 0.0);
   }
 
   /**

gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp

@@ -352,6 +352,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor )
   values.insert(x2, pose_right);
   // initialize third pose with some noise, we expect it to move back to original pose_above
   values.insert(x3, pose_above * noise_pose);
+  EXPECT( // check that the pose is actually noisy
+      assert_equal(
+          Pose3(
+              Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
+                   -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
+                   Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
 
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);