g2o 中定义好的常用边的类型

滴滴哒哒答答

已于 2024-10-15 10:37:04 修改

阅读量224

点赞数 1

分类专栏： g2o 文章标签：机器人 ubuntu 自动驾驶

于 2024-09-29 19:51:27 首次发布

本文链接：https://blog.csdn.net/m0_49384824/article/details/142642365

版权

g2o 专栏收录该内容

4 篇文章 0 订阅

订阅专栏

前言

G2O的边分为两种类型的边，固定长度的边（BaseFixedSizedEdge）、可变长度的边（BaseVariableSizedEdge）。其中，固定长度的边又分为BaseUnaryEdge（一元边）、BaseBinaryEdge（二元边），可变长度的边又引申出了BaseMultiEdge（多元边）。

g2o 边的源码

//***g2o源码 g2o/g2o/core/base_unary_edge.h ***//
/**
 * \brief 模板化 BaseUnaryEdge，单边
 *
 * D  : int 类型，表示测量值的维度（Dimension）
 * E  : 测量值的数据类型
 */
template <int D, typename E, typename VertexXi>
class BaseUnaryEdge : public BaseEdge<D,E>{
    // 类的具体实现...
};

//***g2o源码 g2o/g2o/core/base_binary_edge.h ***
/**
 * \brief 模板化 BaseBinaryEdge，双边
 *
 * D  : int 类型，表示测量值的维度（Dimension）
 * E  : 测量值的数据类型
 * VertexXi  : 边连接的顶点类型
 * VertexXj  : 边连接的顶点类型
 */
template <int D, typename E, typename VertexXi, typename VertexXj>
class BaseBinaryEdge : public BaseEdge<D, E>{
    // 类的具体实现...
};

//***g2o源码 g2o/g2o/core/base_multi_edge.h ***//
/**
 * D  : int 类型，表示测量值的维度（Dimension）
 * E  : 测量值的数据类型
 */
template <int D, typename E>
using BaseMultiEdge = BaseVariableSizedEdge<D, E>;

一般来说定义 Edge 需要重写这几个函数，需要注意的是，一旦指定了linearizeOplus()，就会使用该雅可比矩阵进行优化；如果不对该函数进行重写，就会使用自动求导的方式进行优化。

//*** g2o源码 g2o/g2o/core/optimizable_graph.h ***// 

// 读盘、存盘函数，不需要进行读/写操作的话，仅仅声明一下就可以
// 纯虚函数，从流中读取顶点，即顶点的内部状态
virtual bool read(std::istream& is) = 0;
// 纯虚函数，将顶点写入到流
virtual bool write(std::ostream& os) const = 0;

// 边误差计算函数，使用当前边对应顶点计算得到的测量值与真实的测量值之间的误差
// 计算边的误差并将其存储在内部结构中
virtual void computeError() = 0;
// 边雅可比函数，计算在当前顶点的值下，误差对优化变量的偏导数，即 Jacobian
// 将流形空间中边的约束线性化，并将结果存储在给定的工作空间中
virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace) = 0;

g2o 边模版

 class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>{
   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW      

     // myEdge 类的构造函数，调用了基类 BaseBinaryEdge<D, E, VertexXi, VertexXj> 的构造函数，初始化该边。
     myEdge() : BaseBinaryEdge<D, E, VertexXi, VertexXj>() {}
    

     bool read(std::istream& is) override {
       // 从输入流读取边数据的实现
     }
 
     bool write(std::ostream& os) const override {
       // 将边数据写入输出流的实现
     }

     void computeError() override{
          // 误差 = 估计值 - 测量值
          _error = /* 估计值 */ - _measurement;
          /**
          _error = /* _measurement 矩阵的逆*/  * /* 估计值 */;
          */
      }      

      // 误差对优化变量的偏导数
      void linearizeOplus() override{
          _jacobianOplusXi(pos, pos) = /* Jacobian */;       
          /**
          _jocobianOplusXj(pos, pos) = /* Jacobian */;
          */
      }      

    private:
      // data
  }

g2o 中定义好的常用边的类型汇总如下，我们挑选几个常用的详细介绍

// 测量值：平面2D位姿，顶点：平面2D位姿
EdgeSE2Prior : public BaseUnaryEdge<3, SE2, VertexSE2>
// 测量值：平面2D点，顶点：平面2D位姿
EdgeSE2XYPrior : public BaseUnaryEdge<2, Vector2, VertexSE2>
// 测量值：平面2D点，顶点：空间3D点
EdgeXYPrior : public BaseUnaryEdge<2, Vector2, VertexPointXY>
// 测量值：欧式变换矩阵(4x4)，顶点：空间3D位姿（四元数旋转）
EdgeSE3Prior : public BaseUnaryEdge<6, Isometry3, VertexSE3>
// 测量值：空间3D点，顶点：空间3D位姿（四元数旋转）
EdgeSE3XYZPrior : public BaseUnaryEdge<3, Vector3, VertexSE3>
// 测量值：空间3D点，顶点：空间3D点
EdgeXYZPrior : public BaseUnaryEdge<3, Vector3, VertexPointXYZ>


// 测量值：平面2D点，顶点：平面2D点、平面2D点
EdgePointXY : public BaseBinaryEdge<2, Vector2, VertexPointXY, VertexPointXY>
// 测量值：平面2D位姿，顶点：平面2D位姿、平面2D位姿
EdgeSE2 : public BaseBinaryEdge<3, SE2, VertexSE2, VertexSE2>
// 测量值：平面2D点，顶点：平面2D位姿、平面2D点
EdgeSE2PointXY : public BaseBinaryEdge<2, Vector2, VertexSE2, VertexPointXY>
// 测量值：空间3D点，顶点：空间3D点、空间3D点
EdgePointXYZ : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexPointXYZ>
// 测量值：欧式变换矩阵(4x4)，顶点：空间3D位姿（四元数旋转）、空间3D位姿（四元数旋转）
EdgeSE3 : public BaseBinaryEdge<6, Isometry3, VertexSE3, VertexSE3>
// 测量值：空间3D点，顶点：空间3D位姿（四元数旋转）、空间3D点
EdgeSE3PointXYZ : public BaseBinaryEdge<3, Vector3, VertexSE3, VertexPointXYZ>
// 测量值：空间3D点，顶点：空间3D点、空间3D位姿（SE3，李代数旋转）
EdgeStereoSE3ProjectXYZ : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSE3Expmap>
// 测量值：平面2D点，顶点：空间3D点、空间3D位姿（SE3，李代数旋转）
EdgeSE3ProjectXYZ : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap>
// 测量值：平面2D点，顶点：空间3D点、空间3D位姿（SE3，李代数旋转）
EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap>
// 测量值：空间3D点，顶点：空间3D点、空间3D位姿（SE3，李代数旋转）
EdgeProjectXYZ2UVU : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSE3Expmap>
// 测量值：Sim3，顶点：Sim3、Sim3
EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>
// 测量值：空间2D点，顶点：空间3D点、Sim3
EdgeSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSim3Expmap>
// 测量值：空间2D点，顶点：空间3D点、Sim3
EdgeInverseSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSim3Expmap>

一、EdgeSE3Prior : public BaseUnaryEdge<6, Isometry3, VertexSE3>

//*** g2o源码 g2o/g2o/types/slam3d/edge_se3_prior.h ***//
#ifndef G2O_EDGE_SE3_PRIOR_H_
#define G2O_EDGE_SE3_PRIOR_H_

#include "vertex_se3.h"
#include "g2o/core/base_unary_edge.h"
#include "parameter_se3_offset.h"
#include "g2o_types_slam3d_api.h"
namespace g2o {
  /**
   * \brief prior for an SE3 element
   *
   * Provides a prior for a 3d pose vertex. Again the measurement is represented by an
   * Isometry3 matrix.
   */
  class G2O_TYPES_SLAM3D_API EdgeSE3Prior : public BaseUnaryEdge<6, Isometry3, VertexSE3> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeSE3Prior();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    // return the error estimate as a 3-vector
    void computeError();
    
    // jacobian
    virtual void linearizeOplus();

    /* 类中其他函数 */


} // end namespace

//*** g2o源码 g2o/g2o/types/slam3d/edge_se3_prior.cpp ***//
#include "edge_se3_prior.h"

#include <iostream>

#include "isometry3d_gradients.h"

namespace g2o {
using namespace std;

// point to camera projection, monocular
EdgeSE3Prior::EdgeSE3Prior() : BaseUnaryEdge<6, Isometry3, VertexSE3>() {
  setMeasurement(Isometry3::Identity());
  information().setIdentity();
  _cache = 0;
  _offsetParam = 0;
  resizeParameters(1);
  installParameter(_offsetParam, 0);
}

bool EdgeSE3Prior::read(std::istream& is) {
  bool state = readParamIds(is);
  Vector7 meas;
  state &= internal::readVector(is, meas);
  setMeasurement(internal::fromVectorQT(meas));
  state &= readInformationMatrix(is);
  return state;
}

bool EdgeSE3Prior::write(std::ostream& os) const {
  writeParamIds(os);
  internal::writeVector(os, internal::toVectorQT(measurement()));
  writeInformationMatrix(os);
  return os.good();
}

void EdgeSE3Prior::computeError() {
  Isometry3 delta = _inverseMeasurement * _cache->n2w();
  _error = internal::toVectorMQT(delta);
}

void EdgeSE3Prior::linearizeOplus() {
  VertexSE3* from = static_cast<VertexSE3*>(_vertices[0]);
  Isometry3 E;
  Isometry3 Z, X, P;
  X = from->estimate();
  P = _cache->offsetParam()->offset();
  Z = _measurement;
  internal::computeEdgeSE3PriorGradient(E, _jacobianOplusXi, Z, X, P);
}

//*** g2o源码 g2o/g2o/types/slam3d/isometry3d_gradients.h ***//
template <typename Derived>
void computeEdgeSE3PriorGradient(Isometry3& E,
                                 const Eigen::MatrixBase<Derived>& JConstRef,
                                 const Isometry3& Z, const Isometry3& X,
                                 const Isometry3& P = Isometry3::Identity()) {
  Eigen::MatrixBase<Derived>& J =
      const_cast<Eigen::MatrixBase<Derived>&>(JConstRef);
  J.derived().resize(6, 6);
  // compute the error at the linearization point
  const Isometry3 A = Z.inverse() * X;
  const Isometry3& B = P;
  Isometry3::ConstLinearPart Ra = extractRotation(A);
  Isometry3::ConstLinearPart Rb = extractRotation(B);
  Isometry3::ConstTranslationPart tb = B.translation();
  E = A * B;
  Isometry3::ConstLinearPart Re = extractRotation(E);

  Eigen::Matrix<number_t, 3, 9, Eigen::ColMajor> dq_dR;
  compute_dq_dR(dq_dR, Re(0, 0), Re(1, 0), Re(2, 0), Re(0, 1), Re(1, 1),
                Re(2, 1), Re(0, 2), Re(1, 2), Re(2, 2));

  J.setZero();

  // dte/dt
  J.template block<3, 3>(0, 0) = Ra;

  // dte/dq =0
  // dte/dqj
  {
    Matrix3 S;
    skew(S, tb);
    J.template block<3, 3>(0, 3) = Ra * S;
  }

  // dre/dt =0

  // dre/dq
  {
    number_t buf[27];
    Eigen::Map<Eigen::Matrix<number_t, 9, 3, Eigen::ColMajor> > M(buf);
    Matrix3 Sx, Sy, Sz;
    internal::skew(Sx, Sy, Sz, Rb);
// 检测当前编译器是否为 Clang
#ifdef __clang__
    Matrix3 temp = Ra * Sx;
    Eigen::Map<Matrix3> M2(temp.data());
    Eigen::Map<Matrix3> Mx(buf);
    Mx = M2;
    temp = Ra * Sy;
    Eigen::Map<Matrix3> My(buf + 9);
    My = M2;
    temp = Ra * Sz;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz = M2;
#else
    Eigen::Map<Matrix3> Mx(buf);
    Mx = Ra * Sx;
    Eigen::Map<Matrix3> My(buf + 9);
    My = Ra * Sy;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz = Ra * Sz;
#endif
    J.template block<3, 3>(3, 3) = dq_dR * M;
  }
}

二、EdgeSE3XYZPrior : public BaseUnaryEdge<3, Vector3, VertexSE3>

//*** g2o源码 g2o/g2o/types/slam3d/edge_se3_xyzprior.h ***//
#ifndef G2O_EDGE_SE3_PRIOR_XYZ_H
#define G2O_EDGE_SE3_PRIOR_XYZ_H

#include "g2o/core/base_unary_edge.h"
#include "g2o_types_slam3d_api.h"
#include "parameter_se3_offset.h"
#include "vertex_se3.h"

namespace g2o {
/**
 * \brief Prior for a 3D pose with constraints only in xyz direction
 * \brief 仅在 xyz 方向上具有约束的 3D位姿 先验
 */
class G2O_TYPES_SLAM3D_API EdgeSE3XYZPrior
    : public BaseUnaryEdge<3, Vector3, VertexSE3> {
 public:
  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;
  virtual void computeError();
  virtual void linearizeOplus();
  
  /* 类中其他函数 */

 protected:
  virtual bool resolveCaches();
  ParameterSE3Offset* _offsetParam;
  CacheSE3Offset* _cache;
};

}  // namespace g2o

#endif

//*** g2o源码 g2o/g2o/types/slam3d/edge_se3_xyzprior.cpp ***//
#include "edge_se3_xyzprior.h"

namespace g2o {

bool EdgeSE3XYZPrior::read(std::istream& is) {
  readParamIds(is);
  internal::readVector(is, _measurement);
  return readInformationMatrix(is);
}

bool EdgeSE3XYZPrior::write(std::ostream& os) const {
  writeParamIds(os);
  internal::writeVector(os, measurement());
  return writeInformationMatrix(os);
}

void EdgeSE3XYZPrior::computeError() {
  const VertexSE3* v = static_cast<const VertexSE3*>(_vertices[0]);
  _error = v->estimate().translation() - _measurement;
}

void EdgeSE3XYZPrior::linearizeOplus() {
  const VertexSE3* v = static_cast<const VertexSE3*>(_vertices[0]);
  _jacobianOplusXi.block<3, 3>(0, 0) = v->estimate().rotation();
  _jacobianOplusXi.block<3, 3>(0, 3) = Eigen::Matrix3d::Zero();
}

}  // namespace g2o

三、 EdgePointXYZ : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexPointXYZ>

//*** g2o源码 g2o/g2o/types/slam3d/edge_pointxyz.h ***//
#ifndef G2O_EDGE_POINTXYZ_H
#define G2O_EDGE_POINTXYZ_H

#include "g2o/config.h"
#include "g2o/core/base_binary_edge.h"
#include "g2o_types_slam3d_api.h"
#include "vertex_pointxyz.h"

namespace g2o {

class G2O_TYPES_SLAM3D_API EdgePointXYZ
    : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexPointXYZ> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  EdgePointXYZ();

  void computeError() {
    const VertexPointXYZ* v1 = static_cast<const VertexPointXYZ*>(_vertices[0]);
    const VertexPointXYZ* v2 = static_cast<const VertexPointXYZ*>(_vertices[1]);
    _error = (v2->estimate() - v1->estimate()) - _measurement;
  }
  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;

  /* 类中其他函数 */

#ifndef NUMERIC_JACOBIAN_THREE_D_TYPES
  virtual void linearizeOplus();
#endif

}  // namespace g2o

#endif

//*** g2o源码 g2o/g2o/types/slam3d/edge_pointxyz.cpp ***//
#include "edge_pointxyz.h"

namespace g2o {

EdgePointXYZ::EdgePointXYZ()
    : BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexPointXYZ>() {
  _information.setIdentity();
  _error.setZero();
}

bool EdgePointXYZ::read(std::istream& is) {
  Vector3 p;
  is >> p[0] >> p[1] >> p[2];
  setMeasurement(p);
  for (int i = 0; i < 3; ++i)
    for (int j = i; j < 3; ++j) {
      is >> information()(i, j);
      if (i != j) information()(j, i) = information()(i, j);
    }
  return true;
}

bool EdgePointXYZ::write(std::ostream& os) const {
  Vector3 p = measurement();
  os << p.x() << " " << p.y() << " " << p.z();
  for (int i = 0; i < 3; ++i)
    for (int j = i; j < 3; ++j) os << " " << information()(i, j);
  return os.good();
}

#ifndef NUMERIC_JACOBIAN_THREE_D_TYPES
void EdgePointXYZ::linearizeOplus() {
  _jacobianOplusXi = -Matrix3::Identity();
  _jacobianOplusXj = Matrix3::Identity();
}
#endif

}  // namespace g2o

四、EdgeSE3 : public BaseBinaryEdge<6, Isometry3, VertexSE3, VertexSE3>

测量值：欧式变换矩阵(4x4)，顶点：两个空间3D位姿（四元数旋转）

其中，测量值是两个顶点之间的相对 3D 位姿测量

//*** g2o源码 g2o/g2o/types/slam3d/edge_se3.h ***//
#ifndef G2O_EDGE_SE3_H_
#define G2O_EDGE_SE3_H_

#include "g2o/core/base_binary_edge.h"
#include "g2o_types_slam3d_api.h"
#include "vertex_se3.h"

namespace g2o {

/**
 * \brief Edge between two 3D pose vertices
 *
 * The transformation between the two vertices is given as an Isometry3.
 * If z denotes the measurement, then the error function is given as follows:
 * z^-1 * (x_i^-1 * x_j)
 */
class G2O_TYPES_SLAM3D_API EdgeSE3
    : public BaseBinaryEdge<6, Isometry3, VertexSE3, VertexSE3> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  EdgeSE3();
  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;

  void computeError();

  /* 类中其他函数 */

  void linearizeOplus();

};

}  // namespace g2o

//*** g2o源码 g2o/g2o/types/slam3d/edge_se3.cpp ***//
#include "edge_se3.h"

#include <iostream>

#include "isometry3d_gradients.h"

#ifdef G2O_HAVE_OPENGL
#include "g2o/stuff/opengl_primitives.h"
#include "g2o/stuff/opengl_wrapper.h"
#endif

namespace g2o {
using namespace std;

EdgeSE3::EdgeSE3() : BaseBinaryEdge<6, Isometry3, VertexSE3, VertexSE3>() {
  information().setIdentity();
}

bool EdgeSE3::read(std::istream& is) {
  Vector7 meas;
  internal::readVector(is, meas);
  // normalize the quaternion to recover numerical precision lost by storing as
  // human readable text
  Vector4::MapType(meas.data() + 3).normalize();
  setMeasurement(internal::fromVectorQT(meas));
  if (is.bad()) return false;
  readInformationMatrix(is);
  return is.good() || is.eof();
}

bool EdgeSE3::write(std::ostream& os) const {
  internal::writeVector(os, internal::toVectorQT(measurement()));
  return writeInformationMatrix(os);
}

void EdgeSE3::computeError() {
  VertexSE3* from = static_cast<VertexSE3*>(_vertices[0]);
  VertexSE3* to = static_cast<VertexSE3*>(_vertices[1]);
  Isometry3 delta =
      _inverseMeasurement * from->estimate().inverse() * to->estimate();
  _error = internal::toVectorMQT(delta);
}

void EdgeSE3::linearizeOplus() {
  // BaseBinaryEdge<6, Isometry3, VertexSE3, VertexSE3>::linearizeOplus();
  // return;

  VertexSE3* from = static_cast<VertexSE3*>(_vertices[0]);
  VertexSE3* to = static_cast<VertexSE3*>(_vertices[1]);
  Isometry3 E;
  const Isometry3& Xi = from->estimate();
  const Isometry3& Xj = to->estimate();
  const Isometry3& Z = _measurement;
  internal::computeEdgeSE3Gradient(E, _jacobianOplusXi, _jacobianOplusXj, Z, Xi,
                                   Xj);
}

}  // namespace g2o

//*** g2o源码 g2o/g2o/types/slam3d/isometry3d_gradients.h ***//
template <typename Derived>
void computeEdgeSE3Gradient(Isometry3& E,
                            Eigen::MatrixBase<Derived> const& JiConstRef,
                            Eigen::MatrixBase<Derived> const& JjConstRef,
                            const Isometry3& Z, const Isometry3& Xi,
                            const Isometry3& Xj) {
  Eigen::MatrixBase<Derived>& Ji =
      const_cast<Eigen::MatrixBase<Derived>&>(JiConstRef);
  Eigen::MatrixBase<Derived>& Jj =
      const_cast<Eigen::MatrixBase<Derived>&>(JjConstRef);
  Ji.derived().resize(6, 6);
  Jj.derived().resize(6, 6);
  // compute the error at the linearization point
  const Isometry3 A = Z.inverse();
  const Isometry3 B = Xi.inverse() * Xj;

  E = A * B;

  Isometry3::ConstLinearPart Re = extractRotation(E);
  Isometry3::ConstLinearPart Ra = extractRotation(A);
  Isometry3::ConstLinearPart Rb = extractRotation(B);
  Isometry3::ConstTranslationPart tb = B.translation();

  Eigen::Matrix<number_t, 3, 9, Eigen::ColMajor> dq_dR;
  compute_dq_dR(dq_dR, Re(0, 0), Re(1, 0), Re(2, 0), Re(0, 1), Re(1, 1),
                Re(2, 1), Re(0, 2), Re(1, 2), Re(2, 2));

  Ji.setZero();
  Jj.setZero();

  // dte/dti
  Ji.template block<3, 3>(0, 0) = -Ra;

  // dte/dtj
  Jj.template block<3, 3>(0, 0) = Re;

  // dte/dqi
  {
    Matrix3 S;
    skewT(S, tb);
    Ji.template block<3, 3>(0, 3) = Ra * S;
  }

  // dte/dqj: this is zero

  number_t buf[27];
  Eigen::Map<Eigen::Matrix<number_t, 9, 3, Eigen::ColMajor> > M(buf);
  Matrix3 Sxt, Syt, Szt;
  // dre/dqi
  {
    skewT(Sxt, Syt, Szt, Rb);
    Eigen::Map<Matrix3> Mx(buf);
    Mx.noalias() = Ra * Sxt;
    Eigen::Map<Matrix3> My(buf + 9);
    My.noalias() = Ra * Syt;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz.noalias() = Ra * Szt;
    Ji.template block<3, 3>(3, 3) = dq_dR * M;
  }

  // dre/dqj
  {
    Matrix3& Sx = Sxt;
    Matrix3& Sy = Syt;
    Matrix3& Sz = Szt;
    skew(Sx, Sy, Sz, Matrix3::Identity());
    Eigen::Map<Matrix3> Mx(buf);
    Mx.noalias() = Re * Sx;
    Eigen::Map<Matrix3> My(buf + 9);
    My.noalias() = Re * Sy;
    Eigen::Map<Matrix3> Mz(buf + 18);
    Mz.noalias() = Re * Sz;
    Jj.template block<3, 3>(3, 3) = dq_dR * M;
  }
}

五、EdgeSE3PointXYZ : public BaseBinaryEdge<3, Vector3, VertexSE3, VertexPointXYZ>

//*** g2o源码 g2o/g2o/types/slam3d/edge_se3_pointxyz.h ***//
#ifndef G2O_EDGE_SE3_POINT_XYZ_H_
#define G2O_EDGE_SE3_POINT_XYZ_H_

#include "g2o/core/base_binary_edge.h"
#include "g2o_types_slam3d_api.h"
#include "parameter_se3_offset.h"
#include "vertex_pointxyz.h"
#include "vertex_se3.h"

namespace g2o {

/*! \class EdgeSE3PointXYZ
 * \brief g2o edge from a track to a point node
 */
// first two args are the measurement type, second two the connection classes
class G2O_TYPES_SLAM3D_API EdgeSE3PointXYZ
    : public BaseBinaryEdge<3, Vector3, VertexSE3, VertexPointXYZ> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  EdgeSE3PointXYZ();
  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;

  // return the error estimate as a 3-vector
  void computeError();
  // jacobian
  virtual void linearizeOplus();

  /* 类中其他函数 */

}  // namespace g2o

//*** g2o源码 g2o/g2o/types/slam3d/edge_se3_pointxyz.cpp ***//
#include "edge_se3_pointxyz.h"

#include "parameter_se3_offset.h"

#ifdef G2O_HAVE_OPENGL
#include "g2o/stuff/opengl_wrapper.h"
#endif

#include <iostream>

#ifdef G2O_HAVE_OPENGL
#include "g2o/stuff/opengl_primitives.h"
#include "g2o/stuff/opengl_wrapper.h"
#endif

namespace g2o {
using namespace std;

// point to camera projection, monocular
EdgeSE3PointXYZ::EdgeSE3PointXYZ()
    : BaseBinaryEdge<3, Vector3, VertexSE3, VertexPointXYZ>() {
  information().setIdentity();
  J.fill(0);
  J.block<3, 3>(0, 0) = -Matrix3::Identity();
  cache = 0;
  offsetParam = 0;
  resizeParameters(1);
  installParameter(offsetParam, 0);
}

bool EdgeSE3PointXYZ::read(std::istream& is) {
  readParamIds(is);
  Vector3 meas;
  internal::readVector(is, meas);
  setMeasurement(meas);
  readInformationMatrix(is);
  return is.good() || is.eof();
}

bool EdgeSE3PointXYZ::write(std::ostream& os) const {
  bool state = writeParamIds(os);
  state &= internal::writeVector(os, measurement());
  state &= writeInformationMatrix(os);
  return state;
}

void EdgeSE3PointXYZ::computeError() {
  // from cam to point (track)
  // VertexSE3 *cam = static_cast<VertexSE3*>(_vertices[0]);
  VertexPointXYZ* point = static_cast<VertexPointXYZ*>(_vertices[1]);

  Vector3 perr = cache->w2n() * point->estimate();

  // error, which is backwards from the normal observed - calculated
  // _measurement is the measured projection
  _error = perr - _measurement;
  //    std::cout << _error << std::endl << std::endl;
}

void EdgeSE3PointXYZ::linearizeOplus() {
  // VertexSE3 *cam = static_cast<VertexSE3 *>(_vertices[0]);
  VertexPointXYZ* vp = static_cast<VertexPointXYZ*>(_vertices[1]);

  Vector3 Zcam = cache->w2l() * vp->estimate();

  //  J(0,3) = -0.0;
  J(0, 4) = -2 * Zcam(2);
  J(0, 5) = 2 * Zcam(1);

  J(1, 3) = 2 * Zcam(2);
  //  J(1,4) = -0.0;
  J(1, 5) = -2 * Zcam(0);

  J(2, 3) = -2 * Zcam(1);
  J(2, 4) = 2 * Zcam(0);
  //  J(2,5) = -0.0;

  J.block<3, 3>(0, 6) = cache->w2l().rotation();

  Eigen::Matrix<number_t, 3, 9, Eigen::ColMajor> Jhom =
      offsetParam->inverseOffset().rotation() * J;

  _jacobianOplusXi = Jhom.block<3, 6>(0, 0);
  _jacobianOplusXj = Jhom.block<3, 3>(0, 6);

  // std::cerr << "just linearized." << std::endl;
  // std::cerr << "_jacobianOplusXi:" << std::endl << _jacobianOplusXi <<
  // std::endl; std::cerr << "_jacobianOplusXj:" << std::endl <<
  // _jacobianOplusXj << std::endl;
}

}  // namespace g2o

六、EdgeSE3ProjectXYZ : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap>

//*** g2o源码 g2o/g2o/types/sba/edge_project_xyz.h ***//
#ifndef G2O_SBA_EDGEPROJECTXYZ_H
#define G2O_SBA_EDGEPROJECTXYZ_H

#include "g2o/core/base_binary_edge.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"
#include "g2o_types_sba_api.h"
#include "vertex_se3_expmap.h"

namespace g2o {

// Projection using focal_length in x and y directions
class G2O_TYPES_SBA_API EdgeSE3ProjectXYZ
    : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EdgeSE3ProjectXYZ();

  bool read(std::istream &is);
  bool write(std::ostream &os) const;
  void computeError();
  bool isDepthPositive();

  virtual void linearizeOplus();
  Vector2 cam_project(const Vector3 &trans_xyz) const;

  number_t fx, fy, cx, cy;
};

}  // namespace g2o

#endif

//*** g2o源码 g2o/g2o/types/sba/edge_project_xyz.h ***//
include "edge_project_xyz.h"

namespace g2o {

EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ()
    : BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap>() {}

bool EdgeSE3ProjectXYZ::read(std::istream &is) {
  internal::readVector(is, _measurement);
  return readInformationMatrix(is);
}

bool EdgeSE3ProjectXYZ::write(std::ostream &os) const {
  internal::writeVector(os, measurement());
  return writeInformationMatrix(os);
}

void EdgeSE3ProjectXYZ::computeError() {
  const VertexSE3Expmap *v1 =
      static_cast<const VertexSE3Expmap *>(_vertices[1]);
  const VertexPointXYZ *v2 = static_cast<const VertexPointXYZ *>(_vertices[0]);
  Vector2 obs(_measurement);
  _error = obs - cam_project(v1->estimate().map(v2->estimate()));
}

void EdgeSE3ProjectXYZ::linearizeOplus() {
  VertexSE3Expmap *vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
  SE3Quat T(vj->estimate());
  VertexPointXYZ *vi = static_cast<VertexPointXYZ *>(_vertices[0]);
  Vector3 xyz = vi->estimate();
  Vector3 xyz_trans = T.map(xyz);

  number_t x = xyz_trans[0];
  number_t y = xyz_trans[1];
  number_t z = xyz_trans[2];
  number_t z_2 = z * z;

  Eigen::Matrix<number_t, 2, 3> tmp;
  tmp(0, 0) = fx;
  tmp(0, 1) = 0;
  tmp(0, 2) = -x / z * fx;

  tmp(1, 0) = 0;
  tmp(1, 1) = fy;
  tmp(1, 2) = -y / z * fy;

  _jacobianOplusXi = -1. / z * tmp * T.rotation().toRotationMatrix();

  _jacobianOplusXj(0, 0) = x * y / z_2 * fx;
  _jacobianOplusXj(0, 1) = -(1 + (x * x / z_2)) * fx;
  _jacobianOplusXj(0, 2) = y / z * fx;
  _jacobianOplusXj(0, 3) = -1. / z * fx;
  _jacobianOplusXj(0, 4) = 0;
  _jacobianOplusXj(0, 5) = x / z_2 * fx;

  _jacobianOplusXj(1, 0) = (1 + y * y / z_2) * fy;
  _jacobianOplusXj(1, 1) = -x * y / z_2 * fy;
  _jacobianOplusXj(1, 2) = -x / z * fy;
  _jacobianOplusXj(1, 3) = 0;
  _jacobianOplusXj(1, 4) = -1. / z * fy;
  _jacobianOplusXj(1, 5) = y / z_2 * fy;
}

}  // namespace g2o

七、EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap>

测量值：平面2D点，顶点：空间3D点、空间3D位姿（SE3，李代数旋转）

3D-2D点的PnP问题，其中，测量值是是该空间3D点对应的图像2D坐标

//*** g2o源码 g2o/g2o/types/sba/edge_project_xyz2uv.h ***//
#ifndef G2O_SBA_EDGEPROJECTXYZ2UV_H
#define G2O_SBA_EDGEPROJECTXYZ2UV_H

#include "g2o/core/base_binary_edge.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"
#include "g2o_types_sba_api.h"
#include "parameter_cameraparameters.h"
#include "vertex_se3_expmap.h"

namespace g2o {

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV
    : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjectXYZ2UV();
  bool read(std::istream& is);
  bool write(std::ostream& os) const;
  void computeError();
  virtual void linearizeOplus();

 public:
  CameraParameters* _cam;  // TODO make protected member?
};

}  // namespace g2o

#endif

//*** g2o源码 g2o/g2o/types/sba/edge_project_xyz2uv.cpp ***//
#include "edge_project_xyz2uv.h"

namespace g2o {

EdgeProjectXYZ2UV::EdgeProjectXYZ2UV()
    : BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap>() {
  _cam = 0;
  resizeParameters(1);
  installParameter(_cam, 0);
}

bool EdgeProjectXYZ2UV::read(std::istream& is) {
  readParamIds(is);
  internal::readVector(is, _measurement);
  return readInformationMatrix(is);
}

bool EdgeProjectXYZ2UV::write(std::ostream& os) const {
  writeParamIds(os);
  internal::writeVector(os, measurement());
  return writeInformationMatrix(os);
}

void EdgeProjectXYZ2UV::computeError() {
  const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
  const VertexPointXYZ* v2 = static_cast<const VertexPointXYZ*>(_vertices[0]);
  const CameraParameters* cam =
      static_cast<const CameraParameters*>(parameter(0));
  // cam_map()：将相机坐标系下三维点（输入）用内参转换为像素坐标系下的图像坐标（输出）
  // map()：将世界坐标系下三维点转换到相机坐标系下
  // v1->estimate().map(v2->estimate()
  // 用 v1 估计的位姿（T_camera_world）把 v2 代表的三维点（P_world）转换到相机坐标系下
  _error = measurement() - cam->cam_map(v1->estimate().map(v2->estimate()));
}

void EdgeProjectXYZ2UV::linearizeOplus() {
  VertexSE3Expmap* vj = static_cast<VertexSE3Expmap*>(_vertices[1]);
  SE3Quat T(vj->estimate());
  VertexPointXYZ* vi = static_cast<VertexPointXYZ*>(_vertices[0]);
  Vector3 xyz = vi->estimate();
  Vector3 xyz_trans = T.map(xyz); //相机坐标系下的点

  number_t x = xyz_trans[0];
  number_t y = xyz_trans[1];
  number_t z = xyz_trans[2];
  number_t z_2 = z * z;

  const CameraParameters* cam =
      static_cast<const CameraParameters*>(parameter(0));

  // 重投影误差关于 三维点 的雅可比矩阵
  // g2o 中 SE3Quat 的定义方式是旋转在前，平移在后
  Eigen::Matrix<number_t, 2, 3, Eigen::ColMajor> tmp;
  tmp(0, 0) = cam->focal_length;
  tmp(0, 1) = 0;
  tmp(0, 2) = -x / z * cam->focal_length;

  tmp(1, 0) = 0;
  tmp(1, 1) = cam->focal_length;
  tmp(1, 2) = -y / z * cam->focal_length;

  _jacobianOplusXi = -1. / z * tmp * T.rotation().toRotationMatrix();

  // 重投影误差关于 相机位姿 的雅可比矩阵
  _jacobianOplusXj(0, 0) = x * y / z_2 * cam->focal_length;
  _jacobianOplusXj(0, 1) = -(1 + (x * x / z_2)) * cam->focal_length;
  _jacobianOplusXj(0, 2) = y / z * cam->focal_length;
  _jacobianOplusXj(0, 3) = -1. / z * cam->focal_length;
  _jacobianOplusXj(0, 4) = 0;
  _jacobianOplusXj(0, 5) = x / z_2 * cam->focal_length;

  _jacobianOplusXj(1, 0) = (1 + y * y / z_2) * cam->focal_length;
  _jacobianOplusXj(1, 1) = -x * y / z_2 * cam->focal_length;
  _jacobianOplusXj(1, 2) = -x / z * cam->focal_length;
  _jacobianOplusXj(1, 3) = 0;
  _jacobianOplusXj(1, 4) = -1. / z * cam->focal_length;
  _jacobianOplusXj(1, 5) = y / z_2 * cam->focal_length;
}

}  // namespace g2o

//*** g2o源码 g2o/g2o/types/sba/parameter_cameraparameters.cpp ***//
Vector2 CameraParameters::cam_map(const Vector3 &trans_xyz) const {
  Vector2 proj = project(trans_xyz);
  Vector2 res;
  res[0] = proj[0] * focal_length + principle_point[0];
  res[1] = proj[1] * focal_length + principle_point[1];
  return res;
}

//*** g2o源码 g2o/g2o/types/slam3d/se3quat.h ***//
Vector3 map(const Vector3 & xyz) const
{
  return _r*xyz + _t;
}

八、EdgeProjectXYZ2UVU : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSE3Expmap>

//*** g2o源码 g2o/g2o/types/sba/edge_project_xyz2uvu.h ***//
#ifndef G2O_SBA_EDGEPROJECTXYZ2UVU_H
#define G2O_SBA_EDGEPROJECTXYZ2UVU_H

#include "g2o/core/base_binary_edge.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"
#include "g2o_types_sba_api.h"
#include "parameter_cameraparameters.h"
#include "vertex_se3_expmap.h"

namespace g2o {

// Stereo Observations
// U: left u
// V: left v
// U: right u
class G2O_TYPES_SBA_API EdgeProjectXYZ2UVU
    : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSE3Expmap> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjectXYZ2UVU();
  bool read(std::istream& is);
  bool write(std::ostream& os) const;
  void computeError();
  //  virtual void linearizeOplus();
 protected:
  CameraParameters* _cam;
};

}  // namespace g2o

#endif

//*** g2o源码 g2o/g2o/types/sba/edge_project_xyz2uvu.cpp ***//
#include "edge_project_xyz2uvu.h"

namespace g2o {

EdgeProjectXYZ2UVU::EdgeProjectXYZ2UVU()
    : BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSE3Expmap>() {
  _cam = nullptr;
  resizeParameters(1);
  installParameter(_cam, 0);
}

void EdgeProjectXYZ2UVU::computeError() {
  const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
  const VertexPointXYZ* v2 = static_cast<const VertexPointXYZ*>(_vertices[0]);
  const CameraParameters* cam =
      static_cast<const CameraParameters*>(parameter(0));
  _error = measurement() -
           cam->stereocam_uvu_map(v1->estimate().map(v2->estimate()));
}

bool EdgeProjectXYZ2UVU::read(std::istream& is) {
  readParamIds(is);
  internal::readVector(is, _measurement);
  return readInformationMatrix(is);
}

bool EdgeProjectXYZ2UVU::write(std::ostream& os) const {
  writeParamIds(os);
  internal::writeVector(os, measurement());
  return writeInformationMatrix(os);
}

}  // namespace g2o

九、EdgeStereoSE3ProjectXYZ : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSE3Expmap>

//*** g2o源码 g2o/g2o/types/sba/edge_project_stereo_xyz.h ***//
#ifndef G2O_SBA_EDGEPROJECTSTEREOXYZ_H
#define G2O_SBA_EDGEPROJECTSTEREOXYZ_H

#include "g2o/core/base_binary_edge.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"
#include "g2o_types_sba_api.h"
#include "vertex_se3_expmap.h"

namespace g2o {

// Projection using focal_length in x and y directions stereo
class G2O_TYPES_SBA_API EdgeStereoSE3ProjectXYZ
    : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSE3Expmap> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EdgeStereoSE3ProjectXYZ();

  bool read(std::istream &is);

  bool write(std::ostream &os) const;

  void computeError() {
    const VertexSE3Expmap *v1 =
        static_cast<const VertexSE3Expmap *>(_vertices[1]);
    const VertexPointXYZ *v2 =
        static_cast<const VertexPointXYZ *>(_vertices[0]);
    Vector3 obs(_measurement);
    _error = obs - cam_project(v1->estimate().map(v2->estimate()), bf);
  }

  bool isDepthPositive() {
    const VertexSE3Expmap *v1 =
        static_cast<const VertexSE3Expmap *>(_vertices[1]);
    const VertexPointXYZ *v2 =
        static_cast<const VertexPointXYZ *>(_vertices[0]);
    return (v1->estimate().map(v2->estimate()))(2) > 0;
  }

  virtual void linearizeOplus();

  Vector3 cam_project(const Vector3 &trans_xyz, const float &bf) const;

  number_t fx, fy, cx, cy, bf;
};

}  // namespace g2o

#endif

//*** g2o源码 g2o/g2o/types/sba/edge_project_stereo_xyz.cpp ***//
#include "edge_project_stereo_xyz.h"

namespace g2o {

EdgeStereoSE3ProjectXYZ::EdgeStereoSE3ProjectXYZ()
    : BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSE3Expmap>() {}

Vector3 EdgeStereoSE3ProjectXYZ::cam_project(const Vector3 &trans_xyz,
                                             const float &bf) const {
  const number_t invz = 1.0f / trans_xyz[2];
  Vector3 res;
  res[0] = trans_xyz[0] * invz * fx + cx;
  res[1] = trans_xyz[1] * invz * fy + cy;
  res[2] = res[0] - bf * invz;
  return res;
}

bool EdgeStereoSE3ProjectXYZ::read(std::istream &is) {
  internal::readVector(is, _measurement);
  return readInformationMatrix(is);
}

bool EdgeStereoSE3ProjectXYZ::write(std::ostream &os) const {
  internal::writeVector(os, measurement());
  return writeInformationMatrix(os);
}

void EdgeStereoSE3ProjectXYZ::linearizeOplus() {
  VertexSE3Expmap *vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
  SE3Quat T(vj->estimate());
  VertexPointXYZ *vi = static_cast<VertexPointXYZ *>(_vertices[0]);
  Vector3 xyz = vi->estimate();
  Vector3 xyz_trans = T.map(xyz);

  const Matrix3 R = T.rotation().toRotationMatrix();

  number_t x = xyz_trans[0];
  number_t y = xyz_trans[1];
  number_t z = xyz_trans[2];
  number_t z_2 = z * z;

  _jacobianOplusXi(0, 0) = -fx * R(0, 0) / z + fx * x * R(2, 0) / z_2;
  _jacobianOplusXi(0, 1) = -fx * R(0, 1) / z + fx * x * R(2, 1) / z_2;
  _jacobianOplusXi(0, 2) = -fx * R(0, 2) / z + fx * x * R(2, 2) / z_2;

  _jacobianOplusXi(1, 0) = -fy * R(1, 0) / z + fy * y * R(2, 0) / z_2;
  _jacobianOplusXi(1, 1) = -fy * R(1, 1) / z + fy * y * R(2, 1) / z_2;
  _jacobianOplusXi(1, 2) = -fy * R(1, 2) / z + fy * y * R(2, 2) / z_2;

  _jacobianOplusXi(2, 0) = _jacobianOplusXi(0, 0) - bf * R(2, 0) / z_2;
  _jacobianOplusXi(2, 1) = _jacobianOplusXi(0, 1) - bf * R(2, 1) / z_2;
  _jacobianOplusXi(2, 2) = _jacobianOplusXi(0, 2) - bf * R(2, 2) / z_2;

  _jacobianOplusXj(0, 0) = x * y / z_2 * fx;
  _jacobianOplusXj(0, 1) = -(1 + (x * x / z_2)) * fx;
  _jacobianOplusXj(0, 2) = y / z * fx;
  _jacobianOplusXj(0, 3) = -1. / z * fx;
  _jacobianOplusXj(0, 4) = 0;
  _jacobianOplusXj(0, 5) = x / z_2 * fx;

  _jacobianOplusXj(1, 0) = (1 + y * y / z_2) * fy;
  _jacobianOplusXj(1, 1) = -x * y / z_2 * fy;
  _jacobianOplusXj(1, 2) = -x / z * fy;
  _jacobianOplusXj(1, 3) = 0;
  _jacobianOplusXj(1, 4) = -1. / z * fy;
  _jacobianOplusXj(1, 5) = y / z_2 * fy;

  _jacobianOplusXj(2, 0) = _jacobianOplusXj(0, 0) - bf * y / z_2;
  _jacobianOplusXj(2, 1) = _jacobianOplusXj(0, 1) + bf * x / z_2;
  _jacobianOplusXj(2, 2) = _jacobianOplusXj(0, 2);
  _jacobianOplusXj(2, 3) = _jacobianOplusXj(0, 3);
  _jacobianOplusXj(2, 4) = 0;
  _jacobianOplusXj(2, 5) = _jacobianOplusXj(0, 5) - bf / z_2;
}

}  // namespace g2o