From d26abf3ccfcff95152f68f5bf08f634c351fd457 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 13 Nov 2009 06:18:27 +0000
Subject: [PATCH] addLandmarkConstraint

---
 cpp/VSLAMGraph.cpp | 42 ++++++++++++++++--------------------------
 cpp/VSLAMGraph.h   | 19 ++++++++-----------
 2 files changed, 24 insertions(+), 37 deletions(-)

diff --git a/cpp/VSLAMGraph.cpp b/cpp/VSLAMGraph.cpp
index 196680ca4..45d9ac5f7 100644
--- a/cpp/VSLAMGraph.cpp
+++ b/cpp/VSLAMGraph.cpp
@@ -12,9 +12,11 @@
 #include "VSLAMGraph.h"
 #include "NonlinearFactorGraph-inl.h"
 #include "NonlinearOptimizer-inl.h"
+#include "NonlinearEquality.h"
 
 using namespace std;
-namespace gtsam{
+
+namespace gtsam {
 
 // explicit instantiation so all the code is there and we can link with it
 template class FactorGraph<VSLAMFactor>;
@@ -76,32 +78,20 @@ VSLAMGraph::VSLAMGraph(const std::string& path)
 }
 
 /* ************************************************************************* */
-VSLAMGraph::VSLAMGraph(const std::string& path,
-			 int nrFrames, double sigma,
-			 const gtsam::Cal3_S2 &K)
-{
-  ifstream ifs(path.c_str(), ios::in);
+bool compareLandmark(const std::string& key,
+					const VSLAMConfig& feasible,
+					const VSLAMConfig& input) {
+	int j = atoi(key.substr(1, key.size() - 1).c_str());
+	return feasible.landmarkPoint(j).equals(input.landmarkPoint(j));
+}
 
-  if(ifs) {
-    int cameraFrameNumber, landmarkNumber;
-    double landmarkX, landmarkY, landmarkZ;
-    double u, v;
-    ifs >> cameraFrameNumber >> landmarkNumber >> u >> v >> landmarkX >> landmarkY >> landmarkZ;
-
-    //Store the measurements
-    Vector z(2);
-    z(0)=u;
-    z(1)=v;
-
-    //VSLAMFactor::shared_ptr f1(new VSLAMFactor<VSLAMConfig>::VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K));
-    //factors_.push_back(f1);
-  }
-  else {
-    printf("Unable to load values in %s\n", path.c_str());
-    exit(0);
-  }
-
-  ifs.close();
+/* ************************************************************************* */
+void VSLAMGraph::addLandmarkConstraint(int j, const gtsam::Point3& p) {
+  typedef NonlinearEquality<VSLAMConfig> NLE;
+  VSLAMConfig feasible;
+  feasible.addLandmarkPoint(j,p);
+  boost::shared_ptr<NLE> factor(new NLE(symbol('l',j), feasible, 3, compareLandmark));
+  push_back(factor);
 }
 
 /* ************************************************************************* */
diff --git a/cpp/VSLAMGraph.h b/cpp/VSLAMGraph.h
index 2ce58a266..dd116072f 100644
--- a/cpp/VSLAMGraph.h
+++ b/cpp/VSLAMGraph.h
@@ -17,7 +17,6 @@
 #include "VSLAMFactor.h"
 #include "VSLAMConfig.h"
 
-using namespace std;
 namespace gtsam{
 
 /**
@@ -40,14 +39,6 @@ public:
    */
   VSLAMGraph(const std::string& path);
 
-  /**
-   * Constructor that loads from VO file (not tested)
-   * @param path to the file
-   * @param nrFrames the number of frames to load
-   * @return new factor graph
-   */
-  VSLAMGraph(const std::string& path, int nrFrames, double sigma, const gtsam::Cal3_S2& K);
-
   /**
    * print out graph
    */
@@ -55,11 +46,17 @@ public:
     gtsam::NonlinearFactorGraph<VSLAMConfig>::print(s);
   }
 
-  void load_dumped(const std::string& path);
-
+  // Getters
   int Get_nFrames(){return nFrames;};
   int Get_nFeat_ids(){return feat_ids.size();};
   feat_ids_type* Get_feat_ids_map(){return &feat_ids;};
+
+  /**
+   *  Add a constraint on a landmark (for now, *must* be satisfied in any Config)
+   *  @param j index of landmark
+   *  @param p to which point to constrain it to
+   */
+  void addLandmarkConstraint(int j, const Point3& p = Point3());
 };
 
 } // namespace gtsam