RealSense二次开发

转载：librealsense2查看相机设备信息 - JavaShuo

文章目录

• • 1. librealsense2设备信息读取
  • ２．realsense 投影函数和反投影函数
  • 3.深度相机与彩色相机的坐标变换

1. librealsense2设备信息读取

librealsense2提供的接口可以检测连接到系统的所有realsense设备，如sr300，并可以读取每个设备的编号、彩色相机和深度相机的内参（相机内参矩阵参数和畸变系数及其畸变模型）、彩色相机和深度相机坐标系间的变换矩阵。
接口调用如下：

#include  // Include RealSense Cross Platform API
#include
#include

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: "<

 

可以看到我连接了一个设备，设备编号为541142001592，同时也可以看到彩色相机和深度相机的内参和畸变系数及其畸变模型，彩色相机畸变模型为：None, 并且畸变系数都为0，表示彩色相机无畸变；而深度相机畸变模型为：Inverse Brown Conrady，并且畸变系数不为0，表示深度相机存在畸变。畸变系数从左到右分别是：k1, k2, p1, p2, k3。在librealsense2安装路径下的rsutil.h文件可以看到将3d坐标点投影到2d图像平面的函数 rs2_project_point_to_pixel 写着“ //assert(intrin->model != RS2_DISTORTION_INVERSE_BROWN_CONRADY); // Cannot project to an inverse-distorted image”，也就是说我使用的设备的深度相机无法将3d坐标图像直接投影到2d图像平面，有关相机内参和畸变模型参考另一篇博客相机内参与畸变模型

２．realsense 投影函数和反投影函数

在librealsense2安装路径下的rsutil.h文件里可以找到相机的投影函数和反投影函数，投影函数即将相机坐标系下的一个3d点投影到2d图像平面上的像素坐标，反投影函数即已知相机2d图像平面上像素点对应的深度值可以计算出该点在相机坐标系下的3d坐标值，下面粘贴这两个函数出来：
投影函数：

/* Given a point in 3D space, compute the corresponding pixel coordinates in an image with no distortion or forward distortion coefficients produced by the same camera */
static void rs2_project_point_to_pixel(float pixel[2], const struct rs2_intrinsics * intrin, const float point[3])
{
    //assert(intrin->model != RS2_DISTORTION_INVERSE_BROWN_CONRADY); // Cannot project to an inverse-distorted image

    float x = point[0] / point[2], y = point[1] / point[2];

    if(intrin->model == RS2_DISTORTION_MODIFIED_BROWN_CONRADY)
    {

        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;
    }
    if (intrin->model == RS2_DISTORTION_FTHETA)
    {
        float r = sqrt(x*x + y*y);
            auto rd = (1.0f / intrin->coeffs[0] * atan(2 * r* tan(intrin->coeffs[0] / 2.0f)));
            x *= rd / r;
            y *= rd / r;
    }

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

反投影函数：

/* Given pixel coordinates and depth in an image with no distortion or inverse distortion coefficients, compute the corresponding point in 3D space relative to the same camera */
static void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics * intrin, const float pixel[2], float depth)
{
    assert(intrin->model != RS2_DISTORTION_MODIFIED_BROWN_CONRADY); // Cannot deproject from a forward-distorted image
    assert(intrin->model != RS2_DISTORTION_FTHETA); // Cannot deproject to an ftheta image
    //assert(intrin->model != RS2_DISTORTION_BROWN_CONRADY); // Cannot deproject to an brown conrady model

    float x = (pixel[0] - intrin->ppx) / intrin->fx;
    float y = (pixel[1] - intrin->ppy) / intrin->fy;
    if(intrin->model == RS2_DISTORTION_INVERSE_BROWN_CONRADY)
    {
        float r2  = x*x + y*y;
        float f = 1 + intrin->coeffs[0]*r2 + intrin->coeffs[1]*r2*r2 + intrin->coeffs[4]*r2*r2*r2;
        float ux = x*f + 2*intrin->coeffs[2]*x*y + intrin->coeffs[3]*(r2 + 2*x*x);
        float uy = y*f + 2*intrin->coeffs[3]*x*y + intrin->coeffs[2]*(r2 + 2*y*y);
        x = ux;
        y = uy;
    }
    point[0] = depth * x;
    point[1] = depth * y;
    point[2] = depth;
}

3.深度相机与彩色相机的坐标变换

第一节中我们获取了深度相机到彩色相机的坐标变换矩阵，下列realsense函数将深度相机坐标系下的一个坐标点转换到彩色相机坐标系下的坐标值，同理也可以计算彩色相机坐标系下的坐标值在深度相机坐标系下的坐标值：

/* Transform 3D coordinates relative to one sensor to 3D coordinates relative to another viewpoint */
static void rs2_transform_point_to_point(float to_point[3], const struct rs2_extrinsics * extrin, const float from_point[3])
{
    to_point[0] = extrin->rotation[0] * from_point[0] + extrin->rotation[3] * from_point[1] + extrin->rotation[6] * from_point[2] + extrin->translation[0];
    to_point[1] = extrin->rotation[1] * from_point[0] + extrin->rotation[4] * from_point[1] + extrin->rotation[7] * from_point[2] + extrin->translation[1];
    to_point[2] = extrin->rotation[2] * from_point[0] + extrin->rotation[5] * from_point[1] + extrin->rotation[8] * from_point[2] + extrin->translation[2];
}