https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247487082&idx=1&sn=d4a27e4c9a76760fffb571f57f4f7719&chksm=97d7ebfda0a062eba412877e9ecf5933f2051f0210c0d56f03267985512d97f2db434ab7356c&cur_album_id=1361700104461467649&scene=189#wechat_redirect
一元边你可以理解为一条边只连接一个顶点
两元边理解为一条边连接两个顶点
多元边理解为一条边可以连接多个（3个以上）顶点
一元一个点
曲线优化
三个参数，打包[a,b,c] 算一个点，没有[a1,b1,c1] [ai,bi,ci]多个参数
一元多个点
位姿优化或者三维点优化
1元[Rt] 或 [x,y,z] 打包算一个点
有多个点（多个优化位姿或者三维点）
二元多个点
元1 位姿 多个位姿
元2 三维点 多个点
函数
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
参数D, E, VertexXi, VertexXj，
D 是 int 型，表示测量值的维度 （dimension）
E 表示测量值的数据类型
VertexXi，VertexXj 分别表示不同顶点的类型
函数例子说明
三维点投影到图像平面的重投影误差
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
参数1 纬度 2维
参数2 类型 Vector2D 两个顶点
参数3 两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap
函数
virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; virtual void computeError(); virtual void linearizeOplus();
read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
computeError函数：非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差
linearizeOplus函数：非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian
除了上面几个成员函数，还有几个重要的成员变量和函数也一并解释一下：
_measurement：存储观测值 _error：存储computeError() 函数计算的误差 _vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1） setId(int)：来定义边的编号（决定了在H矩阵中的位置） setMeasurement(type) 函数来定义观测值 setVertex(int, vertex) 来定义顶点 setInformation() 来定义协方差矩阵的逆
例子0 通用模型 一元边
class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
myEdge(){}
virtual bool read(istream& in) {}
virtual bool write(ostream& out) const {}
virtual void computeError() override
{
// ...
_error = _measurement - Something;
}
virtual void linearizeOplus() override
{
_jacobianOplusXi(pos, pos) = something;
// ...
/*
_jocobianOplusXj(pos, pos) = something;
...
*/
}
private:
// data
}
最重要的就是
误差计算 computeError()，
雅克比方程 linearizeOplus()两个函数了
例子1 曲线拟合 一元边
参数1 纬度 1维
参数2 类型 double
定义
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差，测量值减去估计值得到误差。
virtual void computeError() override {
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}
// 计算雅可比矩阵，和上一篇高斯牛顿法里面的求解方式是一样的。
virtual void linearizeOplus() override {
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
_jacobianOplusXi[0] = -_x * _x * y;
_jacobianOplusXi[1] = -_x * y;
_jacobianOplusXi[2] = -y;
}
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
public:
double _x; // x 值， y 值为 _measurement
};
例子2 3D-2D点的PnP 最小化重投影误差问题 二元边
二元边 定义
//继承了BaseBinaryEdge类，观测值是2维，类型Vector2D,顶点分别是三维点、李群位姿
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
//1. 默认初始化
EdgeProjectXYZ2UV();
//2. 计算误差
void computeError() {
//李群相机位姿v1
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
// 顶点v2
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
//相机参数
const CameraParameters * cam
= static_cast<const CameraParameters *>(parameter(0));
//误差计算，测量值减去估计值，也就是重投影误差obs-cam
//估计值计算方法是T*p,得到相机坐标系下坐标，然后在利用camera2pixel()函数得到像素坐标。
Vector2D obs(_measurement);
_error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
}
//3. 线性增量函数，也就是雅克比矩阵J的计算方法
virtual void linearizeOplus();
//4. 相机参数
CameraParameters * _cam;
bool read(std::istream& is);
bool write(std::ostream& os) const;
};
解析理解
v1->estimate().map(v2->estimate())
用V1估计的pose把V2代表的三维点，变换到相机坐标系三维点下。
_error = obs - cam->cam_map(v1->estimate().map(v2->estimate()));
cam_map函数，它的定义在
g2o/types/sba/types_six_dof_expmap.cpp
cam_map 函数功能是把相机坐标系下三维点（输入）用内参转换为图像坐标（输出），具体代码如下所示
Vector2 CameraParameters::cam_map(const Vector3 & trans_xyz) const {
Vector2 proj = project2d(trans_xyz);
Vector2 res;
res[0] = proj[0]*focal_length + principle_point[0];
res[1] = proj[1]*focal_length + principle_point[1];
return res;
}
.map函数，它的功能是把世界坐标系下三维点变换到相机坐标系，函数在
g2o/types/sim3/sim3.h
Vector3 map (const Vector3& xyz) const {
return s*(r*xyz) + t;
}
添加边
例子1 曲线拟合添加一元边
// 往图中增加边
for ( int i=0; i<N; i++ )
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge( edge );
}
例子2 二元边添加
这个例子比刚才的复杂一点，因为它是二元边，需要用边连接两个顶点
index = 1;
for ( const Point2f p:points_2d )
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setId ( index );
edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) ); 链接点1 三维点xyz
edge->setVertex ( 1, pose ); 链接点2 位姿Rt
edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) ); 观测值
edge->setParameterId ( 0,0 );
edge->setInformation ( Eigen::Matrix2d::Identity() );
optimizer.addEdge ( edge );
index++;
}
这里的setMeasurement函数里的p来自向量points_2d，也就是特征点的图像坐标(x,y)
0和1对应节点
g2o::EdgeProjectXYZ2UV
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV ..... //李群相机位姿v1 const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]); // 顶点v2 const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
_vertices[0] 对应的是 VertexSBAPointXYZ 类型的顶点，也就是三维点，_vertices[1] 对应的是VertexSE3Expmap 类型的顶点，也就是位姿pose。
标签：const,函数,virtual,edge,顶点,g2o,位姿 From： https://www.cnblogs.com/gooutlook/p/17835089.html