From fc7944b42d32ff9ff9f7b8d176b73c03f20b18c7 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 18 Aug 2020 22:28:04 -0400
Subject: [PATCH] Recreated initializeRandomlyAt

---
 gtsam/gtsam.i                 |  2 ++
 gtsam/sfm/ShonanAveraging.cpp | 17 +++++++++++++++--
 gtsam/sfm/ShonanAveraging.h   |  9 +++++++++
 3 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 058f039a2..d67b74812 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -2960,6 +2960,7 @@ class ShonanAveraging2 {
 
   // Advanced API
   gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
+  gtsam::Values initializeRandomlyAt(size_t p) const;
   double costAt(size_t p, const gtsam::Values& values) const;
   pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
   bool checkOptimality(const gtsam::Values& values) const;
@@ -3004,6 +3005,7 @@ class ShonanAveraging3 {
 
   // Advanced API
   gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const;
+  gtsam::Values initializeRandomlyAt(size_t p) const;
   double costAt(size_t p, const gtsam::Values& values) const;
   pair<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
   bool checkOptimality(const gtsam::Values& values) const;
diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index d4f189b0b..2485418cf 100644
--- a/gtsam/sfm/ShonanAveraging.cpp
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -749,10 +749,23 @@ Values ShonanAveraging<d>::initializeRandomly(std::mt19937 &rng) const {
   return initial;
 }
 
+/* ************************************************************************* */
+template <size_t d> Values ShonanAveraging<d>::initializeRandomly() const {
+  return initializeRandomly(kRandomNumberGenerator);
+}
+
 /* ************************************************************************* */
 template <size_t d>
-Values ShonanAveraging<d>::initializeRandomly() const {
-  return ShonanAveraging<d>::initializeRandomly(kRandomNumberGenerator);
+Values ShonanAveraging<d>::initializeRandomlyAt(size_t p,
+                                                std::mt19937 &rng) const {
+  const Values randomRotations = initializeRandomly(rng);
+  return LiftTo<Rot3>(p, randomRotations); // lift to p!
+}
+
+/* ************************************************************************* */
+template <size_t d>
+Values ShonanAveraging<d>::initializeRandomlyAt(size_t p) const {
+  return initializeRandomlyAt(p, kRandomNumberGenerator);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h
index 90c0bda63..ed94329a2 100644
--- a/gtsam/sfm/ShonanAveraging.h
+++ b/gtsam/sfm/ShonanAveraging.h
@@ -235,6 +235,15 @@ public:
    */
   NonlinearFactorGraph buildGraphAt(size_t p) const;
 
+  /**
+   * Create initial Values of type SO(p)
+   * @param p the dimensionality of the rotation manifold 
+   */
+  Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;
+
+  /// Version of initializeRandomlyAt with fixed random seed. 
+  Values initializeRandomlyAt(size_t p) const;
+
   /**
    * Calculate cost for SO(p)
    * Values should be of type SO(p)