本例子可以当作一个多视图三角化的入门小例子,结合项目和slam十四讲ch7的学习,以及参考了https://zhaoxuhui.top/blog/2018/04/10/g2o&bundle_adjustment.html
以及https://github.com/zhaoxuhui/bundle-adjustment
现在拍摄了8张照片,照片里面包含了一个aruco mark board以及一个marker,
那么可以直接用mark 板估计相机位姿 std::vector<cv::Mat> transformationMatrix_Vector;,从而得到相机在mark板坐标系的位姿,以后就把mark板当作世界坐标系。
也可以用cv::aruco::DICT_ARUCO_ORIGINAL检测出那个单独的mark的中心的像素坐标
detect_board_one(camMatrix, distCoeffs, photo_1, transformationMatrix_Vector.at(0), center_vector.at(0));
detect_board_one(camMatrix, distCoeffs, photo_2, transformationMatrix_Vector.at(1), center_vector.at(1));
detect_board_one(camMatrix, distCoeffs, photo_3, transformationMatrix_Vector.at(2), center_vector.at(2));
detect_board_one(camMatrix, distCoeffs, photo_4, transformationMatrix_Vector.at(3), center_vector.at(3));
好了,现在我们有了相机外参T,相机内参已知,目标点(mark中心)的像素坐标
我们就可以用opencv的三角化求目标点的3D位置了,
cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);
其中T1为第一张图片的相机位姿,T2为第二张相机位姿,pts_1为目标点在第一张图片的归一化平面位置,就是把我们得到的像素坐标转到归一化平面,pts_2为目标点在第2张图片,pts_4d就是我们需要得到的3D位置,不过需要把最后一维归一后,取前三维数。
结果如下
手动三角化的结果RESULTRESULTRESULTRESULTRESULTRESULTRESULT:
[0.294733, 0.17403, 0.000187977]
此3d坐标是mark中心在mark板坐标系下的位置。
前面只用了前2张图片做了三角化,现在我们需要综合8张图片信息用g2o库优化,
函数需要一个相机位姿T数组、目标点像素坐标points_2d数组、相机内参K、需要估计的目标点3D位置XYZ。
void bundleAdjustmentG2O(
const VecSE3d& T,
const VecVector2d& points_2d,
const Mat& K,
Eigen::Vector3d& XYZ)
{
// 构建图优化,先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 6>> BlockSolverType; // pose is 6, landmark is 3
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); // 打开调试输出
// vertex //[0.294733, 0.17403, 0.000187977]真实值
//Eigen::Vector3d initialEstimate = Eigen::Vector3d::UnitX(); // 单位向量 (1, 0, 0) NO
//Eigen::Vector3d initialEstimate = Eigen::Vector3d (1, 1, 1); // OK
Eigen::Vector3d initialEstimate = Eigen::Vector3d(0, 0, 0); // OK
摄像头位姿节点添加
for (size_t i = 0; i < T.size(); ++i)
{
auto Ti = T[i];
// 创建一个g2o::SE3Quat对象
g2o::SE3Quat g2o_se3quat;
// 从Sophus::SE3d对象中提取平移向量和旋转矩阵
Eigen::Vector3d translation = Ti.translation();
Eigen::Matrix3d rotation = Ti.rotationMatrix();
// 使用提取的信息填充g2o::SE3Quat对象
g2o_se3quat.setRotation(Eigen::Quaterniond(rotation));
g2o_se3quat.setTranslation(translation);
g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
v->setId(i+1);
v->setFixed(true);
v->setEstimate(g2o_se3quat);
optimizer.addVertex(v);
}
/// <summary>
/// 特征点节点添加
/// </summary>
/// <param name="T"></param>
/// <param name="points_2d"></param>
/// <param name="K"></param>
/// <param name="XYZ"></param>
g2o::VertexSBAPointXYZ* vertex_pose = new g2o::VertexSBAPointXYZ();
vertex_pose->setId(0);
vertex_pose->setMarginalized(false);
vertex_pose->setEstimate(initialEstimate);
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);
///摄像头参数设置
//相机内参
g2o::CameraParameters* camera = new g2o::CameraParameters(K.at<double>(1, 1), Eigen::Vector2d(K.at<double>(0, 2), K.at<double>(1, 2)), 0);
camera->setId(0);
optimizer.addParameter(camera);
// edges
int index = 1;
for (size_t i = 0; i < points_2d.size(); ++i)
{
Eigen::Vector2d p2d = points_2d[i];
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(0)));
edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(i + 1)));
edge->setMeasurement(p2d);
edge->setInformation(Eigen::Matrix2d::Identity());
edge->setParameterId(0, 0);
edge->setRobustKernel(new g2o::RobustKernelHuber());
optimizer.addEdge(edge);
index++;
}
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl << endl;
XYZ = vertex_pose->estimate();
}
函数核心是optimizer.addVertex(v);优化器添加了8个相机位姿节点、1个目标点3D位置、8个边也就是映射关系,其中 v->setFixed(true);我把8个相机位姿节点设置了不变量,其实我也想把相机位姿和3D点一起优化,但是我v->setFixed(false);程序就会出错,目前不知道原因。
最终的效果如上图,两种结果欧式距离大概在1mm
pose estimated by g2o =
0.293432
0.172331
0.00136134
手动三角化的结果RESULTRESULTRESULTRESULTRESULTRESULTRESULT:
[0.294733, 0.17403, 0.000187977]
本工程已经上传github,地址为https://github.com/applefb/little-sfm
依赖opencv contrib 4.6.0,需要viz模块
以及g2o库,建议用vcpkg安装,比自己cmake安装方便。