added function to create initial values from db

gtsam/slam/dataset.cpp

@@ -720,4 +720,22 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
   return writeBAL(filename, data);
 }
 
+Values initialCamerasEstimate(const SfM_data& db) {
+  Values initial;
+  size_t i = 0; // NO POINTS:  j = 0;
+  BOOST_FOREACH(const SfM_Camera& camera, db.cameras)
+  initial.insert(i++, camera);
+  return initial;
+}
+
+Values initialCamerasAndPointsEstimate(const SfM_data& db) {
+  Values initial;
+  size_t i = 0, j = 0;
+  BOOST_FOREACH(const SfM_Camera& camera, db.cameras)
+  initial.insert((i++), camera);
+  BOOST_FOREACH(const SfM_Track& track, db.tracks)
+  initial.insert(P(j++), track.p);
+  return initial;
+}
+
 } // \namespace gtsam

gtsam/slam/dataset.h

@@ -175,4 +175,19 @@ GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
  */
 GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
 
+/**
+ * @brief This function creates initial values for cameras from db
+ * @param SfM_data
+ * @return Values
+ */
+GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db);
+
+/**
+ * @brief This function creates initial values for cameras and points from db
+ * @param SfM_data
+ * @return Values
+ */
+GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db);
+
+
 } // namespace gtsam