Generalized ICP

[braddodson]

Have you considered implementing a Plane-To-Plane Error Minimizer based on generalized icp (http://www.roboticsproceedings.org/rss05/p21.pdf)?

[pomerlef]

That would be a great add-on! It would be nice to compare it with the other point-to-point and point-to-plane error minimizer. Libpointmatcher is modular in the sense that once would only need to implement a new errorMinimizer class to have it in. We have a class named IdentityErrorMinimizer that can be copy pasted as a starting point https://github.com/ethz-asl/libpointmatcher/blob/master/pointmatcher/ErrorMinimizersImpl.cpp#L47

Currently, we are missing a bit of time to develop new module our self, but we welcome merge request from developers. If you're willing to do it, I can support you with the unit tests and integration standardization. If you want a starting point, we can read the Developer section of our tutorials: https://github.com/ethz-asl/libpointmatcher/blob/master/doc/Tutorials.md#developer

[braddodson]

Wow, it turns out to be more complex than I realized. I had read the paper, but it gives no clue about how to solve for the optimal transform. It seems like the implementation in PCL (https://github.com/PointCloudLibrary/pcl/blob/3b867193538bdf57cb7ec04e7a9350f2c210d892/registration/include/pcl/registration/impl/gicp.hpp) uses Broyden-Fletcher-Goldfarb-Shanno algorithm and gets rather involved.

[pomerlef]

ahhh not fun when they are doing that...

There are other papers on plane-to-plane [1], it might worth it to have a look if they have a close form solution. I'm reviewing a paper where they used a damped Gauss- Newton algorithm for GICP. After they get the results of the review, I might ask for their solution.

[1] Kaustubh Pathak, Andreas Birk, Narunas Vaskevicius, and Jann Poppinga. Fast Registration Based on Noisy Planes with Unknown Correspondences for 3D Mapping. IEEE Transactions on Robotics, 26 (3), pp. 424 – 441, 2010

[Niexiaoer]

Is there any procedure realizing the idea of the Kaustubh's paper work? I really want to know how it works and results.

[pomerlef]

It's on the long term plan, but up to now, nobody had time to implement it.

[adrelino]

As far as I know, GICP's plane-to-plane error functional doesn't have a closed-form solution, in contrast to the closed-form solution for point-to-point and the approximated closed-form solution for point-to-plane. Do you only consider closed-form solutions worth integrating in this library?

In the Results section in the original paper, the authors state that they implemented the minimization with a conjugate gradients method.

However, for these iterative minimization methods to work, one needs to compute the Jacobians of the error functional. Since the authors don't mention anything about them, I guess they just used numeric derivatives.

It is more accurate to use analytic derivatives, but the derivation by hand can become quite error prone for complex error functionals such as plane-to-plane. However, this was done in the nonlinear graph optimization framework g2o, but I do not trust their implementation of the analytic Jacobian:

// Jacobian
  // [ -R0'*R1 | R0 * dRdx/ddx * 0p1 ]
  // [  R0'*R1 | R0 * dR'dx/ddx * 0p1 ]

#ifdef GICP_ANALYTIC_JACOBIANS

  // jacobian defined as:
  //    f(T0,T1) =  dR0.inv() * T0.inv() * (T1 * dR1 * p1 + dt1) - dt0
  //    df/dx0 = [-I, d[dR0.inv()]/dq0 * T01 * p1]
  //    df/dx1 = [R0, T01 * d[dR1]/dq1 * p1]
  void Edge_V_V_GICP::linearizeOplus()
  {
    VertexSE3* vp0 = static_cast<VertexSE3*>(_vertices[0]);
    VertexSE3* vp1 = static_cast<VertexSE3*>(_vertices[1]);

    // topLeftCorner<3,3>() is the rotation matrix
    Matrix3D R0T = vp0->estimate().matrix().topLeftCorner<3,3>().transpose();
    Vector3D p1 = measurement().pos1;

    // this could be more efficient
    if (!vp0->fixed())
      {
        Isometry3D T01 = vp0->estimate().inverse() *  vp1->estimate();
        Vector3D p1t = T01 * p1;
        _jacobianOplusXi.block<3,3>(0,0) = -Matrix3D::Identity();
        _jacobianOplusXi.block<3,1>(0,3) = dRidx*p1t;
        _jacobianOplusXi.block<3,1>(0,4) = dRidy*p1t;
        _jacobianOplusXi.block<3,1>(0,5) = dRidz*p1t;
      }

    if (!vp1->fixed())
      {
        Matrix3D R1 = vp1->estimate().matrix().topLeftCorner<3,3>();
        R0T = R0T*R1;
        _jacobianOplusXj.block<3,3>(0,0) = R0T;
        _jacobianOplusXj.block<3,1>(0,3) = R0T*dRidx.transpose()*p1;
        _jacobianOplusXj.block<3,1>(0,4) = R0T*dRidy.transpose()*p1;
        _jacobianOplusXj.block<3,1>(0,5) = R0T*dRidz.transpose()*p1;
      }
  }
#endif

from https://github.com/RainerKuemmerle/g2o/blob/a94eddbb627415cfc47772b27b3b0221e582a937/g2o/types/icp/types_icp.cpp#L165

Instead, I implemented a variant of GICP's plane-to-plane error functional in my master's thesis and let the preprocessor compute the Jacobians using Ceres Solver's Autodifferentation functionality. The only requirement there is that the error functional can be written down fully templated, but by using Eigen, this was not a problem. Then the minimization is carried out with Gauss-Newton or Levenberg-Marquardt using Ceres. I could extract the needed code if wanted and if I find the time.

Is there a possibility to include Ceres or at least Ceres Autodiff functionality into libpointmatcher? Do you already have written your own nonlinear optimizer or can g2o or Ceres be added as a dependency?

[pomerlef]

Thanks for your inputs @adrelino, it's very useful. Is there something in particular that make you don't trust the g2o solution?

In general, we try to keep the dependency to the bare minimum. Otherwise, the support time start to explode given the multi-platform deployment we are dealing with.

I've the impression that there is a way to have an analytic approximated solution based on the transformation of the last iteration. I would need to put more time in this.