摄像机运动估算

目标

在本教程中，您将学习如何使用重建API进行摄像机运动估算

• 加载包含跟踪2D点的文件，并为所有帧构建容器。
• 运行libmv重建管道。
• 使用Viz显示获得的结果。

代码

#include <opencv2/core.hpp>
#include <opencv2/sfm.hpp>
#include <opencv2/viz.hpp>
 
#include <iostream>
#include <fstream>
 
using namespace std;
using namespace cv;
using namespace cv::sfm;
 
static void help() {
cout
<< "\n------------------------------------------------------------------\n"
<< " 该程序展示了OpenCV结构从运动（SFM）模块中摄像机轨迹重建的能力。\n"
<< " \n"
<< " 用法：\n"
<< " example_sfm_trajectory_reconstruction <path_to_tracks_file> <f> <cx> <cy>\n"
<< " 其中：是您的系统中跟踪文件路径的绝对路径。 \n"
<< " 此文件必须有以下格式： \n"
<< " 用法：\n"
<< " 行1：x1 y1 x2 y2 ... x36 y36，用于轨道1\n"
<< " 行2：x1 y1 x2 y2 ... x36 y36，用于轨道2\n"
<< " 等\n"
<< " 每行给出了点2D的测量位置，这些点是按照跟踪\n"
<< " 用法：\n"
<< " 在1到36帧之间。如果某视图没有找到匹配，那么x\n"
<< " 和y的值将是-1。\n"
<< " 每行对应不同的点。\n"
<< " 用法：\n"
<< " f是像素焦距。\n"
<< " 用法：\n"
<< " cx是图像主点x坐标的像素。\n"
<< " cy是图像主点y坐标的像素。\n"
<< "------------------------------------------------------------------\n\n"
<< endl;
<< endl;
}
 
 
/* 构建以下结构数据
 *
* frame1 frame2 frameN
* track1 | (x11,y11) | -> | (x12,y12) | -> | (x1N,y1N) |
* track2 | (x21,y11) | -> | (x22,y22) | -> | (x2N,y2N) |
* trackN | (xN1,yN1) | -> | (xN2,yN2) | -> | (xNN,yNN) |
 *
 *
* 如果在某个帧中没有出现标记（x，y），则其
* 值将是（-1，-1）。
 */
 
static void parser_2D_tracks(const String &_filename, std::vector<Mat> &points2d )
{
ifstream myfile(_filename.c_str());
 
  if (!myfile.is_open())
  {
cout << "无法读取文件： " << _filename << endl;
exit(0);
 
} else {
 
    double x, y;
    string line_str;
    int n_frames = 0, n_tracks = 0;
 
    // 从文本文件中提取数据
 
vector<vector<Vec2d> > tracks;
    for ( ; getline(myfile,line_str); ++n_tracks)
    {
istringstream行=istringstream(line_str);
 
vector轨迹;
      for (n_frames = 0; 行 >> x >> y; ++n_frames)
      {
        if ( x > 0 && y > 0)
轨迹.push_back(Vec2d(x,y));
        else
轨迹.push_back(Vec2d(-1));
      }
tracks.push_back(轨迹);
    }
 
    // 在重建API格式中嵌入数据
 
    for (int i = 0; i < n_frames; ++i)
    {
      Mat_<double>帧(2, n_tracks);
 
      for (int j = 0; j < n_tracks; ++j)
      {
帧(0,j) = 轨迹[j][i][0];
帧(1,j) = 轨迹[j][i][1];
      }
points2d.push_back(Mat(帧));
    }
 
myfile.close();
  }
 
}
 
 
/* 键盘回调以控制3D可视化
 */
 
bool摄像头视角 = false;
 
static void键盘回调(const viz::KeyboardEvent &事件, void* cookie)
{
  if (事件.操作 == 0 &&!事件.符号.compare("s") )
摄像头视角 = !摄像头视角;
}
 
 
/* 示例主代码
 */
 
int main(int argc, char** argv)
{
  // 读取输入参数
 
  if ( argc != 5 )
  {
帮助();
exit(0);
  }
 
  // 从文本文件中读取2D点
std::vector points2d;
parser_2D_tracks( argv[1], points2d );
 
  // 设置相机标定矩阵
  const double f = atof(argv[2]),
cx = atof(argv[3]), cy = atof(argv[4]);
 
  Matx33d K = Matx33d( f, 0, cx,
0, f, cy,
                       0, 0,  1);
 
 
  bool is_projective = true;
vector Rs_est, ts_est, points3d_estimated;
  重建(points2d, Rs_est, ts_est, K, points3d_estimated, is_projective);
 
  // 打印输出
 
cout << "\n----------------------------\n" << endl;
cout << "重建: " << endl;
cout << "============================" << endl;
cout << "估计的3D点: " << points3d_estimated.size() << endl;
cout << "估计的相机: " << Rs_est.size() << endl;
cout << "优化内参: " << endl << K << endl << endl;
 
cout << "3D可视化: " << endl;
cout << "============================" << endl;
 
 
  viz::Viz3d窗口估计("估计坐标系帧");
窗口估计.setBackgroundColor(); // 默认为黑色
窗口估计.registerKeyboardCallback(&keyboard_callback);
 
  // 创建点云
cout << "恢复点 ... ";
 
  // 恢复估计的points3d
vector点云估计;
  for (int i = 0; i < points3d_estimated.size(); ++i)
点云估计.push_back(Vec3f(points3d_estimated[i]));
 
cout << "[完成]" << endl;
 
 
cout << "恢复相机 ... ";
 
vector路径估计;
  for (size_t i = 0; i < Rs_est.size(); ++i)
路径估计.push_back(Affine3d(Rs_est[i],ts_est[i]));
 
cout << "[完成]" << endl;
 
cout << "绘制轨迹 ... ";
 
cout << endl << "按：" << endl;
cout << " 's' 切换相机视角" << endl;
cout << " 'q' 关闭窗口 " << endl;
 
 
  if ( path_est.size() > 0 )
  {
    // 动态轨迹
    int idx = 0, forw = -1, n = static_cast<int>(path_est.size());
 
    while(!window_est.wasStopped())
    {
      for (size_t i = 0; i < point_cloud_est.size(); ++i)
      {
        Vec3d point = point_cloud_est[i];
        Affine3d point_pose(Mat::eye(3,3,CV_64F), point);
 
        char buffer[50];
sprintf (buffer, "%d", static_cast<int>(i));
 
        viz::WCube cube_widget(Point3f(0.1,0.1,0.0), Point3f(0.0,0.0,-0.1), true, viz::Color::blue());
cube_widget.setRenderingProperty(viz::LINE_WIDTH, 2.0);
window_est.showWidget("Cube"+String(buffer), cube_widget, point_pose);
      }
 
      Affine3d cam_pose = path_est[idx];
 
      viz::WCameraPosition cpw(0.25); // 坐标轴
      viz::WCameraPosition cpw_frustum(K, 0.3, viz::Color::yellow()); // 摄像机视锥体
 
      if ( camera_pov )
window_est.setViewerPose(cam_pose);
      else
      {
        // 渲染完整轨迹
window_est.showWidget("cameras_frames_and_lines_est", viz::WTrajectory(path_est, viz::WTrajectory::PATH, 1.0, viz::Color::green()));
 
window_est.showWidget("CPW", cpw, cam_pose);
window_est.showWidget("CPW_FRUSTUM", cpw_frustum, cam_pose);
      }
 
      // 更新轨迹索引（弹簧效果）
forw *= (idx == n || idx == 0) ? -1: 1; idx += forw;
 
      // 帧率 1 秒
window_est.spinOnce(1, true);
window_est.removeAllWidgets();
    }
 
  }
 
  return 0;
}

说明

首先，我们需要加载包含所有帧中跟踪的2D点的文件，并构造一个容器以供重建API使用。在这种情况下，跟踪的2D点将具有以下结构：一个2D点数组向量，其中每个内部数组表示不同的帧。每一帧由一系列2D点组成，例如，第1帧中的第一个点与第2帧中的同一个点相同。如果某帧没有点，则分配的值将为（-1，-1）

/* 构建以下结构数据
 *
* frame1 frame2 frameN
* track1 | (x11,y11) | -> | (x12,y12) | -> | (x1N,y1N) |
* track2 | (x21,y11) | -> | (x22,y22) | -> | (x2N,y2N) |
* trackN | (xN1,yN1) | -> | (xN2,yN2) | -> | (xNN,yNN) |
 *
 *
* 如果在某个帧中没有出现标记（x，y），则其
* 值将是（-1，-1）。
 */
 
 ...
 
for (int i = 0; i < n_frames; ++i)
{
  Mat_<double>帧(2, n_tracks);
 
  for (int j = 0; j < n_tracks; ++j)
  {
帧(0,j) = 轨迹[j][i][0];
帧(1,j) = 轨迹[j][i][1];
  }
points2d.push_back(Mat(帧));
}

其次，构建的容器将用于向重建API提供数据。重要的是要强调，估计结果必须存储在vector中

bool is_projective = true;
vector Rs_est, ts_est, points3d_estimated;
reconstruct(points2d, Rs_est, ts_est, K, points3d_estimated, is_projective);
 
// 打印输出
 
cout << "\n----------------------------\n" << endl;
cout << "重建: " << endl;
cout << "============================" << endl;
cout << "估计的3D点: " << points3d_estimated.size() << endl;
cout << "估计的相机: " << Rs_est.size() << endl;
cout << "优化内参: " << endl << K << endl << endl;

最后，在Viz中显示获得的结果，在这种情况下，通过振荡效果重现相机。

使用方法和结果

为了运行此示例，我们需要指定跟踪点文件的路径、相机的焦距以及中心投影坐标（以像素为单位）。您可以在samples/data/desktop_trakcks.txt中找到一个示例文件。

./example_sfm_trajectory_reconstruction desktop_tracks.txt 1914 640 360

以下图片显示了从跟踪的2D点获得的相机运动