获取realsense内参

文章目录

• • １．无畸变
  • ２．Brown-Conrady 畸变模型
  • 3. Opencv 里畸变图像矫正过程

１．无畸变

相机内参包括相机矩阵参数和畸变系数。
相机内参矩阵为3*3的矩阵：M = [fx 0 ppx ; 0 fy ppy ; 0 0 1]
畸变系数：k1 k2 k3 p1 p2

首先看一下无畸变情况下，已知相机坐标系内的一个点[xc, yc, zc]，通过内参矩阵求出该点在图像平面的投影点[u, v]。如下图，其中M矩阵边上相机的内参矩阵，x = xc / zc, y = yc / zc:

２．Brown-Conrady 畸变模型

当相机存在畸变的时候，需要根据畸变模型修正(x, y)的值为(x’ , y’)， 然后使 u = x’ * fx + ppx, v = y’ * fy + ppy。畸变模型如下图，此处介绍的是Brown-Conrady 畸变模型：

下面给出相应的代码实现：

struct Intrinsics
{
  float         ppx;
  float         ppy;
  float         fx;
  float         fy;
  float         coeffs[5];
  bool          distortion;
};

void Project_Point_To_Pixel(float pixel[2], const struct Intrinsics * intrin, const float point[3])
{
  float x = point[0] / point[2], y = point[1] / point[2];
  
  if(intrin->distortion){
    float r2  = x*x + y*y;
    float f = 1 + intrin->coeffs[0]*r2 + intrin->coeffs[1]*r2*r2 + intrin->coeffs[4]*r2*r2*r2;
    x *= f;
    y *= f;
    float dx = x + 2*intrin->coeffs[2]*x*y + intrin->coeffs[3]*(r2 + 2*x*x);
    float dy = y + 2*intrin->coeffs[3]*x*y + intrin->coeffs[2]*(r2 + 2*y*y);
    x = dx;
    y = dy;
  }

  pixel[0] = x * intrin->fx + intrin->ppx;
  pixel[1] = y * intrin->fy + intrin->ppy;
}

realsense获取内参

#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include<iostream>
#include<opencv2/opencv.hpp>

int main(int argc, char * argv[]) try
{
  rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);

  /// Create librealsense context for managing devices
  rs2::context ctx;   
  auto devs = ctx.query_devices();                  ///获取设备列表
  int device_num = devs.size();
  std::cout<<"device num: "<<device_num<<std::endl;///设备数量

  ///我只连了一个设备，因此我查看第0个设备的信息
  /// 当无设备连接时此处抛出rs2::error异常
  rs2::device dev = devs[0];
  ///设备编号，每个设备都有一个不同的编号, 如果连接了多个设备，便可根据此编号找到你希望启动的设备
  char serial_number[100] = {0};
  strcpy(serial_number, dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER));
  printf("serial_number: %s\n",serial_number); 

  ///设置从设备管道获取的深度图和彩色图的配置对象
  rs2::config cfg;
  ///配置彩色图像流：分辨率640*480，图像格式：BGR， 帧率：30帧/秒
  ///默认配置任意一个设备，若要配置指定的设备可以根据设备在设备列表里的序列号进行制定:
  ///int indx = 0; ///表示第0个设备
  ///cfg.enable_stream(RS2_STREAM_COLOR,indx, 640, 480, RS2_FORMAT_BGR8, 30);
  cfg.enable_stream(RS2_STREAM_COLOR,640, 480, RS2_FORMAT_BGR8, 30);
  ///配置深度图像流：分辨率640*480，图像格式：Z16， 帧率：30帧/秒
  cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);

  ///生成Realsense管道，用来封装实际的相机设备
  rs2::pipeline pipe;
  pipe.start(cfg); ///根据给定的配置启动相机管道

  rs2::frameset data;
  data = pipe.wait_for_frames();///等待一帧数据，默认等待5s

  rs2::depth_frame depth       = data.get_depth_frame(); ///获取深度图像数据
  rs2::video_frame color       = data.get_color_frame();  ///获取彩色图像数据
  rs2::stream_profile dprofile =  depth.get_profile();
  rs2::stream_profile cprofile =  color.get_profile();

  ///获取彩色相机内参
  rs2::video_stream_profile cvsprofile(cprofile);
  rs2_intrinsics color_intrin =  cvsprofile.get_intrinsics();
  std::cout<<"\ncolor intrinsics: ";
  std::cout<<color_intrin.width<<"  "<<color_intrin.height<<"  ";
  std::cout<<color_intrin.ppx<<"  "<<color_intrin.ppy<<"  ";
  std::cout<<color_intrin.fx<<"  "<<color_intrin.fy<<std::endl;
  std::cout<<"coeffs: ";
  for(auto value : color_intrin.coeffs)
    std::cout<<value<<"  ";
  std::cout<<std::endl;
  std::cout<<"distortion model: "<<color_intrin.model<<std::endl;///畸变模型

  ///获取深度相机内参
  rs2::video_stream_profile dvsprofile(dprofile);
  rs2_intrinsics depth_intrin =  dvsprofile.get_intrinsics();
  std::cout<<"\ndepth intrinsics: ";
  std::cout<<depth_intrin.width<<"  "<<depth_intrin.height<<"  ";
  std::cout<<depth_intrin.ppx<<"  "<<depth_intrin.ppy<<"  ";
  std::cout<<depth_intrin.fx<<"  "<<depth_intrin.fy<<std::endl;
  std::cout<<"coeffs: ";
  for(auto value : depth_intrin.coeffs)
    std::cout<<value<<"  ";
  std::cout<<std::endl;
  std::cout<<"distortion model: "<<depth_intrin.model<<std::endl;///畸变模型

  ///获取深度相机相对于彩色相机的外参，即变换矩阵: P_color = R * P_depth + T
  rs2_extrinsics extrin = dprofile.get_extrinsics_to(cprofile);
  std::cout<<"\nextrinsics of depth camera to color camera: \nrotaion: "<<std::endl;
  for(int i = 0; i < 3; ++i){
    for(int j = 0; j < 3; ++j){
      float value = extrin.rotation[3*i + j];
      std::cout<<value<<"  ";
    }
    std::cout<<std::endl;
  }
  std::cout<<std::endl;

  std::cout<<"translation: ";
  for(auto value : extrin.translation)
    std::cout<<value<<"  ";
  std::cout<<std::endl;

  while(1)
  {
	///等待一帧数据，默认等待5s
    data = pipe.wait_for_frames();

    rs2::depth_frame depth  = data.get_depth_frame(); ///获取深度图像数据
    rs2::video_frame color  = data.get_color_frame();  ///获取彩色图像数据
    int color_width  = color.get_width();
    int color_height = color.get_height();

    int depth_width  = depth.get_width();
    int depth_height = depth.get_height();

    if (!color || !depth) break;                ///如果获取不到数据则退出

	///将彩色图像和深度图像转换为Opencv格式
    cv::Mat image(cv::Size(color_width, color_height), CV_8UC3, (void*)color.get_data(), cv::Mat::AUTO_STEP);
    cv::Mat depthmat(cv::Size(depth_width, depth_height), CV_16U, (void*)depth.get_data(), cv::Mat::AUTO_STEP);
		
	///显示
    cv::imshow("image",image);
    cv::imshow("depth",depthmat);
    cv::waitKey(1);
  }
  return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{	
    ///捕获相机设备的异常
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr<<"Other error : " << e.what() << std::endl;
    return EXIT_FAILURE;
}

畸变知识