Better mechanism for optional Jacobians

[Frank Dellaert]

boost::optional<Matrix> is bad as it forces - allocation on heap - dynamic matrices. There must be some Eigen magic that we can pass blocks, constant sized matrices, memory maps, if we want to ?

[Richard Roberts]

Yes, this would really speed up linearization I think. This is the object we want, I believe: http://eigen.tuxfamily.org/dox-devel/classEigen_1_1Ref.html It allows you to reference blocks, constant-size matrices, and full matrices (I'm not sure about generic maps), without making the function templated.

In fact, since I am already working on a linearizeInPlace function (which allows the GaussianJunctionTree to be reused), I will go ahead and make it accept Ref<Matrix> so that we can test this out there.

[Frank Dellaert]

To write to it might not be that easy, though. I did try something in LM branch, here, see URL in comment:

  /**
   * Calculate Jacobian with respect to pose
   * @param pn projection in normalized coordinates
   * @param d disparity (inverse depth)
   * @param Dpi_pn derivative of uncalibrate with respect to pn
   * @param Dpose Output argument, can be matrix or block, assumed right size !
   * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
   */
  template<typename Derived>
  static void calculateDpose(const Point2& pn, double d, const Matrix& Dpi_pn,
      Eigen::MatrixBase<Derived> const & Dpose) {
    // optimized version of derivatives, see CalibratedCamera.nb
    const double u = pn.x(), v = pn.y();
    double uv = u * v, uu = u * u, vv = v * v;
    Eigen::Matrix<double, 2, 6> Dpn_pose;
    Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
    assert(Dpose.rows()==2 && Dpose.cols()==6);
    const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
        Dpi_pn.block<2, 2>(0, 0) * Dpn_pose;
  }

[Richard Roberts]

Yes, this is the template way to do it, but Ref should make it much easier. Let me test it out today in the linearizeInPlace function, and if that is straightforward, I'll send you a code snippet so you can use Ref other places.