diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index db91fbd45..37d44a0c6 100644 --- a/gtsam/slam/dataset.cpp +++ b/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 diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 0ee5aad9f..070bfc000 100644 --- a/gtsam/slam/dataset.h +++ b/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