fixed issues

gtsam_unstable/examples/SmartProjectionFactorTesting.cpp

@@ -203,6 +203,8 @@ int main(int argc, char** argv) {
 
   // Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used
   bool useSmartProjectionFactor = true;
+  bool doTriangulation = true; // we read points initial guess from file or we triangulate
+
   bool useLM = true; 
   bool addNoise = false;
 
@@ -221,8 +223,14 @@ int main(int argc, char** argv) {
 
   // Get home directory and dataset
   string HOME = getenv("HOME");
-  string input_dir = HOME + "/data/SfM/BAL/Ladybug/";
+  string datasetFile = HOME + "/data/SfM/BAL/Ladybug/problem-1031-110968-pre.txt";
-  string datasetName = "problem-1031-110968-pre.txt";
+//  string datasetFile = HOME + "/data/SfM/BAL/Ladybug/problem-1723-156502-pre.txt";
+//  string datasetFile = HOME + "/data/SfM/BAL/final/problem-1936-649673-pre.txt";
+
+//  1936     649673      5213733    problem-1936-649673-pre.txt.bz2
+//  3068     310854      1653812    problem-3068-310854-pre.txt.bz2
+//  4585     1324582     9125125    problem-4585-1324582-pre.txt.bz2
+//  13682    4456117     28987644   problem-13682-4456117-pre.txt.bz2
 
   static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2));
   NonlinearFactorGraph graphSmart, graphProjection;
@@ -234,7 +242,7 @@ int main(int argc, char** argv) {
 
   // Read in kitti dataset
   ifstream fin;
-  fin.open((input_dir+datasetName).c_str());
+  fin.open((datasetFile).c_str());
   if(!fin) {
     cerr << "Could not open dataset" << endl;
     exit(1);
@@ -252,7 +260,6 @@ int main(int argc, char** argv) {
   boost::shared_ptr<Ordering> ordering(new Ordering());
 
 //   std::vector< boost::shared_ptr<Cal3Bundler> > K_cameras; // TODO: uncomment
-  std::vector< boost::shared_ptr<Cal3_S2> > K_cameras;
 
 //  boost::shared_ptr<Cal3Bundler> K(new Cal3Bundler()); // TODO: uncomment
   Cal3_S2::shared_ptr K(new Cal3_S2());
@@ -285,6 +292,8 @@ int main(int argc, char** argv) {
 
   cout << "last measurement: " << r << " " << l << " " << u << " " << v << endl;
 
+  std::vector< boost::shared_ptr<Cal3_S2> > K_cameras;
+
   // create values
   for(unsigned int i = 0; i < totNumPoses; i++){
     // R,t,f,k1 and k2.
@@ -292,6 +301,7 @@ int main(int argc, char** argv) {
 //    boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(f, k1, k2, 0.0, 0.0)); //TODO: uncomment
 //    K_cameras.push_back(Kbundler); //TODO: uncomment
     boost::shared_ptr<Cal3_S2> K_S2(new Cal3_S2(f, f, 0.0, 0.0, 0.0));
+    // cout << "f "<< f << endl;
     K_cameras.push_back(K_S2);
     Vector3 rotVect(rotx,roty,rotz);
     // FORMAT CONVERSION!! R -> R'
@@ -336,7 +346,7 @@ int main(int argc, char** argv) {
 
     if (useSmartProjectionFactor) {
 
-      smartCreator.add(L(l), X(r), Point2(u,v), graphSmart);
+      smartCreator.add(L(l), X(r), Point2(u,v), pixel_sigma, K_cameras.at(r), graphSmart);
       numLandmarks = smartCreator.getNumLandmarks();
 
       // Add initial pose value if pose does not exist
@@ -363,10 +373,8 @@ int main(int argc, char** argv) {
   cout << "---------------------------------------------------------- " << endl;
 
   if (!useSmartProjectionFactor) {
-    bool doTriangulation = false; // we read points initial guess from file
     projectionCreator.update(graphProjection, loadedValues, graphProjectionValues, doTriangulation);
-
+    // graphProjectionValues = loadedValues;
-    graphProjectionValues = loadedValues;
     ordering = projectionCreator.getOrdering();
   }
 
@@ -383,13 +391,16 @@ int main(int argc, char** argv) {
       optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
     else
       optimizeGraphISAM2(graphSmart, graphSmartValues, result);
+
+    cout << "Final reprojection error (smart): " << graphSmart.error(result);
   } else {
     if (useLM)
       optimizeGraphLM(graphProjection, graphProjectionValues, result, ordering);
     else
-      optimizeGraphISAM2(graphSmart, graphSmartValues, result); // ?
+      optimizeGraphISAM2(graphProjection, graphProjectionValues, result); // ?
+
+    cout << "Final reprojection error (standard): " << graphProjection.error(result);
   }
-  // *graphSmartValues = result; // we use optimized solution as initial guess for the next one
 
   optimized = true;
 
@@ -398,6 +409,7 @@ int main(int argc, char** argv) {
   cout << "===================================================" << endl;
   writeValues("./", result);
 
-  // if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
+  if (debug) cout << numLandmarks << " " <<  numPoses << " " << optimized << endl;
+
   exit(0);
 }

gtsam_unstable/geometry/triangulation.h

@@ -73,8 +73,11 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   std::vector<Matrix> projection_matrices;
 
   // construct projection matrices from poses & calibration
-  BOOST_FOREACH(const Pose3& pose, poses)
+  BOOST_FOREACH(const Pose3& pose, poses){
   projection_matrices.push_back( K.K() * sub(pose.inverse().matrix(),0,3,0,4) );
+  // std::cout << "Calibration i \n" << K.K() << std::endl;
+  // std::cout << "rank_tol i \n" << rank_tol << std::endl;
+  }
 
   Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol);
 
@@ -104,6 +107,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   // construct projection matrices from poses & calibration
   for(size_t i = 0; i<poses.size(); i++){
     projection_matrices.push_back( Ks.at(i)->K() * sub(poses.at(i).inverse().matrix(),0,3,0,4) );
+    // std::cout << "2Calibration i \n" << Ks.at(i)->K() << std::endl;
+    // std::cout << "2rank_tol i \n" << rank_tol << std::endl;
   }
 
   Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol);

gtsam_unstable/slam/GenericProjectionFactorsCreator.h

@@ -37,6 +37,7 @@ namespace gtsam {
 
     typedef GenericProjectionFactor<Pose3, Point3, CALIBRATION> ProjectionFactor;
     typedef FastMap<Key, std::vector<boost::shared_ptr<ProjectionFactor> > > ProjectionFactorMap;
+    typedef FastMap<Key, boost::shared_ptr<CALIBRATION> > CalibrationMap;
     typedef FastMap<Key, int> OrderingMap;
 
   public:
@@ -90,6 +91,12 @@ namespace gtsam {
             NonlinearFactorGraph &graph) {
             bool debug = false;
 
+            // Check if landmark exists in mapping
+            typename CalibrationMap::iterator calfit = keyCalibrationMap.find(poseKey);
+            if (calfit == keyCalibrationMap.end()){
+              keyCalibrationMap.insert( std::make_pair(poseKey, K) );
+            }
+
             // Create projection factor
             typename ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(measurement, model, poseKey, landmarkKey, K));
 
@@ -167,6 +174,10 @@ namespace gtsam {
       else
         std::cout << "Reading initial guess for 3D points from file" << std::endl;
 
+      double rankTolerance=1;
+
+      std::cout << "rankTolerance " << rankTolerance << std::endl;
+
       std::vector<boost::shared_ptr<ProjectionFactor> > projectionFactorVector;
       typename std::vector<boost::shared_ptr<ProjectionFactor> >::iterator vfit;
       Point3 point;
@@ -188,6 +199,7 @@ namespace gtsam {
 
         std::vector<Pose3> cameraPoses;
         std::vector<Point2> measured;
+        typename std::vector< boost::shared_ptr<CALIBRATION> > Ks;
 
         // Iterate through projection factors
         for (vfit = projectionFactorVector.begin(); vfit != projectionFactorVector.end(); vfit++) { // for each factors connected to the landmark
@@ -196,8 +208,16 @@ namespace gtsam {
           if (debug) std::cout << "ProjectionFactor: " << std::endl;
           if (debug) (*vfit)->print("ProjectionFactor");
 
-          // Iterate through poses
+          // Iterate through poses // find calibration
-          cameraPoses.push_back( loadedValues->at<Pose3>((*vfit)->key1() ) ); // get poses connected to the landmark
+          Key poseKey = (*vfit)->key1();
+          typename CalibrationMap::iterator calfit = keyCalibrationMap.find(poseKey);
+          if (calfit == keyCalibrationMap.end()){ // the pose is not there
+            std::cout << "ProjectionFactor: " << std::endl;
+          }else{
+            Ks.push_back(calfit->second);
+          }
+
+          cameraPoses.push_back( loadedValues->at<Pose3>(poseKey) ); // get poses connected to the landmark
           measured.push_back( (*vfit)->measured() ); // get measurements of the landmark
         }
 
@@ -205,7 +225,7 @@ namespace gtsam {
         if (doTriangualatePoints){
           // std::cout << "Triangulating points " << std::endl;
           try {
-            point = triangulatePoint3(cameraPoses, measured, *K_);
+            point = triangulatePoint3(cameraPoses, measured, Ks, rankTolerance);
             if (debug) std::cout << "Triangulation succeeded: " << point << std::endl;
           } catch( TriangulationUnderconstrainedException& e) {
             if (debug) std::cout << "Triangulation failed because of unconstrained exception" << std::endl;
@@ -272,6 +292,7 @@ namespace gtsam {
     std::vector<Key> cameraPoseKeys;
     std::vector<Key> landmarkKeys;
     ProjectionFactorMap projectionFactors;
+    CalibrationMap keyCalibrationMap;
     boost::shared_ptr<Ordering> ordering;
     // orderingMethod: 0 - COLAMD, 1 - landmark first, then COLAMD on poses (constrained ordering)
     int orderingMethod;

gtsam_unstable/slam/SmartProjectionFactorsCreator.h

@@ -89,12 +89,14 @@ namespace gtsam {
     void add(Key landmarkKey, Key poseKey,
         Point2 measurement,
         const SharedNoiseModel& model,
-        const boost::shared_ptr<CALIBRATION>& K,
+        const boost::shared_ptr<CALIBRATION>& Ki,
         NonlinearFactorGraph &graph) {
 
       std::vector<Key> views;
       std::vector<Point2> measurements;
 
+      // std::cout << "matrix : " << K->K() << std::endl;
+
       bool debug = false;
 
       // Check if landmark exists in mapping
@@ -102,24 +104,31 @@ namespace gtsam {
       typename SmartFactorMap::iterator fit = smartFactors.find(landmarkKey);
       if (fsit != smartFactorStates.end() && fit != smartFactors.end()) {
         if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
-
+        // (*fit).second->print("before: ");
         // Add measurement to smart factor
-        (*fit).second->add(measurement, poseKey, model, K);
+        (*fit).second->add(measurement, poseKey, model, Ki);
+        // (*fit).second->print("after: ");
         totalNumMeasurements++;
         if (debug) (*fit).second->print();
 
       } else {
 
-        if (debug) fprintf(stderr,"New landmark (%d,%d)\n", fsit != smartFactorStates.end(), fit != smartFactors.end());
+        if (debug) std::cout <<"landmark " << DefaultKeyFormatter(landmarkKey) << "pose " << DefaultKeyFormatter(poseKey) << std::endl;
+
+        // if (debug) fprintf(stderr,"landmarkKey %d poseKey %d measurement\n", landmarkKey, fit != smartFactors.end());
 
         // This is a new landmark, create a new factor and add to mapping
         boost::shared_ptr<SmartProjectionHessianFactorState> smartFactorState(new SmartProjectionHessianFactorState());
         boost::shared_ptr<SmartFactor> smartFactor(new SmartFactor(rankTolerance_, linearizationThreshold_));
-        smartFactor->add(measurement, poseKey, model, K);
+        smartFactor->add(measurement, poseKey, model, Ki);
+        // smartFactor->print("created: ");
+        // smartFactor->print(" ");
         smartFactorStates.insert( std::make_pair(landmarkKey, smartFactorState) );
         smartFactors.insert( std::make_pair(landmarkKey, smartFactor) );
         graph.push_back(smartFactor);
 
+        if (debug) std::cout <<" graph size " << graph.size() << std::endl;
+
         numLandmarks++;
         totalNumMeasurements++;
 
@@ -144,6 +153,7 @@ namespace gtsam {
     SmartFactorMap smartFactors;
 
     unsigned int totalNumMeasurements;
+    //landmarkKeys.push_back( L(l) );
     unsigned int numLandmarks;
 
   };

gtsam_unstable/slam/SmartProjectionHessianFactor.h

@@ -40,7 +40,7 @@ namespace gtsam {
   // default threshold for rank deficient triangulation
   static double defaultRankTolerance = 1; // this value may be scenario-dependent and has to be larger in  presence of larger noise
   // if set to true will use the rotation-only version for degenerate cases
-  static bool manageDegeneracy = true;
+  static bool manageDegeneracy = false;
 
   /**
    * Structure for storing some state memory, used to speed up optimization
@@ -250,6 +250,10 @@ namespace gtsam {
       BOOST_FOREACH(const SharedNoiseModel& noise_i, noise_) {
         noise_i->print("noise model = ");
       }
+      BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, K_all_) {
+        K->print("calibration = ");
+      }
+
       if(this->body_P_sensor_){
         this->body_P_sensor_->print("  sensor pose in body frame: ");
       }
@@ -318,6 +322,7 @@ namespace gtsam {
       if (retriangulate) {
         // We triangulate the 3D position of the landmark
         try {
+          std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
           state_->point = triangulatePoint3(cameraPoses, measured_, K_all_, rankTolerance);
           state_->degenerate = false;
           state_->cheiralityException = false;
@@ -521,6 +526,7 @@ namespace gtsam {
         }
 
         if(state_->degenerate){
+          // return 0.0; // TODO: this maybe should be zero?
           for(size_t i = 0; i < measured_.size(); i++) {
             Pose3 pose = cameraPoses.at(i);
             PinholeCamera<CALIBRATION> camera(pose, *K_all_.at(i));