Intel RealSense D415/435：C# 中的坐标映射

Vangos

5.00/5 (5投票s)

2018 年 10 月 10 日

CPOL

5分钟阅读

13211

在过去的几个月里，我一直在深入研究 Intel RealSense D415 和 D435 深度摄像头。今天，我将向您展示如何轻松地在不同的坐标系之间进行转换。RealSense D415/435 是一款低成本设备……

在过去的几个月里，我一直在深入研究 Intel RealSense D415 和 D435 深度摄像头。今天，我将向您展示如何轻松地在不同的坐标系之间进行转换。RealSense D415/435 是一款低成本设备，可以为您的应用程序增强 3D 感知能力。其技术规格如下所示：

使用环境	室内/室外
深度技术	主动红外立体（全局快门）
主要 Intel® RealSense™ 组件	Intel® RealSense™ 视觉处理器 D4 Intel® RealSense™ 模块 D430
深度视场角（水平 × 垂直 × 对角线）	86 x 57 x 94 (+/- 3°)
深度流输出分辨率	高达 1280 x 720
深度流输出帧率	高达 90 fps
最小深度距离（Min-Z）	0.2 米
传感器快门类型	全局快门
最大范围	约 10.0 米
RGB 传感器分辨率和帧率	1920 x 1080，30 fps
RGB 传感器视场角（水平 x 垂直 x 对角线）	69.4° x 42.5° x 77° (+/- 3°)
相机尺寸（长 x 深 x 高）	90 毫米 x 25 毫米 x 25 毫米
连接器	USB 3.0 Type – C
安装机制	一个 1/4-20 UNC 螺纹安装点两个 M3 螺纹安装点

什么是坐标映射？

使用深度摄像头时，最常见的任务之一是将 3D 世界空间坐标映射到 2D 屏幕空间坐标（反之亦然）。

3D 世界空间的坐标系以米为单位。因此，3D 点由 X、Y 和 Z 坐标集表示，分别对应水平、垂直和深度轴。3D 坐标系的原点 (0, 0, 0) 是摄像机的位置。

Intel RealSense D435 3D view

2D 屏幕空间的坐标系以像素为单位。2D 点由 X 和 Y 坐标集表示，分别对应帧的最左侧和最顶部位置。笛卡尔坐标系的 (0, 0) 原点是帧的左上角。RealSense 由 RGB 和深度传感器组成。这意味着每个传感器将生成两种离散的帧类型。目前，我们只关注深度帧。

Intel RealSense D435 Color & Depth views

坐标映射是在 3D 和 2D 系统之间进行转换的过程。

此类转换需要了解相机的内部配置（内部参数和外部参数）。内部和外部参数指定了诸如镜头畸变、焦点、图像格式、旋转矩阵等属性。因此，所需的数学转换通常由 SDK 内部执行。事实上，英特尔在其 **C++** 和 **Python** 框架中公开了这些转换。

如果您一直关注我的博客，您就知道我大部分商业应用程序都是用 C# 和 Unity3D 构建的。因此，令我非常惊讶的是，坐标映射功能并未包含在官方的 C# 框架中。这就是为什么我决定将原始 C++ 投影和去投影代码移植到 C#。

值得庆幸的是，C# API 已经包含了一个 **Intrinsics** 结构体。Intrinsics 结构体为我们提供了原始的相机内部参数。

背景（镜头、畸变、摄影）

请记住，我们将使用一些在摄影和摄影测量学中常见的术语。RealSense 内部参数如下：

宽度/高度

宽度和高度参数是相机图像的尺寸，以像素为单位。

主点

您可以将主点视为相机中平行光线聚焦的点。主点的坐标存储在 **ppx** 和 **ppy** 值中。

焦距

焦距是从镜头中心到主点的距离。它由 **fx** 和 **fy** 参数表示。

下图将帮助您直观地理解这一概念。

Camera principal focal point - focal length illustration

畸变模型

镜头看到的世界并非一个完美的矩形框架。相反，框架会偏离其直线投影。这种偏离会导致鱼眼效应。如果我们知道畸变的系数，就可以“校正”它。校正使用 Brown-Conrady 变换。

畸变系数

畸变系数是数学上描述畸变的值，存储在 **coeffs** 矩阵中。

如果这一切看起来很难理解，那是因为它确实很难。不过别担心。我保证会为您处理好数学问题

必备组件

要测试源代码，您需要一台 RealSense 摄像头，以及官方的 RealSense SDK for C# 或 Unity。

推荐的开发环境是 Visual Studio 2017 和 Unity3D。

3D 到 2D 映射

首先，我们将 3D 世界坐标转换为 2D 相机坐标。为了封装 3D 坐标，我创建了一个简单的 **Vector3D** 结构体（X、Y、Z 浮点值）。为了表示 2D 坐标，我创建了一个 **Vector2D** 结构体（X、Y 浮点值）。

public struct Vector3D
{
    public float X;
    public float Y;
    public float Z;
}

public struct Vector2D
{
    public float X;
    public float Y;
}

当然，如果您愿意，也可以使用原生的 .NET 或 Unity 向量结构。

以下方法扩展了 Intrinsics 类。它接收一个 3D 点作为输入，并输出相应的 2D 坐标。如果转换因任何原因失败，将返回 (0,0) 点。

public static Vector2D Map3DTo2D(this Intrinsics intrinsics, Vector3D point)
{
    Vector2D pixel = new Vector2D();

    float x = point.X / point.Z;
    float y = point.Y / point.Z;

    if (intrinsics.model == Distortion.ModifiedBrownConrady)
    {
        float r2 = x * x + y * y;
        float f = 1f + intrinsics.coeffs[0] * r2 + intrinsics.coeffs[1] * r2 * r2 + intrinsics.coeffs[4] * r2 * r2 * r2;

        x *= f;
        y *= f;

        float dx = x + 2f * intrinsics.coeffs[2] * x * y + intrinsics.coeffs[3] * (r2 + 2 * x * x);
        float dy = y + 2f * intrinsics.coeffs[3] * x * y + intrinsics.coeffs[2] * (r2 + 2 * y * y);

        x = dx;
        y = dy;
    }

    if (intrinsics.model == Distortion.Ftheta)
    {
        float r = (float)Math.Sqrt(x * x + y * y);
        float rd = (1f / intrinsics.coeffs[0] * (float)Math.Atan(2f * r * (float)Math.Tan(intrinsics.coeffs[0] / 2f)));

        x *= rd / r;
        y *= rd / r;
    }

    pixel.X = x * intrinsics.fx + intrinsics.ppx;
    pixel.Y = y * intrinsics.fy + intrinsics.ppy;

    return pixel;
}

2D 到 3D 映射

现在，我们将执行相反的操作：将 2D 点映射到 3D 空间。为此，我们需要了解几件事：

2D 点的 X 和 Y，以及
2D 点的深度

所需点的深度可以通过调用 DepthFrame 类的 GetDistance() 方法来找到。

float depth = depthFrame.GetDistance(x, y);

有了这些输入，以下方法将把 2D 屏幕坐标转换为 3D 世界坐标：

public static Vector3D Map2DTo3D(this Intrinsics intrinsics, Vector2D pixel, float depth)
{
    Vector3D point = new Vector3D();

    float x = (pixel.X - intrinsics.ppx) / intrinsics.fx;
    float y = (pixel.Y - intrinsics.ppy) / intrinsics.fy;

    if (intrinsics.model == Distortion.InverseBrownConrady)
    {
        float r2 = x * x + y * y;
        float f = 1 + intrinsics.coeffs[0] * r2 + intrinsics.coeffs[1] * r2 * r2 + intrinsics.coeffs[4] * r2 * r2 * r2;
        float ux = x * f + 2 * intrinsics.coeffs[2] * x * y + intrinsics.coeffs[3] * (r2 + 2 * x * x);
        float uy = y * f + 2 * intrinsics.coeffs[3] * x * y + intrinsics.coeffs[2] * (r2 + 2 * y * y);

        x = ux;
        y = uy;
    }

    point.X = depth * x;
    point.Y = depth * y;
    point.Z = depth;

    return point;
}

2D 颜色到深度映射

最后，还有另一种坐标映射场景：在 2D **颜色**坐标和 2D **深度**坐标之间进行转换。C# SDK 为此目的提供了 **Align** 类，因此您可以参考官方文档了解使用说明。

摘要

各位，这就是全部内容！供您参考，这是完整的源代码：

using Intel.RealSense;
using System;

namespace Intel.RealSense.Extensions
{
    /// <summary>
    /// Converts between 2D and 3D RealSense coordinates.
    /// </summary>
    public static class CoordinateMapper
    {
        /// <summary>
        /// Maps the specified 3D point to the 2D space.
        /// </summary>
        /// <param name="intrinsics">The camera intrinsics to use.</param>
        /// <param name="point">The 3D point to map.</param>
        /// <returns>The corresponding 2D point.</returns>
        public static Vector2D Map3DTo2D(this Intrinsics intrinsics, Vector3D point)
        {
            Vector2D pixel = new Vector2D();

            float x = point.X / point.Z;
            float y = point.Y / point.Z;

            if (intrinsics.model == Distortion.ModifiedBrownConrady)
            {
                float r2 = x * x + y * y;
                float f = 1f + intrinsics.coeffs[0] * r2 + intrinsics.coeffs[1] * r2 * r2 + intrinsics.coeffs[4] * r2 * r2 * r2;

                x *= f;
                y *= f;

                float dx = x + 2f * intrinsics.coeffs[2] * x * y + intrinsics.coeffs[3] * (r2 + 2 * x * x);
                float dy = y + 2f * intrinsics.coeffs[3] * x * y + intrinsics.coeffs[2] * (r2 + 2 * y * y);

                x = dx;
                y = dy;
            }

            if (intrinsics.model == Distortion.Ftheta)
            {
                float r = (float)Math.Sqrt(x * x + y * y);
                float rd = (1f / intrinsics.coeffs[0] * (float)Math.Atan(2f * r * (float)Math.Tan(intrinsics.coeffs[0] / 2f)));

                x *= rd / r;
                y *= rd / r;
            }

            pixel.X = x * intrinsics.fx + intrinsics.ppx;
            pixel.Y = y * intrinsics.fy + intrinsics.ppy;

            return pixel;
        }

        /// <summary>
        /// Maps the specified 2D point to the 3D space.
        /// </summary>
        /// <param name="intrinsics">The camera intrinsics to use.</param>
        /// <param name="pixel">The 2D point to map.</param>
        /// <param name="depth">The depth of the 2D point to map.</param>
        /// <returns>The corresponding 3D point.</returns>
        public static Vector3D Map2DTo3D(this Intrinsics intrinsics, Vector2D pixel, float depth)
        {
            Vector3D point = new Vector3D();

            float x = (pixel.X - intrinsics.ppx) / intrinsics.fx;
            float y = (pixel.Y - intrinsics.ppy) / intrinsics.fy;

            if (intrinsics.model == Distortion.InverseBrownConrady)
            {
                float r2 = x * x + y * y;
                float f = 1 + intrinsics.coeffs[0] * r2 + intrinsics.coeffs[1] * r2 * r2 + intrinsics.coeffs[4] * r2 * r2 * r2;
                float ux = x * f + 2 * intrinsics.coeffs[2] * x * y + intrinsics.coeffs[3] * (r2 + 2 * x * x);
                float uy = y * f + 2 * intrinsics.coeffs[3] * x * y + intrinsics.coeffs[2] * (r2 + 2 * y * y);

                x = ux;
                y = uy;
            }

            point.X = depth * x;
            point.Y = depth * y;
            point.Z = depth;

            return point;
        }
    }

    /// <summary>
    /// Represensts a 2D vector/point.
    /// </summary>
    public struct Vector2D
    {
        public float X;
        public float Y;
    }

    /// <summary>
    /// Represensts a 3D vector/point.
    /// </summary>
    public struct Vector3D
    {
        public float X;
        public float Y;
        public float Z;
    }
}

好了，现在您可以在 C# 和 Unity 应用程序中使用 RealSense 的全部强大功能了！

下次见……继续编码！

文章 Intel RealSense D415/435：C# 中的坐标映射最初发布于 Vangos Pterneas。