OpencvSharp 算子学习教案之 - Cv2.ComposeRT 重载3

大家好，Opencv在很多工程项目中都会用到，而OpencvSharp则是以C#开发与实现的Opencv操作库，对.NET开发人员友好，但很多API的中文资料、应用场景及常见坑点等缺乏系统性归纳，因此这系列博客将给大家带来Cv2及Mat对象全系列算子学习教案，供大家参考学习。

Cv2.ComposeRT

• 教案版本：V1.0
• 面向对象：OpenCvSharp 初学者
• 所属模块：calib3d
• 源码位置：OpenCvSharp/Cv2/Cv2_calib3d.cs:594

摘要：ComposeRT 的基础双数组重载用于把两个姿态串联为一个新姿态，只返回合成后的旋转向量和位移向量。本文通过手工矩阵推导说明 R3 = R2 × R1、t3 = R2 × t1 + t2 的含义，并给出可直接运行的 Console 示例。

1. 函数名称（带参数签名）

public static void ComposeRT(double[] rvec1, double[] tvec1, double[] rvec2, double[] tvec2, out double[] rvec3, out double[] tvec3)

2. 函数用途

Cv2.ComposeRT(...) 用来把两个相机/物体位姿合成为一个新的位姿。它最常见的用途包括：

1. 机器人坐标系拼接。
2. 多阶段外参链式传递。
3. 位姿优化前后的增量叠加。
4. 教学场景下理解“先转再平移”的空间变换顺序。

这个基础重载只输出 rvec3 和 tvec3，非常适合先把公式理解清楚，再学习 Jacobian 版本。

3. 函数公式

先把旋转向量通过 Rodrigues 变成旋转矩阵：

$$R_1=Rodrigues(rve{c}_1),R_2=Rodrigues(rve{c}_2)$$

合成后的旋转和位移满足：

$$R_3=R_2{R}_1$$

$$t_3=R_2{t}_1+t_2$$

最后再把 R3 转回旋转向量 rvec3。

4. 函数原理说明

这个函数内部做的事情可以理解成三步：

1. 把两个输入旋转向量转成旋转矩阵。
2. 按坐标变换规则完成矩阵乘法和向量加法。
3. 把合成后的旋转矩阵再转回旋转向量。

因此，ComposeRT 本质上不是简单的向量相加，而是完整的三维刚体变换叠加。

5. 参数含义解析

参数名	类型	含义
rvec1	double[]	第一个旋转向量，长度必须为 3
tvec1	double[]	第一个平移向量，长度必须为 3
rvec2	double[]	第二个旋转向量，长度必须为 3
tvec2	double[]	第二个平移向量，长度必须为 3
rvec3	out double[]	合成后的旋转向量
tvec3	out double[]	合成后的平移向量

补充说明：

1. rvec 的单位是弧度，不是角度。
2. tvec 的单位由你的业务场景决定，但四个向量必须处在同一坐标单位下。
3. 输入数组长度不对时，底层会抛出异常。

6. 应用场景列表

场景名	场景说明	典型用途
场景A：两段位姿拼接	把两个外参合成一个外参	机器人、AR、视觉里程计
场景B：局部增量叠加	把小的修正姿态叠加到当前姿态	优化迭代
场景C：坐标系切换	在不同参考系之间传递位姿	多传感器融合
场景D：教学验证	用矩阵公式手工验证函数输出	初学者入门

7. 函数使用示例

下面的 Console 示例保留数组数据语义，但为了避开当前数组重载在本仓库环境下可能触发的 native 尺寸断言，先把数组包装成 Mat，再调用稳定的 InputArray / OutputArray 版本，最后通过 Rodrigues 和矩阵乘法做手工校验。

using System;
using System.Globalization;
using OpenCvSharp;

internal static class Program
{
    private static void Main()
    {
        // 控制台输出切换为 UTF-8，避免中文注释和日志乱码。
        Console.OutputEncoding = System.Text.Encoding.UTF8;

        // 第一个姿态：基础姿态。
        var rvec1 = new[] { 0.18, -0.12, 0.25 };
        var tvec1 = new[] { 45.0, -20.0, 420.0 };

        // 第二个姿态：增量姿态。
        var rvec2 = new[] { -0.08, 0.14, 0.06 };
        var tvec2 = new[] { -12.0, 18.0, 75.0 };

        // 先把数组包装成 Mat，再调用更稳定的 InputArray / OutputArray 版本。
        using var rvec1Mat = Mat.FromArray(rvec1);
        using var tvec1Mat = Mat.FromArray(tvec1);
        using var rvec2Mat = Mat.FromArray(rvec2);
        using var tvec2Mat = Mat.FromArray(tvec2);
        using var rvec3Mat = new Mat();
        using var tvec3Mat = new Mat();

        Cv2.ComposeRT(rvec1Mat, tvec1Mat, rvec2Mat, tvec2Mat, rvec3Mat, tvec3Mat);

        // 再从 Mat 中读回结果，后续仍以 double[] 方式展示。
        var rvec3 = ReadVector3(rvec3Mat);
        var tvec3 = ReadVector3(tvec3Mat);

        // 再手工算一遍，用来验证函数输出是否和公式一致。
        var expected = ComposeManually(rvec1, tvec1, rvec2, tvec2);

        Console.WriteLine("=== ComposeRT 基础重载演示 ===");
        Console.WriteLine($"rvec1 = [{rvec1[0]:F6}, {rvec1[1]:F6}, {rvec1[2]:F6}]");
        Console.WriteLine($"tvec1 = [{tvec1[0]:F6}, {tvec1[1]:F6}, {tvec1[2]:F6}]");
        Console.WriteLine($"rvec2 = [{rvec2[0]:F6}, {rvec2[1]:F6}, {rvec2[2]:F6}]");
        Console.WriteLine($"tvec2 = [{tvec2[0]:F6}, {tvec2[1]:F6}, {tvec2[2]:F6}]");
        Console.WriteLine();
        Console.WriteLine($"rvec3 = [{rvec3[0]:F6}, {rvec3[1]:F6}, {rvec3[2]:F6}]");
        Console.WriteLine($"tvec3 = [{tvec3[0]:F6}, {tvec3[1]:F6}, {tvec3[2]:F6}]");
        Console.WriteLine();
        Console.WriteLine("=== 手工校验 ===");
        Console.WriteLine($"expected rvec3 = [{expected.Rvec3[0]:F6}, {expected.Rvec3[1]:F6}, {expected.Rvec3[2]:F6}]");
        Console.WriteLine($"expected tvec3 = [{expected.Tvec3[0]:F6}, {expected.Tvec3[1]:F6}, {expected.Tvec3[2]:F6}]");
        Console.WriteLine($"Δr = {ComputeVectorDiffNorm(rvec3, expected.Rvec3).ToString("F9", CultureInfo.InvariantCulture)}");
        Console.WriteLine($"Δt = {ComputeVectorDiffNorm(tvec3, expected.Tvec3).ToString("F9", CultureInfo.InvariantCulture)}");
    }

    /// <summary>
    /// 将两个姿态按 ComposeRT 的公式手工合成。
    /// </summary>
    private static (double[] Rvec3, double[] Tvec3) ComposeManually(double[] rvec1, double[] tvec1, double[] rvec2, double[] tvec2)
    {
        // 把旋转向量变成旋转矩阵。
        Cv2.Rodrigues(rvec1, out double[,] r1, out _);
        Cv2.Rodrigues(rvec2, out double[,] r2, out _);

        // 旋转矩阵相乘得到 R3。
        var r3 = Multiply3x3(r2, r1);

        // t3 = R2 * t1 + t2。
        var t3 = AddVector(MultiplyMatrixVector(r2, tvec1), tvec2);

        // 再把 R3 转回旋转向量。
        Cv2.Rodrigues(r3, out double[] rvec3, out _);
        return (rvec3, t3);
    }

    /// <summary>
    /// 从 3x1 或 1x3 Mat 中读取三维向量。
    /// </summary>
    private static double[] ReadVector3(Mat vectorMat)
    {
        return vectorMat.Rows >= 3
            ? new[] { vectorMat.At<double>(0, 0), vectorMat.At<double>(1, 0), vectorMat.At<double>(2, 0) }
            : new[] { vectorMat.At<double>(0, 0), vectorMat.At<double>(0, 1), vectorMat.At<double>(0, 2) };
    }

    /// <summary>
    /// 计算两个三维向量的欧氏距离。
    /// </summary>
    private static double ComputeVectorDiffNorm(double[] a, double[] b)
    {
        var dx = a[0] - b[0];
        var dy = a[1] - b[1];
        var dz = a[2] - b[2];
        return Math.Sqrt(dx * dx + dy * dy + dz * dz);
    }

    /// <summary>
    /// 3x3 矩阵乘法。
    /// </summary>
    private static double[,] Multiply3x3(double[,] left, double[,] right)
    {
        var result = new double[3, 3];
        for (var r = 0; r < 3; r++)
        {
            for (var c = 0; c < 3; c++)
            {
                var sum = 0.0;
                for (var k = 0; k < 3; k++)
                {
                    sum += left[r, k] * right[k, c];
                }

                result[r, c] = sum;
            }
        }

        return result;
    }

    /// <summary>
    /// 3x3 矩阵乘 3 维向量。
    /// </summary>
    private static double[] MultiplyMatrixVector(double[,] matrix, double[] vector)
    {
        return new[]
        {
            matrix[0, 0] * vector[0] + matrix[0, 1] * vector[1] + matrix[0, 2] * vector[2],
            matrix[1, 0] * vector[0] + matrix[1, 1] * vector[1] + matrix[1, 2] * vector[2],
            matrix[2, 0] * vector[0] + matrix[2, 1] * vector[1] + matrix[2, 2] * vector[2],
        };
    }

    /// <summary>
    /// 向量加法。
    /// </summary>
    private static double[] AddVector(double[] a, double[] b)
    {
        return new[] { a[0] + b[0], a[1] + b[1], a[2] + b[2] };
    }
}

8. 注意事项

1. 输入数组必须长度为 3。
2. 输入的旋转量必须使用弧度。
3. 这个函数不会改变输入数组本身。
4. 合成顺序很重要，R3 = R2 × R1 不是 R1 × R2。
5. 如果位姿属于不同坐标系，必须先统一参考系再合成。

9. 调优建议

1. 初学阶段先把姿态写成小角度，便于观察结果变化。
2. 如果怀疑合成顺序有误，先用手工矩阵公式验证。
3. 在实际工程里，尽量给位姿向量和坐标系写清楚命名。
4. 调试时可以先打印旋转矩阵，再打印旋转向量。

10. 进阶扩展

1. 继续学习 Cv2.ComposeRT(...) 的 Jacobian 重载。
2. 把多个姿态连续合成，观察误差如何累计。
3. 将本例和 Cv2.Rodrigues(...)、Cv2.ProjectPoints(...) 联动起来，构建完整的三维到二维教学链路。

11. 常见错误排查

1. 旋转向量长度不是 3。
2. 把角度当成弧度传入。
3. 把 R2 * R1 写成了 R1 * R2。
4. 不同坐标系之间的位姿直接相加。
5. 平移向量单位不一致。