Optimizing Your Own Types

Python and C++ code of this example can be found at optimize_custom_type.py and optimize_custom_type.cpp respectively.

---

miniSAM already has build-in support for optimizing various commonly used manifold types in C++ and Python, including Eigen::Vector types in C++, numpy.array in Python, and Lie groups \(SO(2)\), \(SE(2)\), \(SO(3)\), \(SE(3)\) and \(Sim(3)\), which are commonly used in SLAM and robotics problems, provided by Sophus library in both C++ and Python.

In this page we see how to customize manifold properties of any C++ or Python class for miniSAM. The example is given by defining a miniSAM-optimizable 2D vector space \(\mathbb{R}^2\) type.

Python example

In Python this is done by defining manifold-related member functions, including

• dim(self), function returns local manifold dimensionality. For \(\mathbb{R}^2\) the dimensionality is just 2.
• local(self, other), function returns local coordinate of other defined at origin self. For \(\mathbb{R}^2\) since the local manifold is itself, this function returns other - self.
• retract(self, vec), given local coordinate vec defined at origin self, this function returns point projected on manifold of vec. For \(\mathbb{R}^2\) since the local manifold is itself, this function returns self + vec.
• __repr__(self), function print self to a string.

The example code:

# 2D point optimizable in miniSAM
class Point2D(object):
    # constructor
    def __init__(self, x, y):
        self.x_ = float(x)
        self.y_ = float(y)

    # print function
    def __repr__(self):
        return 'custom 2D point [' + str(self.x_) + ', ' + str(self.y_) + ']\''

    # local coordinate dimension
    def dim(self):
        return 2

    # map manifold point other to local coordinate
    def local(self, other):
        return np.array([other.x_ - self.x_, other.y_ - self.y_], dtype=np.float)

    # apply changes in local coordinate to manifold, \oplus operator
    def retract(self, vec):
        return Point2D(self.x_ + vec[0], self.y_ + vec[1])

C++ example

In C++ we use a non-intrusive technique called traits, which is a specialization of template minisam::traits<> for the type we are adding manifold properties.

Using traits to define manifold properties has two major advantages:

• Optimizing a class without modifying it, or even without knowing details of implementation (e.g. optimization support for third-party C/C++ types).
• Making optimizing primitive types (like double) possible.

The specialization of template minisam::traits<> needs following typedefs/static functions defined.

• typedef manifold_tag, use manifold_tag for regular manifold types and lie_group_tag for a Lie group types.
• typedef TangentVector, fixed or dynamic size Eigen::Vector which has size of manifold dimensionality.
• static Dim(), function returns local manifold dimensionality.
• static Local(), function returns local coordinate of input defined at origin.
• static Retract(), given local coordinate input defined at origin, this function returns point projected on manifold.
• static Print(), function to print to std::ostream.

Given a C++ 2D vector space class Point2D

class Point2D {
 private:
  double x_, y_;

 public:
  // ctor
  Point2D(double x, double y) : x_(x), y_(y) {}

  // access
  double x() const { return x_; }
  double y() const { return y_; }
};

The minisam::traits<Point2D> specialization is defined by

// speialization of minisam::traits<Point2D>
namespace minisam {
template <>
struct traits<Point2D> {
  // minisam type category tag, used for static checking
  // must be manifold_tag or lie_group_tag to be optimizable
  typedef manifold_tag type_category;

  // print
  static void Print(const Point2D& m, std::ostream& out = std::cout) {
    out << "custom 2D point [" << m.x() << ", " << m.y() << "]'";
  }

  // tangent vector type defs
  typedef Eigen::Matrix<double, 2, 1> TangentVector;

  // local coordinate dimension
  static constexpr size_t Dim(const Point2D&) { return 2; }

  // map manifold point s to local coordinate
  static TangentVector Local(const Point2D& origin, const Point2D& s) {
    return Eigen::Matrix<double, 2, 1>(s.x() - origin.x(), s.y() - origin.y());
  }

  // apply changes in local coordinate to manifold, \oplus operator
  static Point2D Retract(const Point2D& origin, const TangentVector& v) {
    return Point2D(origin.x() + v[0], origin.y() + v[1]);
  }
};
}  // namespace minisam