started serious testing: all tests pass for now

gtsam/base/Lie.h

@@ -17,7 +17,7 @@
  * @author Frank Dellaert
  * @author Mike Bosse
  * @author Duy Nguyen Ta
- * @author shteren1
+ * @author Yotam Stern
  */
 
 

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -50,7 +50,7 @@ static Point2 measurement1(323.0, 240.0);
 
 LevenbergMarquardtParams lmParams;
 // Make more verbose like so (in tests):
-// params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
+// lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY;
 
 /* ************************************************************************* */
 TEST( SmartProjectionPoseFactor, Constructor) {

gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h

@@ -167,12 +167,17 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
     return K_all_;
   }
 
+  /// return (for each observation) the key of the pair of poses from which we interpolate
+  const std::vector<std::pair<Key, Key>> world_P_body_key_pairs() const {
+    return world_P_body_key_pairs_;
+  }
+
   /// return the interpolation factors gammas
   const std::vector<double> getGammas() const {
     return gammas_;
   }
 
-  /// return the interpolation factors gammas
+  /// return the extrinsic camera calibration body_P_sensors
   const std::vector<Pose3> body_P_sensors() const {
     return body_P_sensors_;
   }
@@ -192,7 +197,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
       std::cout << " pose2 key: "
           << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
       std::cout << " gamma: " << gammas_[i] << std::endl;
-      K_all_[i]->print("calibration = ");
+      body_P_sensors_[i].print("extrinsic calibration:\n");
+      K_all_[i]->print("intrinsic calibration = ");
     }
     Base::print("", keyFormatter);
   }
@@ -202,8 +208,30 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
     const SmartProjectionPoseFactorRollingShutter<CALIBRATION>* e =
         dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CALIBRATION>*>(&p);
 
+    double keyPairsEqual = true;
+    if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){
+      for(size_t k=0; k< this->world_P_body_key_pairs_.size(); k++){
+        const Key key1own = world_P_body_key_pairs_[k].first;
+        const Key key1e = e->world_P_body_key_pairs()[k].first;
+        const Key key2own = world_P_body_key_pairs_[k].second;
+        const Key key2e = e->world_P_body_key_pairs()[k].second;
+        if ( !(key1own == key1e) || !(key2own == key2e) ){
+          keyPairsEqual = false; break;
+        }
+      }
+    }else{ keyPairsEqual = false; }
+
+    double extrinsicCalibrationEqual = true;
+    if(this->body_P_sensors_.size() == e->body_P_sensors().size()){
+      for(size_t i=0; i< this->body_P_sensors_.size(); i++){
+        if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])){
+          extrinsicCalibrationEqual = false; break;
+        }
+      }
+    }else{ extrinsicCalibrationEqual = false; }
+
     return e && Base::equals(p, tol) && K_all_ == e->calibration()
-        && gammas_ == e->getGammas() && body_P_sensors_ == e->body_P_sensors();
+        && gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual;
   }
 
   /**
@@ -249,8 +277,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
    * and the error vector b. Note that the jacobians are computed for a given point.
    */
   void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b,
-                                             const Values& values) const
+                                             const Values& values) const {
-                                                 override {
     if (!this->result_) {
       throw("computeJacobiansWithTriangulatedPoint");
     } else {  // valid result: compute jacobians
@@ -296,7 +323,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
   /// linearize and return a Hessianfactor that is an approximation of error(p)
   boost::shared_ptr<RegularHessianFactor<DimBlock> > createHessianFactor(
       const Values& values, const double lambda = 0.0, bool diagonalDamping =
-          false) const override {
+          false) const {
 
     // we may have multiple cameras sharing the same extrinsic cals, hence the number
     // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we
@@ -313,29 +340,29 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
       throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: "
                                "measured_.size() inconsistent with input");
 
-    // triangulate 3D point at given linearization point
+//    // triangulate 3D point at given linearization point
-    triangulateSafe(cameras(values));
+//    triangulateSafe(cameras(values));
-
+//
-    if (!this->result_) { // failed: return "empty/zero" Hessian
+//    if (!this->result_) { // failed: return "empty/zero" Hessian
-      for (Matrix& m : Gs)
+//      for (Matrix& m : Gs)
-        m = Matrix::Zero(DimPose, DimPose);
+//        m = Matrix::Zero(DimPose, DimPose);
-      for (Vector& v : gs)
+//      for (Vector& v : gs)
-        v = Vector::Zero(DimPose);
+//        v = Vector::Zero(DimPose);
-      return boost::make_shared < RegularHessianFactor<DimPose>
+//      return boost::make_shared < RegularHessianFactor<DimPose>
-          > ( this->keys_, Gs, gs, 0.0);
+//          > ( this->keys_, Gs, gs, 0.0);
-    }
+//    }
-
+//
-    // compute Jacobian given triangulated 3D Point
+//    // compute Jacobian given triangulated 3D Point
-    FBlocks Fs;
+//    FBlocks Fs;
-    Matrix F, E;
+//    Matrix F, E;
-    Vector b;
+//    Vector b;
-    computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
+//    computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
-
+//
-    // Whiten using noise model
+//    // Whiten using noise model
-    this->noiseModel_->WhitenSystem(E, b);
+//    this->noiseModel_->WhitenSystem(E, b);
-    for (size_t i = 0; i < Fs.size(); i++)
+//    for (size_t i = 0; i < Fs.size(); i++)
-      Fs[i] = this->noiseModel_->Whiten(Fs[i]);
+//      Fs[i] = this->noiseModel_->Whiten(Fs[i]);
-
+//
 //    // build augmented Hessian (with last row/column being the information vector)
 //    Matrix3 P;
 //    This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);

gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp

@@ -10,13 +10,13 @@
  * -------------------------------------------------------------------------- */
 
 /**
- *  @file  testSmartProjectionPoseFactorRollingShutter.cpp
+ *  @file  testSmartProjectionPoseFactorRollingShutterRollingShutter.cpp
- *  @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class
+ *  @brief Unit tests for SmartProjectionPoseFactorRollingShutterRollingShutter Class
  *  @author Luca Carlone
  *  @date   July 2021
  */
 
-#include "gtsam/slam/tests/smartFactorScenarios.h"
+//#include "gtsam/slam/tests/smartFactorScenarios.h"
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/slam/PoseTranslationPrior.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -27,6 +27,7 @@
 #include <boost/assign/std/map.hpp>
 #include <iostream>
 
+using namespace gtsam;
 using namespace boost::assign;
 using namespace std::placeholders;
 
@@ -47,17 +48,15 @@ static Symbol x3('X', 3);
 static Point2 measurement1(323.0, 240.0);
 
 LevenbergMarquardtParams lmParams;
-// Make more verbose like so (in tests):
+typedef SmartProjectionPoseFactorRollingShutter<Cal3_S2> SmartFactorRS;
-// params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
 
-/* ************************************************************************* *
+/* ************************************************************************* */
-TEST( SmartProjectionPoseFactor, Constructor) {
+TEST( SmartProjectionPoseFactorRollingShutter, Constructor) {
-  using namespace vanillaPose;
+  SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
-  SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
 }
 
 /* ************************************************************************* *
-TEST( SmartProjectionPoseFactor, Constructor2) {
+TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) {
   using namespace vanillaPose;
   SmartProjectionParams params;
   params.setRankTolerance(rankTol);
@@ -65,14 +64,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) {
 }
 
 /* ************************************************************************* *
-TEST( SmartProjectionPoseFactor, Constructor3) {
+TEST( SmartProjectionPoseFactorRollingShutter, Constructor3) {
   using namespace vanillaPose;
   SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
   factor1->add(measurement1, x1);
 }
 
 /* ************************************************************************* *
-TEST( SmartProjectionPoseFactor, Constructor4) {
+TEST( SmartProjectionPoseFactorRollingShutter, Constructor4) {
   using namespace vanillaPose;
   SmartProjectionParams params;
   params.setRankTolerance(rankTol);
@@ -81,7 +80,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) {
 }
 
 /* ************************************************************************* *
-TEST( SmartProjectionPoseFactor, params) {
+TEST( SmartProjectionPoseFactorRollingShutter, params) {
   using namespace vanillaPose;
   SmartProjectionParams params;
   double rt = params.getRetriangulationThreshold();
@@ -92,7 +91,7 @@ TEST( SmartProjectionPoseFactor, params) {
 }
 
 /* ************************************************************************* *
-TEST( SmartProjectionPoseFactor, Equals ) {
+TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
   using namespace vanillaPose;
   SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
   factor1->add(measurement1, x1);
@@ -104,7 +103,7 @@ TEST( SmartProjectionPoseFactor, Equals ) {
 }
 
 /* *************************************************************************
-TEST( SmartProjectionPoseFactor, noiseless ) {
+TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) {
 
   using namespace vanillaPose;
 
@@ -162,7 +161,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
 }
 
 /* *************************************************************************
-TEST( SmartProjectionPoseFactor, noisy ) {
+TEST( SmartProjectionPoseFactorRollingShutter, noisy ) {
 
   using namespace vanillaPose;
 
@@ -196,7 +195,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
 }
 
 /* *************************************************************************
-TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
+TEST(SmartProjectionPoseFactorRollingShutter, smartFactorWithSensorBodyTransform) {
   using namespace vanillaPose;
 
   // create arbitrary body_T_sensor (transforms from sensor to body)
@@ -271,7 +270,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
 }
 
 /* *************************************************************************
-TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
+TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) {
 
   using namespace vanillaPose2;
   Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@@ -332,7 +331,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
 }
 
 /* *************************************************************************
-TEST( SmartProjectionPoseFactor, Factors ) {
+TEST( SmartProjectionPoseFactorRollingShutter, Factors ) {
 
   using namespace vanillaPose;
 
@@ -496,7 +495,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
 }
 
 /* *************************************************************************
-TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
+TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) {
 
   using namespace vanillaPose;
 
@@ -550,7 +549,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
 }
 
 /* *************************************************************************
-TEST( SmartProjectionPoseFactor, jacobianSVD ) {
+TEST( SmartProjectionPoseFactorRollingShutter, jacobianSVD ) {
 
   using namespace vanillaPose;
 
@@ -606,7 +605,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
 }
 
 /* *************************************************************************
-TEST( SmartProjectionPoseFactor, landmarkDistance ) {
+TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) {
 
   using namespace vanillaPose;
 
@@ -665,7 +664,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
 }
 
 /* *************************************************************************
-TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
+TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) {
 
   using namespace vanillaPose;
 
@@ -731,7 +730,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
 }
 
 /* *************************************************************************
-TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
+TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) {
 
   using namespace vanillaPose2;
 
@@ -778,7 +777,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
 }
 
 /* *************************************************************************
-TEST( SmartProjectionPoseFactor, CheckHessian) {
+TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) {
 
   KeyVector views {x1, x2, x3};
 
@@ -786,7 +785,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
 
   // Two slightly different cameras
   Pose3 pose2 = level_pose
-      * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
+ * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
   Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
   Camera cam2(pose2, sharedK);
   Camera cam3(pose3, sharedK);
@@ -860,7 +859,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
 }
 
 /* *************************************************************************
-TEST( SmartProjectionPoseFactor, Hessian ) {
+TEST( SmartProjectionPoseFactorRollingShutter, Hessian ) {
 
   using namespace vanillaPose2;