added computePoints functionality in smartFactorsCreator

gtsam/slam/dataset.cpp

@@ -682,7 +682,6 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
   if( valuesPoints.size() != data.number_tracks()){
     cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks()
         <<") and values (#points " << valuesPoints.size() << ")!!" << endl;
-    return false;
   }
   if(valuesPoints.size() + valuesPoses.size() != values.size()){
     cout << "writeBALfromValues write only poses and points values!!" << endl;
@@ -703,8 +702,15 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
 
   for (size_t j = 0; j < data.number_tracks(); j++){ // for each point
     Key pointKey = symbol('l',j);
-    Point3 point = values.at<Point3>(pointKey);
-    data.tracks[j].p = point;
+    if(values.exists(pointKey)){
+      Point3 point = values.at<Point3>(pointKey);
+      data.tracks[j].p = point;
+    }else{
+      data.tracks[j].r = 1.0;
+      data.tracks[j].g = 0.0;
+      data.tracks[j].b = 0.0;
+      data.tracks[j].p = Point3();
+    }
   }
 
   return writeBAL(filename, data);

gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp

@@ -370,6 +370,10 @@ int main(int argc, char** argv) {
   cout << "===================================================" << endl;
 
   // --------------- WRITE OUTPUT TO BAL FILE  ----------------------------------------
+  if(useSmartProjectionFactor){
+    smartCreator.computePoints(result);
+  }
+
   cout << "- writing results to (BAL) file... " << endl;
   std::size_t stringCut1 = datasetFile.rfind("/");
   std::size_t stringCut2 = datasetFile.rfind(".txt");

gtsam_unstable/slam/SmartProjectionFactorsCreator.h

@@ -43,6 +43,22 @@ namespace gtsam {
           linearizationThreshold_(linThreshold), body_P_sensor_(body_P_sensor),
           totalNumMeasurements(0), numLandmarks(0) {};
 
+    void computePoints(Values& values) {
+
+      typename SmartFactorMap::iterator fit;
+      // Check if landmark exists in mapping
+      for(fit = smartFactors.begin(); fit != smartFactors.end(); fit++) {
+        Key pointKey = (*fit).first;
+        Point3 currentPoint;
+        if((*fit).second->point() && !(*fit).second->isDegenerate()){
+          currentPoint = *((*fit).second->point());
+        }else{
+          currentPoint = Point3(); // if we cannot the smartFactor is degenerate
+        }
+        values.insert(pointKey, currentPoint);
+      }
+    }
+
     void add(Key landmarkKey,
         Key poseKey, Point2 measurement, NonlinearFactorGraph &graph) {
 

gtsam_unstable/slam/SmartProjectionHessianFactor.h

@@ -581,6 +581,11 @@ namespace gtsam {
       return K_all_;
     }
 
+    /** return the calibration object */
+    inline bool isDegenerate() const {
+      return (state_->cheiralityException || state_->degenerate);
+    }
+
     /** return verbosity */
     inline bool verboseCheirality() const { return verboseCheirality_; }