OpencvSharp 算子学习教案之 - Cv2.Rodrigues 重载1

OpencvSharp 算子学习教案之 – Cv2.Rodrigues 重载1
重载1：Rodrigues(InputArray src, OutputArray dst, OutputArray? jacobian = null)

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

教案：Cv2.Rodrigues(InputArray src, OutputArray dst, OutputArray? jacobian = null)

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

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

public static void Rodrigues(InputArray src, OutputArray dst, OutputArray? jacobian = null)

2. 函数用途

Cv2.Rodrigues 用于在以下两种旋转表示之间进行双向转换：

在相机标定、位姿估计、SLAM、机器人手眼标定中，这个函数非常常用。

3. 函数公式

3.1 从旋转向量到旋转矩阵

给定旋转向量 $\mathbf{r}=[r_x,r_y,r_z{]}^T$，定义：

$$\theta =||\mathbf{r}||,\mathbf{k}=\frac{\mathbf{r}}{\theta }$$

定义叉乘反对称矩阵：

[k]×=[0−kzkykz0−kx−kykx0]
[\\mathbf{k}]_\\times =
\\begin{bmatrix}
0 & -k_z & k_y \\\\
k_z & 0 & -k_x \\\\
-k_y & k_x & 0
\\end{bmatrix}
[k]×​=​0kz​−ky​​−kz​0kx​​ky​−kx​0​​

Rodrigues 公式：

$$\mathbf{R}=\mathbf{I}\cos \theta +(1-\cos \theta )\mathbf{k}{\mathbf{k}}^T+[\mathbf{k}{]}_{\times }\sin \theta$$

3.2 从旋转矩阵到旋转向量

$$\theta =\arccos \left(\frac{\mathrm{trace}(\mathbf{R})-1}{2}\right)$$

当 $\sin \theta \ne 0$ 时：

r=θ⋅12sin⁡θ[R32−R23R13−R31R21−R12]
\\mathbf{r} = \\theta \\cdot
\\frac{1}{2\\sin\\theta}
\\begin{bmatrix}
R_{32} – R_{23} \\\\
R_{13} – R_{31} \\\\
R_{21} – R_{12}
\\end{bmatrix}
r=θ⋅2sinθ1​​R32​−R23​R13​−R31​R21​−R12​​​

3.3 Jacobian 说明

参数 jacobian 是可选输出导数矩阵，常用于优化算法（Gauss-Newton、LM）。

4. 函数原理说明

5. 参数含义解析

6. 应用场景列表

7. 函数使用示例（每个场景一个完整示例）

说明：以下示例均为完整 Program.cs，可直接运行。示例中的注释采用教学模式，核心逻辑尽量逐行解释。

7.1 场景A：旋转向量 -> 旋转矩阵（含 Jacobian）

using System;
using OpenCvSharp;

internal static class Program
{
private static void Main()
{
// 定义输入旋转向量（弧度），长度表示旋转角度。
var rvecArray = new[] { 0.35, –0.2, 0.55 };

// 把托管数组转换为 Mat，作为 InputArray 的数据来源。
using var srcMat = Mat.FromArray(rvecArray);

// 创建 InputArray 包装器，传给 Rodrigues。
using var srcInput = InputArray.Create(srcMat);

// 创建输出旋转矩阵容器（Mat<double> 便于转回 double[,]）。
using var dstMat = new Mat<double>();

// 创建输出 Jacobian 容器。
using var jacobianMat = new Mat<double>();

// 把 Mat 包装为 OutputArray。
using var dstOutput = OutputArray.Create(dstMat);

// 把 Jacobian Mat 包装为 OutputArray。
using var jacobianOutput = OutputArray.Create(jacobianMat);

// 调用目标函数签名：InputArray -> OutputArray，并输出 Jacobian。
Cv2.Rodrigues(srcInput, dstOutput, jacobianOutput);

// 把旋转矩阵读取为托管二维数组。
var rotationMatrix = dstMat.ToRectangularArray();

// 把 Jacobian 读取为托管二维数组。
var jacobian = jacobianMat.ToRectangularArray();

// 打印输入向量。
Console.WriteLine("输入 rvec:");
Console.WriteLine($"[{rvecArray[0]:F9}, {rvecArray[1]:F9}, {rvecArray[2]:F9}]");
Console.WriteLine();

// 打印输出旋转矩阵。
Console.WriteLine("输出 R:");
PrintMatrix(rotationMatrix, "F9");
Console.WriteLine();

// 打印 Jacobian 尺寸。
Console.WriteLine($"Jacobian 尺寸: {jacobian.GetLength(0)} x {jacobian.GetLength(1)}");

// 打印 Jacobian 前 3 行，便于课堂演示。
Console.WriteLine("Jacobian 前 3 行:");
PrintMatrixSlice(jacobian, 3, jacobian.GetLength(1), "F9");
}

private static void PrintMatrix(double[,] matrix, string format)
{
// 外层循环遍历每一行。
for (var r = 0; r < matrix.GetLength(0); r++)
{
// 打印当前行起始。
Console.Write("[");

// 内层循环遍历每一列。
for (var c = 0; c < matrix.GetLength(1); c++)
{
// 按给定格式输出数值。
Console.Write(matrix[r, c].ToString(format));

// 如果不是最后一列，打印分隔符。
if (c < matrix.GetLength(1) – 1)
{
Console.Write(", ");
}
}

// 打印当前行结束。
Console.WriteLine("]");
}
}

private static void PrintMatrixSlice(double[,] matrix, int maxRows, int maxCols, string format)
{
// 防止越界，计算实际输出行列数。
var rows = Math.Min(maxRows, matrix.GetLength(0));
var cols = Math.Min(maxCols, matrix.GetLength(1));

// 按行输出切片。
for (var r = 0; r < rows; r++)
{
// 打印行起始。
Console.Write("[");

// 遍历列并打印。
for (var c = 0; c < cols; c++)
{
// 打印元素。
Console.Write(matrix[r, c].ToString(format));

// 处理分隔符。
if (c < cols – 1)
{
Console.Write(", ");
}
}

// 打印行结束。
Console.WriteLine("]");
}
}
}

7.2 场景B：旋转矩阵 -> 旋转向量

using System;
using OpenCvSharp;

internal static class Program
{
private static void Main()
{
// 先准备一个测试旋转向量。
var seedRvec = new[] { 0.2, 0.1, –0.4 };

// 通过 Rodrigues 正向得到合法旋转矩阵（避免手写矩阵误差）。
var rotationMatrix = BuildRotationMatrix(seedRvec);

// 将旋转矩阵封装为 Mat。
using var srcMat = Mat.FromArray(rotationMatrix);

// 创建 InputArray。
using var srcInput = InputArray.Create(srcMat);

// 创建输出向量容器。
using var dstMat = new Mat<double>();

// 创建 OutputArray。
using var dstOutput = OutputArray.Create(dstMat);

// 调用 Rodrigues 反向转换：R -> rvec。
Cv2.Rodrigues(srcInput, dstOutput, null);

// 读取输出旋转向量。
var rvec = dstMat.ToArray();

// 计算旋转角度（弧度）。
var thetaRad = Math.Sqrt(rvec[0] * rvec[0] + rvec[1] * rvec[1] + rvec[2] * rvec[2]);

// 把弧度转成角度，便于展示。
var thetaDeg = thetaRad * 180.0 / Math.PI;

// 输出输入矩阵。
Console.WriteLine("输入 R:");
PrintMatrix(rotationMatrix, "F9");
Console.WriteLine();

// 输出结果向量。
Console.WriteLine("输出 rvec:");
Console.WriteLine($"[{rvec[0]:F9}, {rvec[1]:F9}, {rvec[2]:F9}]");

// 输出角度。
Console.WriteLine($"theta = {thetaRad:F9} rad = {thetaDeg:F6} deg");
}

private static double[,] BuildRotationMatrix(double[] rvec)
{
// 构建输入 Mat。
using var srcMat = Mat.FromArray(rvec);

// 封装 InputArray。
using var srcInput = InputArray.Create(srcMat);

// 构建输出 Mat。
using var dstMat = new Mat<double>();

// 封装 OutputArray。
using var dstOutput = OutputArray.Create(dstMat);

// 调用 Rodrigues：rvec -> R。
Cv2.Rodrigues(srcInput, dstOutput, null);

// 返回旋转矩阵。
return dstMat.ToRectangularArray();
}

private static void PrintMatrix(double[,] matrix, string format)
{
// 遍历每一行。
for (var r = 0; r < matrix.GetLength(0); r++)
{
// 行起始。
Console.Write("[");

// 遍历每一列。
for (var c = 0; c < matrix.GetLength(1); c++)
{
// 打印当前元素。
Console.Write(matrix[r, c].ToString(format));

// 打印逗号分隔符。
if (c < matrix.GetLength(1) – 1)
{
Console.Write(", ");
}
}

// 行结束。
Console.WriteLine("]");
}
}
}

7.3 场景C：Jacobian 数值验证（解析导数 vs 有限差分）

using System;
using OpenCvSharp;

internal static class Program
{
private static void Main()
{
// 设置测试旋转向量。
var rvec = new[] { 0.35, –0.2, 0.55 };

// 设置有限差分步长。
const double epsilon = 1e-6;

// 计算解析 Jacobian 与旋转矩阵。
var analytic = ComputeAnalyticJacobian(rvec, out var rotationMatrix);

// 计算数值 Jacobian。
var numeric = ComputeNumericJacobian(rvec, epsilon);

// 统计误差。
ComputeErrorStats(analytic, numeric, out var maxAbsError, out var meanAbsError);

// 打印输入。
Console.WriteLine("输入 rvec:");
Console.WriteLine($"[{rvec[0]:F9}, {rvec[1]:F9}, {rvec[2]:F9}]");
Console.WriteLine();

// 打印旋转矩阵。
Console.WriteLine("对应 R:");
PrintMatrix(rotationMatrix, "F9");
Console.WriteLine();

// 打印误差结果。
Console.WriteLine($"epsilon = {epsilon:G17}");
Console.WriteLine($"max |J_analytic – J_numeric| = {maxAbsError:E6}");
Console.WriteLine($"mean |J_analytic – J_numeric| = {meanAbsError:E6}");
}

private static double[,] ComputeAnalyticJacobian(double[] rvec, out double[,] rotationMatrix)
{
// 构建输入 Mat。
using var srcMat = Mat.FromArray(rvec);

// 构建 InputArray。
using var srcInput = InputArray.Create(srcMat);

// 构建旋转矩阵输出 Mat。
using var dstMat = new Mat<double>();

// 构建 Jacobian 输出 Mat。
using var jacobianMat = new Mat<double>();

// 构建 OutputArray（矩阵）。
using var dstOutput = OutputArray.Create(dstMat);

// 构建 OutputArray（Jacobian）。
using var jacobianOutput = OutputArray.Create(jacobianMat);

// 调 Rodrigues。
Cv2.Rodrigues(srcInput, dstOutput, jacobianOutput);

// 导出旋转矩阵。
rotationMatrix = dstMat.ToRectangularArray();

// 导出 Jacobian。
return jacobianMat.ToRectangularArray();
}

private static double[,] ComputeNumericJacobian(double[] rvec, double epsilon)
{
// 9×3：R 展平后 9 项，对 rvec 三个分量求导。
var numeric = new double[9, 3];

// 外层循环遍历参数列。
for (var c = 0; c < 3; c++)
{
// 拷贝向量，构造正扰动。
var plus = (double[])rvec.Clone();

// 加上 epsilon。
plus[c] += epsilon;

// 拷贝向量，构造负扰动。
var minus = (double[])rvec.Clone();

// 减去 epsilon。
minus[c] -= epsilon;

// 计算正扰动矩阵。
var rPlus = ComputeRotationMatrix(plus);

// 计算负扰动矩阵。
var rMinus = ComputeRotationMatrix(minus);

// 展平矩阵（行优先）。
var p = FlattenRowMajor(rPlus);

// 展平矩阵（行优先）。
var m = FlattenRowMajor(rMinus);

// 内层循环计算每一行导数。
for (var r = 0; r < 9; r++)
{
// 中心差分公式。
numeric[r, c] = (p[r] – m[r]) / (2.0 * epsilon);
}
}

// 返回数值 Jacobian。
return numeric;
}

private static double[,] ComputeRotationMatrix(double[] rvec)
{
// 输入 Mat。
using var srcMat = Mat.FromArray(rvec);

// 输入包装。
using var srcInput = InputArray.Create(srcMat);

// 输出 Mat。
using var dstMat = new Mat<double>();

// 输出包装。
using var dstOutput = OutputArray.Create(dstMat);

// Rodrigues 正向计算。
Cv2.Rodrigues(srcInput, dstOutput, null);

// 导出矩阵。
return dstMat.ToRectangularArray();
}

private static double[] FlattenRowMajor(double[,] matrix)
{
// 返回 3×3 矩阵的行优先展平结果。
return
[
matrix[0, 0], matrix[0, 1], matrix[0, 2],
matrix[1, 0], matrix[1, 1], matrix[1, 2],
matrix[2, 0], matrix[2, 1], matrix[2, 2],
];
}

private static void ComputeErrorStats(double[,] analytic, double[,] numeric, out double maxAbsError, out double meanAbsError)
{
// 初始化最大误差。
maxAbsError = 0;

// 初始化误差和。
var sumAbs = 0.0;

// 遍历行。
for (var r = 0; r < analytic.GetLength(0); r++)
{
// 遍历列。
for (var c = 0; c < analytic.GetLength(1); c++)
{
// 当前绝对误差。
var absError = Math.Abs(analytic[r, c] – numeric[r, c]);

// 累加误差。
sumAbs += absError;

// 更新最大误差。
if (absError > maxAbsError)
{
maxAbsError = absError;
}
}
}

// 计算平均误差。
meanAbsError = sumAbs / (analytic.GetLength(0) * analytic.GetLength(1));
}

private static void PrintMatrix(double[,] matrix, string format)
{
// 行循环。
for (var r = 0; r < matrix.GetLength(0); r++)
{
// 输出行前缀。
Console.Write("[");

// 列循环。
for (var c = 0; c < matrix.GetLength(1); c++)
{
// 输出元素。
Console.Write(matrix[r, c].ToString(format));

// 输出分隔符。
if (c < matrix.GetLength(1) – 1)
{
Console.Write(", ");
}
}

// 输出行后缀。
Console.WriteLine("]");
}
}
}

7.4 场景D：姿态投影可视化（代码生成示例图）

using System;
using OpenCvSharp;

internal static class Program
{
private static void Main()
{
// 准备旋转向量。
var rvec = new[] { 0.45, –0.1, 0.25 };

// 准备平移向量，让目标位于相机前方。
var tvec = new[] { 0.0, 0.0, 4.0 };

// 准备相机内参矩阵。
var cameraMatrix = new double[,]
{
{ 680, 0, 400 },
{ 0, 680, 260 },
{ 0, 0, 1 },
};

// 准备畸变参数（这里设为 0，简化教学）。
var distCoeffs = new double[] { 0, 0, 0, 0, 0 };

// 定义 3D 坐标轴关键点：原点 + X/Y/Z。
var objectPoints = new[]
{
new Point3f(0, 0, 0),
new Point3f(1.2f, 0, 0),
new Point3f(0, 1.2f, 0),
new Point3f(0, 0, 1.2f),
};

// 把 3D 点投影到图像平面。
Cv2.ProjectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, out Point2f[] imagePoints, out _);

// 创建画布。
using var canvas = new Mat(new Size(800, 520), MatType.CV_8UC3, new Scalar(247, 251, 254));

// 转为像素点。
var origin = ToPixelPoint(imagePoints[0]);
var xAxis = ToPixelPoint(imagePoints[1]);
var yAxis = ToPixelPoint(imagePoints[2]);
var zAxis = ToPixelPoint(imagePoints[3]);

// 绘制参考十字线。
Cv2.Line(canvas, new Point(0, 260), new Point(799, 260), new Scalar(230, 235, 240), 1, LineTypes.AntiAlias);
Cv2.Line(canvas, new Point(400, 0), new Point(400, 519), new Scalar(230, 235, 240), 1, LineTypes.AntiAlias);

// 绘制三轴箭头。
Cv2.ArrowedLine(canvas, origin, xAxis, new Scalar(30, 30, 220), 3, LineTypes.AntiAlias, 0, 0.08);
Cv2.ArrowedLine(canvas, origin, yAxis, new Scalar(20, 160, 20), 3, LineTypes.AntiAlias, 0, 0.08);
Cv2.ArrowedLine(canvas, origin, zAxis, new Scalar(220, 120, 20), 3, LineTypes.AntiAlias, 0, 0.08);

// 绘制文字标签。
Cv2.PutText(canvas, "X", new Point(xAxis.X + 8, xAxis.Y), HersheyFonts.HersheySimplex, 0.8, new Scalar(20, 20, 200), 2, LineTypes.AntiAlias);
Cv2.PutText(canvas, "Y", new Point(yAxis.X + 8, yAxis.Y), HersheyFonts.HersheySimplex, 0.8, new Scalar(20, 130, 20), 2, LineTypes.AntiAlias);
Cv2.PutText(canvas, "Z", new Point(zAxis.X + 8, zAxis.Y), HersheyFonts.HersheySimplex, 0.8, new Scalar(200, 100, 20), 2, LineTypes.AntiAlias);

// 标注原点。
Cv2.Circle(canvas, origin, 5, new Scalar(80, 80, 80), –1, LineTypes.AntiAlias);

// 输出图像路径。
var outputPath = "rodrigues_projection_demo.png";

// 保存图像文件。
Cv2.ImWrite(outputPath, canvas);

// 打印结果。
Console.WriteLine("投影图已生成：" + outputPath);
Console.WriteLine($"Origin = ({imagePoints[0].X:F3}, {imagePoints[0].Y:F3})");
Console.WriteLine($"X = ({imagePoints[1].X:F3}, {imagePoints[1].Y:F3})");
Console.WriteLine($"Y = ({imagePoints[2].X:F3}, {imagePoints[2].Y:F3})");
Console.WriteLine($"Z = ({imagePoints[3].X:F3}, {imagePoints[3].Y:F3})");
}

private static Point ToPixelPoint(Point2f p)
{
// 将浮点坐标转换为整数像素坐标。
return new Point((int)Math.Round(p.X), (int)Math.Round(p.Y));
}
}

8. 函数使用注意事项

9. 参数调优建议

10. 示例代码运行说明（按场景关键逻辑）

10.1 场景A：旋转向量 -> 旋转矩阵

10.2 场景B：旋转矩阵 -> 旋转向量

10.3 场景C：Jacobian 数值验证

10.4 场景D：姿态投影可视化

11. 常见错误排查

本篇文档对应 Demo 界面：CVSharpDemo 主窗体左侧 Rodrigues 四个场景页面。