Cleaned up some of the documentation, and added Python bindings for the SESyncProblem class

david-m-rosen · david-m-rosen · commit 06d0e4e87353 · 2022-02-22T21:02:58.000-05:00
diff --git a/C++/SE-Sync/include/SESync/SESyncProblem.h b/C++/SE-Sync/include/SESync/SESyncProblem.h
@@ -166,7 +166,7 @@ class SESyncProblem {
 
   /// ACCESSORS
 
-  /** Returns the specific formulation of this SE-Sync problem */
+  /** Returns the specific formulation of this problem */
   Formulation formulation() const { return form_; }
 
   /** Returns the type of matrix factorization used to compute the action of the
@@ -185,8 +185,9 @@ class SESyncProblem {
     return reg_Chol_precon_max_cond_;
   }
 
-  /** Returns the number of poses appearing in this problem */
-  size_t num_poses() const { return n_; }
+  /** Returns the number of states (poses or rotations) appearing in this
+   * problem */
+  size_t num_states() const { return n_; }
 
   /** Returns the number of measurements in this problem */
   size_t num_measurements() const { return m_; }
@@ -195,16 +196,13 @@ class SESyncProblem {
    * over which this problem is defined */
   size_t dimension() const { return d_; }
 
-  /** Returns the relaxation rank r of this problem */
+  /** Returns the current relaxation rank r of this problem */
   size_t relaxation_rank() const { return r_; }
 
   /** Returns the oriented incidence matrix A of the underlying measurement
    * graph over which this problem is defined */
   const SparseMatrix &oriented_incidence_matrix() const { return A_; }
 
-  /** Returns the StiefelProduct manifold underlying this SE-Sync problem */
-  const StiefelProduct &Stiefel_product_manifold() const { return SP_; }
-
   /// OPTIMIZATION AND GEOMETRY
 
   /** Given a matrix X, this function computes and returns the orthogonal
@@ -247,78 +245,75 @@ class SESyncProblem {
   Matrix data_matrix_product(const Matrix &Y) const;
 
   /** Given a matrix Y, this function computes and returns F(Y), the value of
-   * the objective evaluated at X */
+   * the objective evaluated at Y */
   Scalar evaluate_objective(const Matrix &Y) const;
 
   /** Given a matrix Y, this function computes and returns nabla F(Y), the
    * *Euclidean* gradient of F at Y. */
   Matrix Euclidean_gradient(const Matrix &Y) const;
 
-  /** Given a matrix Y in the domain D of the SE-Sync optimization problem and
-   * the *Euclidean* gradient nabla F(Y) at Y, this function computes and
-   * returns the *Riemannian* gradient grad F(Y) of F at Y */
+  /** Given a matrix Y in the domain D of the relaxation and the *Euclidean*
+   * gradient nabla F(Y) at Y, this function computes and returns the
+   * *Riemannian* gradient grad F(Y) of F at Y */
   Matrix Riemannian_gradient(const Matrix &Y, const Matrix &nablaF_Y) const;
 
-  /** Given a matrix Y in the domain D of the SE-Sync optimization problem, this
-   * function computes and returns grad F(Y), the *Riemannian* gradient of F
-   * at Y */
+  /** Given a matrix Y in the domain D of the relaxation, this function computes
+   * and returns grad F(Y), the *Riemannian* gradient of F at Y */
   Matrix Riemannian_gradient(const Matrix &Y) const;
 
-  /** Given a matrix Y in the domain D of the SE-Sync optimization problem, the
-   * *Euclidean* gradient nablaF_Y of F at Y, and a tangent vector dotY in
-   * T_D(Y), the tangent space of the domain of the optimization problem at Y,
-   * this function computes and returns Hess F(Y)[dotY], the action of the
+  /** Given a matrix Y in the domain D of the relaxation, the *Euclidean*
+   * gradient nablaF_Y of F at Y, and a tangent vector dotY in T_Y(D), the
+   * tangent space of the domain of the optimization problem at Y, this function
+   * computes and returns Hess F(Y)[dotY], the action of the
    * Riemannian Hessian on dotY */
   Matrix Riemannian_Hessian_vector_product(const Matrix &Y,
                                            const Matrix &nablaF_Y,
                                            const Matrix &dotY) const;
 
-  /** Given a matrix Y in the domain D of the SE-Sync optimization problem, and
-   * a tangent vector dotY in T_D(Y), the tangent space of the domain of the
-   * optimization problem at Y, this function computes and returns Hess
+  /** Given a matrix Y in the domain D of the relaxation and a tangent vector
+   * dotY in T_Y(D), the tangent space of the domain of the optimization problem
+   * at Y, this function computes and returns Hess
    * F(Y)[dotY], the action of the Riemannian Hessian on dotX */
   Matrix Riemannian_Hessian_vector_product(const Matrix &Y,
                                            const Matrix &dotY) const;
 
-  /** Given a matrix Y in the domain D of the SE-Sync optimization problem, and
-   * a tangent vector dotY in T_D(Y), this function applies the selected
-   * preconditioning strategy to dotY */
+  /** Given a matrix Y in the domain D of the relaxation and a tangent vector
+   * dotY in T_Y(D), this function applies the selected preconditioning strategy
+   * to dotY */
   Matrix precondition(const Matrix &Y, const Matrix &dotY) const;
 
-  /** Given a matrix Y in the domain D of the SE-Sync optimization problem and a
-  tangent vector dotY in T_Y(E), the tangent space of Y considered as a generic
-  matrix, this function computes and returns the orthogonal projection of dotY
-  onto T_D(Y), the tangent space of the domain D at Y*/
+  /** Given a matrix Y in the domain D of the relaxation and a tangent vector
+   * dotY in T_Y(E), the tangent space of Y considered as a generic matrix, this
+   * function computes and returns the orthogonal projection of dotY onto
+   * T_D(Y), the tangent space of the domain D at Y*/
   Matrix tangent_space_projection(const Matrix &Y, const Matrix &dotY) const;
 
-  /** Given a matrix Y in the domain D of the SE-Sync optimization problem and a
-   * tangent vector dotY in T_D(Y), this function returns the point Yplus in D
-   * obtained by retracting along dotY */
+  /** Given a matrix Y in the domain D of the relaxation and a tangent vector
+   * dotY in T_D(Y), this function returns the point Yplus in D obtained by
+   * retracting along dotY */
   Matrix retract(const Matrix &Y, const Matrix &dotY) const;
 
-  /** Given a point Y in the domain D of the rank-r relaxation of the SE-Sync
-   * optimization problem, this function computes and returns a matrix
-   * X = [t | R] comprised of translations and rotations for a set of feasible
-   * poses for the original estimation problem obtained by rounding the point Y
+  /** Given a point Y in the domain D of the rank-r relaxation, this function
+   * computes and returns a matrix X = [t | R] composed of translations and
+   * rotations for a set of feasible poses for the original estimation problem
+   * obtained by rounding the point Y
    */
   Matrix round_solution(const Matrix Y) const;
 
-  /** Given a critical point Y of the rank-r relaxation of the SE-Sync
-   * optimization problem, this function computes and returns a d x dn matrix
-   * comprised of d x d block elements of the associated block-diagonal Lagrange
-   * multiplier matrix associated with the orthonormality constraints on the
-   * generalized orientations of the poses (cf. eq. (119) in the SE-Sync tech
-   * report) */
+  /** Given a critical point Y of the rank-r relaxation, this function computes
+   * and returns a d x dn matrix comprised of d x d block elements of the
+   * associated block-diagonal Lagrange multiplier matrix associated with the
+   * orthonormality constraints on the generalized orientations of the poses
+   * (cf. eq. (119) in the SE-Sync tech report) */
   Matrix compute_Lambda_blocks(const Matrix &Y) const;
 
   /** Given the d x dn block matrix containing the diagonal blocks of Lambda,
    * this function computes and returns the matrix Lambda itself */
   SparseMatrix
   compute_Lambda_from_Lambda_blocks(const Matrix &Lambda_blocks) const;
 
-  /** Given a critical point Y of the rank-r relaxation of the SE-Sync
-   * optimization problem, this function computes and returns the corresponding
-   * Lagrange multiplier matrix Lambda */
+  /** Given a critical point Y of the rank-r relaxation, this function computes
+   * and returns the corresponding Lagrange multiplier matrix Lambda */
   SparseMatrix compute_Lambda(const Matrix &Y) const;
 
   /** Given a critical point Y in the domain of the optimization problem, this
diff --git a/C++/SE-Sync/src/PySESync.cpp b/C++/SE-Sync/src/PySESync.cpp
@@ -1,5 +1,6 @@
 #include "SESync/RelativePoseMeasurement.h"
 #include "SESync/SESync.h"
+#include "SESync/SESyncProblem.h"
 #include "SESync/SESync_types.h"
 #include "SESync/SESync_utils.h"
 
@@ -149,6 +150,9 @@ PYBIND11_MODULE(PySESync, m) {
                      &SESync::SESyncOpts::projection_factorization,
                      "Type of cached matrix factorization to use for computing "
                      "orthogonal projections")
+      .def_readwrite("preconditioner", &SESync::SESyncOpts::preconditioner,
+                     "The preconditioning strategy to use in the Riemannian "
+                     "trust-region algorithm")
       .def_readwrite(
           "reg_Chol_precon_max_cond",
           &SESync::SESyncOpts::reg_Cholesky_precon_max_condition_number)
@@ -367,6 +371,109 @@ PYBIND11_MODULE(PySESync, m) {
       "a pair consisting of (1) the orbit distance d_O(X,Y) between them and "
       "(2) the optimal registration G_O in O(d) aligning Y to X");
 
+  /// Bindings for SESyncProblem class
+  py::class_<SESync::SESyncProblem>(
+      m, "SESyncProblem",
+      "This class encapsulates an instance of the rank-restricted Riemannian "
+      "form of the semidefinite relaxation of a special Euclidean.  It "
+      "contains all of the precomputed and cached data matrices necessary to "
+      "describe the problem and run the optimization algorithm, as well as "
+      "functions for performing geometric operations on the underlying "
+      "manifold (tangent space projection and retraction) and evaluating the "
+      "optimization objective and its gradient and Hessian operator. ")
+      .def(py::init<>(), "Default constructor: produces an empyt "
+                         "(uninitialized) problem instance")
+      .def(py::init<SESync::measurements_t, SESync::Formulation,
+                    SESync::ProjectionFactorization, SESync::Preconditioner,
+                    SESync::Scalar>(),
+           py::arg("measurements"),
+           py::arg("formulation") = SESync::Formulation::Simplified,
+           py::arg("projection_factorization") =
+               SESync::ProjectionFactorization::Cholesky,
+           py::arg("preconditioner") =
+               SESync::Preconditioner::IncompleteCholesky,
+           py::arg("reg_chol_precon_max_cond") = 1e6, "Basic constructor.")
+      .def("set_relaxation_rank", &SESync::SESyncProblem::set_relaxation_rank,
+           "Set maximum rank of the rank-restricted semidefinite relaxation.")
+      .def("formulation", &SESync::SESyncProblem::formulation,
+           "Get the specific formulation of this problem")
+      .def(
+          "projection_factorization",
+          &SESync::SESyncProblem::projection_factorization,
+          "Returns the type of matrix factorization used to compute the action "
+          "of the orthogonal projection operator Pi when solving a Simplified "
+          "instance of the special Euclidean synchronization problem")
+      .def("preconditioner", &SESync::SESyncProblem::preconditioner,
+           "Get the preconditioning strategy")
+      .def("num_states", &SESync::SESyncProblem::num_states,
+           "Get the number of states (poses or rotations) appearing in this "
+           "problem")
+      .def("num_measurements", &SESync::SESyncProblem::num_measurements,
+           "Get the number of measurements appearing in this problem")
+      .def("dimension", &SESync::SESyncProblem::dimension,
+           "Get the dimension d of the group SE(d) or SO(d) over which this "
+           "problem is defined")
+      .def("relaxation_rank", &SESync::SESyncProblem::relaxation_rank,
+           "Get the current relaxation rank r of this problem")
+      .def("oriented_incidence_matrix",
+           &SESync::SESyncProblem::oriented_incidence_matrix,
+           "Returns the oriented incidence matrix A of the underlying "
+           "measurement graph over which the problem is defined")
+      .def("Pi_product", &SESync::SESyncProblem::Pi_product,
+           "Given a matrix X, this function computes and returns the "
+           "orthogonal projection Pi*X")
+      .def("Q_product", &SESync::SESyncProblem::Q_product,
+           "Given a matrix X, computes the product Q*X")
+      .def("data_matrix_product", &SESync::SESyncProblem::data_matrix_product,
+           "Given a matrix Y, this function computes and returns the matrix "
+           "product SY, where S is the symmetric matrix parameterizing the "
+           "quadratic objective")
+      .def("evaluate_objective", &SESync::SESyncProblem::evaluate_objective,
+           "Evaluate the objective of the rank-restricted relaxation")
+      .def("Euclidean_gradient", &SESync::SESyncProblem::Euclidean_gradient,
+           "Evaluate the Euclidean gradient of the objective")
+      .def("Riemannian_gradient",
+           static_cast<SESync::Matrix (SESync::SESyncProblem::*)(
+               const SESync::Matrix &) const>(
+               &SESync::SESyncProblem::Riemannian_gradient),
+           "Evaluate the Riemannian gradient of the objective")
+      .def("Riemannian_Hessian_vector_product",
+           static_cast<SESync::Matrix (SESync::SESyncProblem::*)(
+               const SESync::Matrix &, const SESync::Matrix &) const>(
+               &SESync::SESyncProblem::Riemannian_Hessian_vector_product),
+           "Given a matrix Y in the domain D of the relaxation and a tangent "
+           "vector dotY in T_Y(D), this function computes and returns Hess "
+           "F(Y)[dotY], the action of the Riemannian Hessian on dotY")
+      .def("precondition", &SESync::SESyncProblem::precondition,
+           "Given a matrix Y in the domain D of the relaxation and a tangent "
+           "vector dotY in T_D(Y), this function applies the selected "
+           "preconditioning strategy to dotY")
+      .def("tangent_space_projection",
+           &SESync::SESyncProblem::tangent_space_projection,
+           "Given a matrix Y in the domain D of the relaxation and a tangent "
+           "vector dotY of the ambient Eucliean space at Y, this function "
+           "computes and returns the orthogonal projection of dotY onto T_D(Y)")
+      .def("retract", &SESync::SESyncProblem::retract,
+           "Given a matrix Y in the domain of the relaxation and a tangent "
+           "vector dotY in T_Y(D), this function computes the retraction of Y "
+           "along dotY")
+      .def("round_solution", &SESync::SESyncProblem::round_solution,
+           "Given a point Y in the domain D of the rank-r relaxation, this "
+           "function computes and returns a matrix X = [t|R] composed of "
+           "translations and rotations for a set of feasible poses for the "
+           "original estimation problem obtained by rounding the point Y")
+      .def("compute_Lambda", &SESync::SESyncProblem::compute_Lambda,
+           "Given a critical point Y of the rank-r relaxation, this function "
+           "computes and returns the corresponding Lagrange multiplier matrix "
+           "Lambda")
+      .def("chordal_initialization",
+           &SESync::SESyncProblem::chordal_initialization,
+           "This function computes and returns a chordal initialization for "
+           "the rank-restricted semidefinite relaxation")
+      .def("random_sample", &SESync::SESyncProblem::random_sample,
+           "Randomly sample a point in the domain of the rank-restricted "
+           "semidefinite relaxation");
+
   /// Bindings for the main SESync driver
   m.def(
       "SESync",
@@ -381,7 +488,23 @@ PYBIND11_MODULE(PySESync, m) {
       py::arg("measurements"), py::arg("options") = SESync::SESyncOpts(),
       py::arg("Y0") = SESync::Matrix(),
       "Main SE-Sync function:  Given a list of relative pose measurements "
-      "specifying a special Euclidean synchronization problem, this function "
+      "specifying a special Euclidean synchronization problem, this "
+      "function "
       "computes and returns an estimated solution using the SE-Sync "
       "algorithm ");
+
+  m.def(
+      "SESync",
+      [](SESync::SESyncProblem &problem, const SESync::SESyncOpts &options,
+         const SESync::Matrix &Y0) -> SESync::SESyncResult {
+        // Redirect emitted output from (C++) stdout to (Python) sys.stdout
+        py::scoped_ostream_redirect stream(
+            std::cout, py::module::import("sys").attr("stdout"));
+        return SESync::SESync(problem, options, Y0);
+      },
+      py::arg("problem"), py::arg("options") = SESync::SESyncOpts(),
+      py::arg("Y0") = SESync::Matrix(),
+      "Main SE-Sync function:  Given an SESyncProblem instance, this "
+      "function computes and returns an estimated solution using the SE-Sync "
+      "algorithm ");
 }
diff --git a/C++/SE-Sync/src/SESync.cpp b/C++/SE-Sync/src/SESync.cpp
@@ -467,15 +467,15 @@ SESyncResult SESync(SESyncProblem &problem, const SESyncOpts &options,
   SESyncResults.Fxhat =
       (problem.formulation() == Formulation::Simplified
            ? problem.evaluate_objective(SESyncResults.xhat.block(
-                 0, problem.num_poses(), problem.dimension(),
-                 problem.dimension() * problem.num_poses()))
+                 0, problem.num_states(), problem.dimension(),
+                 problem.dimension() * problem.num_states()))
            : problem.evaluate_objective(SESyncResults.xhat));
 
   // Compute the primal optimal SDP solution Lambda and its objective value
   Matrix Lambda_blocks = problem.compute_Lambda_blocks(SESyncResults.Yopt);
 
   SESyncResults.trLambda = 0;
-  for (size_t i = 0; i < problem.num_poses(); i++)
+  for (size_t i = 0; i < problem.num_states(); i++)
     SESyncResults.trLambda +=
         Lambda_blocks
             .block(0, i * problem.dimension(), problem.dimension(),
diff --git a/C++/SE-Sync/src/SESyncProblem.cpp b/C++/SE-Sync/src/SESyncProblem.cpp
@@ -533,12 +533,12 @@ SESyncProblem::SMinusLambdaProdFunctor::SMinusLambdaProdFunctor(
 
   if ((problem_->formulation() == Formulation::Simplified) ||
       (problem_->formulation() == Formulation::SOSync)) {
-    rows_ = problem_->dimension() * problem_->num_poses();
-    cols_ = problem_->dimension() * problem_->num_poses();
+    rows_ = problem_->dimension() * problem_->num_states();
+    cols_ = problem_->dimension() * problem_->num_states();
   } else // mode == Explicit
   {
-    rows_ = (problem_->dimension() + 1) * problem_->num_poses();
-    cols_ = (problem_->dimension() + 1) * problem_->num_poses();
+    rows_ = (problem_->dimension() + 1) * problem_->num_states();
+    cols_ = (problem_->dimension() + 1) * problem_->num_states();
   }
 
   // Compute and cache this on construction
@@ -557,10 +557,10 @@ void SESyncProblem::SMinusLambdaProdFunctor::perform_op(Scalar *x,
   size_t offset = (((problem_->formulation() == Formulation::Simplified) ||
                     (problem_->formulation() == Formulation::SOSync))
                        ? 0
-                       : problem_->num_poses());
+                       : problem_->num_states());
 
 #pragma omp parallel for
-  for (size_t i = 0; i < problem_->num_poses(); ++i)
+  for (size_t i = 0; i < problem_->num_states(); ++i)
     Y.segment(offset + i * dim_, dim_) -=
         Lambda_blocks_.block(0, i * dim_, dim_, dim_) *
         X.segment(offset + i * dim_, dim_);
