目录:
- 对极约束
- PnP问题
- ICP问题
- Gauss-Newton法
- g2o求解最小二乘问题
对极约束
1.求解基础矩阵/本质矩阵
基础矩阵F
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat(points1, points2, FM_8POINT);
注意findFundamentalMat()函数的参数:
参数1,2:points_1;cv类型的中的特征点1,2容器类,像素坐标。
参数3:为求解方法,有8点法等等,为一参数。
返回值为mat矩阵。
本质矩阵E
Point2d principal_point(325.1, 249.7); //相机光心, TUM dataset标定值
double focal_length = 521;
//相机焦距, TUM dataset标定值
Mat essential_matrix;
essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
参数1,2与基础矩阵求解一样。
参数3,4分别为相机光心与焦距值。
返回值为mat矩阵。
2.求解R,t
recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
//求解R,t
recoverPose(函数有7个参数):
参数1:本质矩阵E;
参数2,3:特征点1,2的容器类,像素坐标
参数4,5:R,t矩阵,mat类型
参数6,7:焦距,光心值
3.三角测量(三角化)
Mat pts_4d;
cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);
// 转换成非齐次坐标
for (int i = 0; i < pts_4d.cols; i++) {
Mat x = pts_4d.col(i);
x /= x.at<float>(3, 0); // 归一化
Point3d p(
x.at<float>(0, 0),
x.at<float>(1, 0),
x.at<float>(2, 0)
);
points.push_back(p);
}
注:triangulatePoints()一共有5个参数
参数1,2:为变换矩阵,一般T1为单位矩阵,因为没有变换,Mat类型
参数3,4: 为特征点,为Point2d类,为归一化坐标系的坐标
参数5:存储求解的结果,为一个4×n的矩阵,每一列存储一个点的结果,但存储的是其次坐标,需要归一化得到相机坐标(X,Y,Z),实际使用时,把Z提取出来即可。
PnP问题
Mat r, t;
solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);
Mat R;
cv::Rodrigues(r, R);
// r为旋转向量形式,用Rodrigues公式转换为矩阵
// 调用OpenCV 的 PnP 求解,可选择EPNP,DLS等方法
solvePnP()有7个参数:
参数1:三维点的P的坐标(X,Y,Z)相机坐标系,实际值,不是归一化坐标系,point容器类型。
参数2:二维点的像素坐标,point容器类型。
参数3:相机内参矩阵,mat类
参数4:畸变参数,没有的化设置问哦Mat()即可
参数5,6:r旋转向量,t平移向量;mat类
参数7:默认false吧
ICP问题
1.求解特征点的质心
Point3f p1, p2; // 质心坐标
int N = pts1.size();
for (int i = 0; i < N; i++) {
p1 += pts1[i];
p2 += pts2[i];
}
p1 = Point3f(Vec3f(p1) / N);
p2 = Point3f(Vec3f(p2) / N);
2.去质心化
vector<Point3f> q1(N), q2(N);
// 去质心化
for (int i = 0; i < N; i++) {
q1[i] = pts1[i] - p1;
q2[i] = pts2[i] - p2;
}
3.计算W矩阵
// compute q1*q2^T
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for (int i = 0; i < N; i++) {
W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
}
cout << "W=" << W << endl;
4.对W进行SVD分解
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
5.求解R,t
Eigen::Matrix3d R_ = U * (V.transpose());
if (R_.determinant() < 0) {
R_ = -R_;
}
Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);
Gauss-Newton法
以下是PnP问题的gauss-newton优化方法:
void bundleAdjustmentGaussNewton(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3d &pose) {
typedef Eigen::Matrix<double, 6, 1> Vector6d;
const int iterations = 10;
double cost = 0, lastCost = 0;
double fx = K.at<double>(0, 0);
double fy = K.at<double>(1, 1);
double cx = K.at<double>(0, 2);
double cy = K.at<double>(1, 2);
for (int iter = 0; iter < iterations; iter++) {
Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
//定义H矩阵
Vector6d b = Vector6d::Zero();
cost = 0;
// 计算cost
for (int i = 0; i < points_3d.size(); i++) {
Eigen::Vector3d pc = pose * points_3d[i];
double inv_z = 1.0 / pc[2];
double inv_z2 = inv_z * inv_z;
Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);
Eigen::Vector2d e = points_2d[i] - proj;
cost += e.squaredNorm();
Eigen::Matrix<double, 2, 6> J;
J << -fx * inv_z,
0,
fx * pc[0] * inv_z2,
fx * pc[0] * pc[1] * inv_z2,
-fx - fx * pc[0] * pc[0] * inv_z2,
fx * pc[1] * inv_z,
0,
-fy * inv_z,
fy * pc[1] * inv_z2,
fy + fy * pc[1] * pc[1] * inv_z2,
-fy * pc[0] * pc[1] * inv_z2,
-fy * pc[0] * inv_z;
H += J.transpose() * J; //计算H,注意J是2×6的矩阵 J.transpose()*J为6×6的矩阵
b += -J.transpose() * e; //计算b,b为2×1的矩阵
}
Vector6d dx;
dx = H.ldlt().solve(b); //求解增量方程
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}//判断迭代值是否为无穷
if (iter > 0 && cost >= lastCost) {
cout << "cost: " << cost << ", last cost: " << lastCost << endl;
break;
}//判断cost的增加方向,理论上是越来越小;
pose = Sophus::SE3d::exp(dx) * pose; //更新位姿,从而更新误差e
lastCost = cost;
cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
if (dx.norm() < 1e-6) {
// converge
break;
}//当增增量很小便停止迭代
}
g2o求解最小二乘问题
g2o求解最小二乘法问题最小分为三个部分:
- 编写Vertex类(优化变量)
- 编写Edge类(误差函数)
- 配置optimazer
以求解PnP问题为例:
Vertex类的主要工作有:
1.编写setToOriginImpl()函数:初始化变量_estimate.
2.编写oplusImpl()函数:设置增量的更新表达式。
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void setToOriginImpl() override {
_estimate = Sophus::SE3d();
}
/// left multiplication on SE3
virtual void oplusImpl(const double *update) override {
Eigen::Matrix<double, 6, 1> update_eigen;
update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
}
virtual bool read(istream &in) override { return true; }
virtual bool write(ostream &out) const override { return true; }
};
注意:
1.BaseVertex<6,Sophus::SE3d>代表优化变量的维度为6,优化变量的类型为Sophs::SE3d;SE3d的维度可以看作是6维
2.oplusImpl(const double *update)中的update应该是一个数组指针,存储着更新量,实际使用的时候需要转换为相应的类型。(不能之间与数组进行运算)
Edge类的主要工作有:
1.编写 computeError()函数:误差计算表达式
2.编写linearizeOplus()函数:输入J矩阵
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}
virtual void computeError() override {
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
pos_pixel /= pos_pixel[2];
_error = _measurement - pos_pixel.head<2>();
}
virtual void linearizeOplus() override {
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d pos_cam = T * _pos3d;
double fx = _K(0, 0);
double fy = _K(1, 1);
double cx = _K(0, 2);
double cy = _K(1, 2);
double X = pos_cam[0];
double Y = pos_cam[1];
double Z = pos_cam[2];
double Z2 = Z * Z;
_jacobianOplusXi
<< -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
}
virtual bool read(istream &in) override { return true; }
virtual bool write(ostream &out) const override { return true; }
private:
Eigen::Vector3d _pos3d;
Eigen::Matrix3d _K;
};
注:
1.computeError()函数中,_error,_measurement变量均为继承的变量,类型为Eigen的向量。其中,_measurement为测量值,需要类外输入。
2.linearizeOplus()的核心是给_jacobianOplusXi矩阵赋值,关于_jacobianOplusXi矩阵的结构如下:
设误差
e
(
x
)
=
[
e
1
e
2
]
e(x)=\begin{bmatrix} e_1 \\ e_2 \end{bmatrix}\quad
e(x)=[e1e2]优化变量
x
=
[
u
v
w
]
x=\begin{bmatrix} u \\ v\\w \end{bmatrix}\quad
x=⎣⎡uvw⎦⎤
则 J 1 = [ d e 1 d u d e 2 d u ] J_1=\begin{bmatrix} \frac{\mathrm{d}e_1}{\mathrm{d}u}\\ \frac{\mathrm{d}e_2}{\mathrm{d}u}\\ \end{bmatrix}\quad J1=[dude1dude2] J 2 = [ d e 1 d v d e 2 d v ] J_2=\begin{bmatrix} \frac{\mathrm{d}e_1}{\mathrm{d}v}\\ \frac{\mathrm{d}e_2}{\mathrm{d}v}\\ \end{bmatrix}\quad J2=[dvde1dvde2] J 1 = [ d e 1 d w d e 2 d w ] J_1=\begin{bmatrix} \frac{\mathrm{d}e_1}{\mathrm{d}w}\\ \frac{\mathrm{d}e_2}{\mathrm{d}w}\\ \end{bmatrix}\quad J1=[dwde1dwde2]
J = [ J 1 J 2 J 3 ] J=\begin{bmatrix} J_1&J_2&J_3\end{bmatrix}\quad J=[J1J2J3]
因此,整个J矩阵的排布是列向量安装一定顺序排列的,所以对于实际的_jacobianOplusXi矩阵也常常安装这种规律依次赋值,在g2o中常用采用以下分块矩阵的形式:
_jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
<3,3>代表分块的大小,(0,x)代表从第x列分块的分界点。
_jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
或者像以下形式:
_jacobianOplusXi.block<2,3>(0,0) = dp_dE;
_jacobianOplusXi.block<2,3>(0,3) = de_dp;
_jacobianOplusXi.block<2,1>(0,6) = de_df;
_jacobianOplusXi.block<2,1>(0,7) = de_dk1;
_jacobianOplusXi.block<2,1>(0,8) = de_dk2;
3.对于class EdgeProjection : publicg2o::BaseUnaryEdge<2,Eigen::Vector2d,VertexPose>;
2表示error的维度,
Eigen::Vector2d表示error的类型,
VertexPose表示定义边所连接的顶点(有可能有多个)
配置optimazer
具体工作如下:
1.定义求解器(solver):设置求解类型,变量个数,求解算法等等
2.定义优化器(optimazer):定义一个optimazer
3.设置Vertex:类初始化,赋初值,设置顶点的个数等
4.设置Edge:类初始化,传递_measurement参数等等。
5.设置optimazer
代码如下:
void bundleAdjustmentG2O(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3d &pose) {
// 构建图优化,先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 2>> BlockSolverType; // 优化变量6个,误差e的维度为2
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法,可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// 设置Vetex
VertexPose *vertex_pose = new VertexPose(); // 空的构造函数
vertex_pose->setId(0); //设置Vertex的个数
vertex_pose->setEstimate(Sophus::SE3d()); //初始化
optimizer.addVertex(vertex_pose);
// K
Eigen::Matrix3d K_eigen;
K_eigen <<
K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);
// 设置Edge
int index = 1;
for (size_t i = 0; i < points_2d.size(); ++i) {
auto p2d = points_2d[i];
auto p3d = points_3d[i];
EdgeProjection *edge = new EdgeProjection(p3d, K_eigen); //构造函数初始化类
edge->setId(index); //Edge的个数
edge->setVertex(0, vertex_pose); //与顶点的连接
edge->setMeasurement(p2d); //赋值给
edge->setInformation(Eigen::Matrix2d::Identity()); //初始化
optimizer.addEdge(edge);
index++;
}
optimizer.setVerbose(true); // 打开调试输出
optimizer.initializeOptimization(); //optimazer的初始化
optimizer.optimize(10); //iteration的数量
pose = vertex_pose->estimate(); //返回位姿
}
补充:
1.当具有多个类型的vertex时,g2o中必须把每个vertex添加到edge中,即顶点与边连接,一般做法是定义vertex容器。
2.每次设置vertex的指针必须保存起来,一般是放到前面定义的容器中,因为后面要把这个指针添加到edge中。
3.g2o中的vertex是连续的,每一个vertex只能占用一个Id,一般是把一类顶点放在一串Id中。
4.g2o中每一条edge中都有一个vertice容器,里面存储这对条边所连接顶点的指针。注意区分2中的容器。
vector<camera_vertex*> cameravertex;
vector<point_vertex*> pointvertex;
//上面是储存vertex的容器,这里一共有两类顶点:camera_vertex和point_vertex
//设置camera_vertex,因为有很多vertex和edge,所以用for语句
for (int i = 0; i < bal_problem.num_cameras(); i++)
{
camera_vertex* v = new camera_vertex(); //必须要加上read和write那几个虚函数
v->setId(i);
//从0-bal_problem.num_cameras()-1都是camera_vertex;
double* camera = cameras + camera_block_size * i;
v->setEstimate(pose_and_intrincis(camera));
optimazer.addVertex(v);
cameravertex.push_back(v); //保存好指针
}
for (int i = 0; i < bal_problem.num_points(); i++)
{
point_vertex* v = new point_vertex();
v->setId(i+bal_problem.num_cameras());
//从 bal_problem.num_cameras()至bal_problem.num_cameras()+bal_problem.num_points()都是point_vertex。
double* point = points + point_block_size * i;
v->setEstimate(Vector3d(point[0],point[1],point[2]));
optimazer.addVertex(v);
pointvertex.push_back(v); //保存好指针
v->setMarginalized(true);
}
//注意,vertex中的指针v必须存储起来,然后添加到后面的edg里面去
//以下是添加边
for (int i = 0; i < bal_problem.num_observations(); i++)
{
edg_projection* edge = new edg_projection();
edge->setVertex(0, cameravertex[bal_problem.camera_index()[i]]);
edge->setVertex(1, pointvertex[bal_problem.point_index()[i]]);
//上面是把保存的指针添加到对于的edge中,
//0,1代表定义edge是vertex的顺序,在这里,0代表camera_vertex,1代表point_vertex
edge->setMeasurement(Vector2d(observations[2 * i + 0], observations[2 * i + 1]));
edge->setInformation(Matrix2d::Identity());
edge->setRobustKernel(new g2o::RobustKernelHuber());
optimazer.addEdge(edge);
}
给出g2o设置的结构图(仅仅求解单顶点的非线性最小二乘问题)
补充: 关于g2o定义求解器的三步:
1.定义BlockSolver
2.定义求解算法类型
3.定义optimazer
//第一步:定义blocksover
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
//这是定义BlockSolver的类型
typedef g2o::LinearSolverEigen <BlockSolverType::PoseMatrixType> LinearSolverType;
//这是定义在定义类型的基础上,定义一种求解器的类型
//第二步:定义求解算法
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
);//这里使用L-M算法,注意看图,算法是继承于第一步所中定义各种类型
//第三步:定义优化器
g2o::SparseOptimizer optimazer;
optimazer.setAlgorithm(solver);//同样,注意看图,优化器继承于算法
这三步有明显的继承关系,从图中可以看出,3继承2继承1。
参考文献:
[1]视觉SLAM十四讲,高翔等
[2]https://blog.csdn.net/johnnyyeh/article/details/82315543
[3]https://blog.csdn.net/qq_34935373/article/details/93898626