考虑n个三维空间点和它们的投影
和
，
第二个相机相对于第一个相机转换
现在已知
和
,求第二个相机相对于第一个相机转换
,
下面随机初始化一个,由
可以推导出
，但是有误差。
: public 表示 CurveFittingVertex 类从 g2o::BaseVertex<3, Eigen::Vector3d> 类中继承了所有的公有成员和保护成员。
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
这段代码中 CurveFittingEdge 类的构造函数，在创建 CurveFittingEdge 对象时会被自动调用。它接收一个 double 类型的参数 x，并使用 x 的值初始化成员变量 _x。同时，在初始化列表中调用父类 BaseUnaryEdge 的构造函数，确保 BaseUnaryEdge 成员变量正确地初始化。_x(x) 表示将传入的参数 x 赋值给类成员变量 _x。
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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(std::istream &in) {}
virtual bool write(std::ostream &out) const {}
public:
double _x; // x 值， y 值为 _measurement
};使用 override 关键字可以强制让编译器检查是否正确地覆盖了基类中的虚函数。
在 C++11 中，virtual 和 override 是两个关键字，virtual 关键字用于实现多态。override 关键字用于标识对基类虚函数的覆盖。
#include <iostream>
class Animal {
public:
virtual void speak() {
std::cout << "I'm an animal" << std::endl;
}
};
class Dog : public Animal {
public:
virtual void speak() override {
std::cout << "I'm a dog" << std::endl;
}
void bark() {
std::cout << "Woof!" << std::endl;
}
};
int main() {
Dog d;
d.speak(); // "I'm a dog"
d.bark(); // "Woof!"
return 0;
}CurveFittingEdge 继承自 BaseUnaryEdge 类，而 CurveFittingVertex 没有继承自其他类。在 C++ 中，继承自其他类的子类在构造时需要先调用父类的构造函数，以保证父类的数据成员和函数成员可以正确初始化。而没有继承自其他类的类，则可以省略构造函数初始化列表，使用默认的无参构造函数完成初始化。
CurveFittingEdge::CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}这里使用了模板和类型别名来定义 BlockSolver 和 LinearSolver 的类型。
其中，BlockSolverType 定义了误差项的优化变量维度为 3，误差值的维度为 1，而 LinearSolverType 定义了使用 BlockSolverType 的 PoseMatrixType 作为矩阵类型的线性求解器。PoseMatrixType 是用于描述优化变量的矩阵类型，它的大小和 BlockSolverTraits 模板参数中的维度有关。
