types_icp.h

Go to the documentation of this file.

// g2o - General Graph Optimization
// Copyright (C) 2011 Kurt Konolige
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
#ifndef G2O_TYPES_ICP
#define G2O_TYPES_ICP
 
#define GICP_ANALYTIC_JACOBIANS
// #define SCAM_ANALYTIC_JACOBIANS
 
#include <Eigen/Geometry>
#include <iostream>
 
#include "g2o/core/base_binary_edge.h"
#include "g2o/core/base_variable_sized_edge.h"
#include "g2o/core/base_vertex.h"
#include "g2o/types/sba/types_sba.h"
#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o_types_icp_api.h"
 
namespace g2o {
 
namespace types_icp {
void init();
}
//
// GICP-type edges
// Each measurement is between two rigid points on each 6DOF vertex
//
 
//
// class for edges between two points rigidly attached to vertices
//
 
class G2O_TYPES_ICP_API EdgeGICP {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
 public:
  // point positions
  Vector3 pos0, pos1;
 
  // unit normals
  Vector3 normal0, normal1;
 
  // rotation matrix for normal
  Matrix3 R0, R1;
 
  // initialize an object
  EdgeGICP() {
    pos0.setZero();
    pos1.setZero();
    normal0 << 0, 0, 1;
    normal1 << 0, 0, 1;
    // makeRot();
    R0.setIdentity();
    R1.setIdentity();
  }
 
  // set up rotation matrix for pos0
  void makeRot0() {
    Vector3 y;
    y << 0, 1, 0;
    R0.row(2) = normal0;
    y = y - normal0(1) * normal0;
    y.normalize();  // need to check if y is close to 0
    R0.row(1) = y;
    R0.row(0) = normal0.cross(R0.row(1));
    //      cout << normal.transpose() << endl;
    //      cout << R0 << endl << endl;
    //      cout << R0*R0.transpose() << endl << endl;
  }
 
  // set up rotation matrix for pos1
  void makeRot1() {
    Vector3 y;
    y << 0, 1, 0;
    R1.row(2) = normal1;
    y = y - normal1(1) * normal1;
    y.normalize();  // need to check if y is close to 0
    R1.row(1) = y;
    R1.row(0) = normal1.cross(R1.row(1));
  }
 
  // returns a precision matrix for point-plane
  Matrix3 prec0(double e) {
    makeRot0();
    Matrix3 prec;
    prec << e, 0, 0, 0, e, 0, 0, 0, 1;
    return R0.transpose() * prec * R0;
  }
 
  // returns a precision matrix for point-plane
  Matrix3 prec1(double e) {
    makeRot1();
    Matrix3 prec;
    prec << e, 0, 0, 0, e, 0, 0, 0, 1;
    return R1.transpose() * prec * R1;
  }
 
  // return a covariance matrix for plane-plane
  Matrix3 cov0(double e) {
    makeRot0();
    Matrix3 cov;
    cov << 1, 0, 0, 0, 1, 0, 0, 0, e;
    return R0.transpose() * cov * R0;
  }
 
  // return a covariance matrix for plane-plane
  Matrix3 cov1(double e) {
    makeRot1();
    Matrix3 cov;
    cov << 1, 0, 0, 0, 1, 0, 0, 0, e;
    return R1.transpose() * cov * R1;
  }
};
 
// 3D rigid constraint
//    3 values for position wrt frame
//    3 values for normal wrt frame, not used here
// first two args are the measurement type, second two the connection classes
class G2O_TYPES_ICP_API Edge_V_V_GICP
    : public BaseBinaryEdge<3, EdgeGICP, VertexSE3, VertexSE3> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  Edge_V_V_GICP() : pl_pl(false) {}
  Edge_V_V_GICP(const Edge_V_V_GICP* e);
 
  // switch to go between point-plane and plane-plane
  bool pl_pl;
  Matrix3 cov0, cov1;
 
  // I/O functions
  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;
 
  // return the error estimate as a 3-vector
  void computeError() {
    // from <ViewPoint> to <Point>
    const VertexSE3* vp0 = static_cast<const VertexSE3*>(_vertices[0]);
    const VertexSE3* vp1 = static_cast<const VertexSE3*>(_vertices[1]);
 
    // get vp1 point into vp0 frame
    // could be more efficient if we computed this transform just once
    Vector3 p1;
 
#if 0
      if (_cnum >= 0 && 0)      // using global cache
        {
          if (_tainted[_cnum])  // set up transform
            {
              _transforms[_cnum] = vp0->estimate().inverse() * vp1->estimate();
              _tainted[_cnum] = 0;
              cout << _transforms[_cnum] << endl;
            }
          p1 = _transforms[_cnum].map(measurement().pos1); // do the transform
        }
      else
#endif
    {
      p1 = vp1->estimate() * measurement().pos1;
      p1 = vp0->estimate().inverse() * p1;
    }
 
    //      cout << endl << "Error computation; points are: " << endl;
    //      cout << p0.transpose() << endl;
    //      cout << p1.transpose() << endl;
 
    // get their difference
    // this is simple Euclidean distance, for now
    _error = p1 - measurement().pos0;
 
#if 0
      cout << "vp0" << endl << vp0->estimate() << endl;
      cout << "vp1" << endl << vp1->estimate() << endl;
      cout << "e Jac Xj" << endl <<  _jacobianOplusXj << endl << endl;
      cout << "e Jac Xi" << endl << _jacobianOplusXi << endl << endl;
#endif
 
    if (!pl_pl) return;
 
    // re-define the information matrix
    // topLeftCorner<3,3>() is the rotation()
    const Matrix3 transform = (vp0->estimate().inverse() * vp1->estimate())
                                  .matrix()
                                  .topLeftCorner<3, 3>();
    information() = (cov0 + transform * cov1 * transform.transpose()).inverse();
  }
 
  // try analytic jacobians
#ifdef GICP_ANALYTIC_JACOBIANS
  virtual void linearizeOplus();
#endif
 
  // global derivative matrices
  static Matrix3 dRidx;
  static Matrix3 dRidy;
  static Matrix3 dRidz;  // differential quat matrices
};
 
class G2O_TYPES_ICP_API VertexSCam : public VertexSE3 {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  VertexSCam();
 
  // I/O
  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;
 
  // capture the update function to reset aux transforms
  virtual void oplusImpl(const double* update) {
    VertexSE3::oplusImpl(update);
    setAll();
  }
 
  // camera matrix and stereo baseline
  static Matrix3 Kcam;
  static double baseline;
 
  // transformations
  Eigen::Matrix<double, 3, 4, Eigen::ColMajor>
      w2n;  // transform from world to node coordinates
  Eigen::Matrix<double, 3, 4, Eigen::ColMajor>
      w2i;  // transform from world to image coordinates
 
  // Derivatives of the rotation matrix transpose wrt quaternion xyz, used for
  // calculating Jacobian wrt pose of a projection.
  Matrix3 dRdx, dRdy, dRdz;
 
  // transforms
  static void transformW2F(Eigen::Matrix<double, 3, 4, Eigen::ColMajor>& m,
                           const Vector3& trans, const Quaternion& qrot) {
    m.block<3, 3>(0, 0) = qrot.toRotationMatrix().transpose();
    m.col(3).setZero();  // make sure there's no translation
    Vector4 tt;
    tt.head(3) = trans;
    tt[3] = 1.0;
    m.col(3) = -m * tt;
  }
 
  static void transformF2W(Eigen::Matrix<double, 3, 4, Eigen::ColMajor>& m,
                           const Vector3& trans, const Quaternion& qrot) {
    m.block<3, 3>(0, 0) = qrot.toRotationMatrix();
    m.col(3) = trans;
  }
 
  // set up camera matrix
  static void setKcam(double fx, double fy, double cx, double cy, double tx) {
    Kcam.setZero();
    Kcam(0, 0) = fx;
    Kcam(1, 1) = fy;
    Kcam(0, 2) = cx;
    Kcam(1, 2) = cy;
    Kcam(2, 2) = 1.0;
    baseline = tx;
  }
 
  // set transform from world to cam coords
  void setTransform() {
    w2n = estimate().inverse().matrix().block<3, 4>(0, 0);
    // transformW2F(w2n,estimate().translation(), estimate().rotation());
  }
 
  // Set up world-to-image projection matrix (w2i), assumes camera parameters
  // are filled.
  void setProjection() { w2i = Kcam * w2n; }
 
  // sets angle derivatives
  void setDr() {
    // inefficient, just for testing
    // use simple multiplications and additions for production code in
    // calculating dRdx,y,z for dS'*R', with dS the incremental change
    dRdx = dRidx * w2n.block<3, 3>(0, 0);
    dRdy = dRidy * w2n.block<3, 3>(0, 0);
    dRdz = dRidz * w2n.block<3, 3>(0, 0);
  }
 
  // set all aux transforms
  void setAll() {
    setTransform();
    setProjection();
    setDr();
  }
 
  // calculate stereo projection
  void mapPoint(Vector3& res, const Vector3& pt3) {
    Vector4 pt;
    pt.head<3>() = pt3;
    pt(3) = cst(1.0);
    Vector3 p1 = w2i * pt;
    Vector3 p2 = w2n * pt;
    Vector3 pb(baseline, 0, 0);
 
    double invp1 = cst(1.0) / p1(2);
    res.head<2>() = p1.head<2>() * invp1;
 
    // right camera px
    p2 = Kcam * (p2 - pb);
    res(2) = p2(0) / p2(2);
  }
 
  static Matrix3 dRidx;
  static Matrix3 dRidy;
  static Matrix3 dRidz;
};
 
// stereo projection
// first two args are the measurement type, second two the connection classes
class G2O_TYPES_ICP_API Edge_XYZ_VSC
    : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSCam> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  Edge_XYZ_VSC();
 
  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;
 
  // return the error estimate as a 2-vector
  void computeError() {
    // from <Point> to <Cam>
    const VertexPointXYZ* point =
        static_cast<const VertexPointXYZ*>(_vertices[0]);
    VertexSCam* cam = static_cast<VertexSCam*>(_vertices[1]);
    // cam->setAll();
 
    // calculate the projection
    Vector3 kp;
    cam->mapPoint(kp, point->estimate());
 
    // std::cout << std::endl << "CAM   " << cam->estimate() << std::endl;
    // std::cout << "POINT " << pt.transpose() << std::endl;
    // std::cout << "PROJ  " << p1.transpose() << std::endl;
    // std::cout << "PROJ  " << p2.transpose() << std::endl;
    // std::cout << "CPROJ " << kp.transpose() << std::endl;
    // std::cout << "MEAS  " << _measurement.transpose() << std::endl;
 
    // error, which is backwards from the normal observed - calculated
    // _measurement is the measured projection
    _error = kp - _measurement;
  }
#ifdef SCAM_ANALYTIC_JACOBIANS
  // jacobian
  virtual void linearizeOplus();
#endif
};
 
}  // namespace g2o
 
#endif  // TYPES_ICP