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

最新推荐文章于 2026-06-23 22:33:34 发布

原创最新推荐文章于 2026-06-23 22:33:34 发布 · 427 阅读

4 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

标签

#学习 #C# #OpencvSharp算子 #Opencv教程

话题

#AI编程·六月创作之星博客挑战赛

OpencvSharp 专栏收录该内容

431 篇文章

订阅专栏

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

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

Cv2.SolvePnP

教案版本：v1.0
面向对象：OpenCvSharp 初学者
所属模块：calib3d / 位姿估计
源码位置：Samples/CVSharpDemo/CVSharpDemo/Samples/Cv2SolvePnP/SolvePnPInputArraySample.cs

摘要：本页讲解 SolvePnP 的 InputArray / OutputArray 重载，演示如何用三维点和二维像素点恢复相机位姿，并用重投影误差验证结果。

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

public static void SolvePnP(
    InputArray objectPoints,
    InputArray imagePoints,
    InputArray cameraMatrix,
    InputArray distCoeffs,
    OutputArray rvec,
    OutputArray tvec,
    bool useExtrinsicGuess = false,
    SolvePnPMethod flags = SolvePnPMethod.Iterative)

这是一组最适合和 Mat、InputArray、OutputArray 配合的重载。它不会返回值，而是把求得的旋转向量 rvec 和平移向量 tvec 写入输出容器。

2. 函数用途

SolvePnP 的作用是：已知一组三维对象点和它们在图像上的二维对应点，求出相机相对于对象坐标系的位姿。

通俗一点说，就是回答“这个三维物体是怎么被相机拍到的”。求出来的 rvec 和 tvec 之后，就可以配合 ProjectPoints 进行重投影验证，或者继续做 AR 叠加、机器人抓取定位、视觉测量等任务。

3. 函数公式

$\min_{r,t}\sum_{i=1}^{N}\left\|u_i-\pi\left(K,D,R(r)X_i+t\right)\right\|_2^2$

其中：

$X_i$ 是三维对象点；
$u_i$ 是图像中的二维观测点；
$K$ 是相机内参矩阵；
$D$ 是畸变系数；
$R (r)$ 是由旋转向量 rvec 通过 Rodrigues 公式转换得到的旋转矩阵；
$π(⋅)\pi(\cdot)$ 表示投影到图像平面。

4. 函数原理说明

OpenCV 会把三维点先从对象坐标系变换到相机坐标系，再通过相机模型投影到二维像素平面。求解过程本质上是一个非线性最小二乘问题：让“投影出来的点”和“真实观测到的点”尽量接近。

对于初学者来说，最重要的理解点有两个。

第一，rvec 不是欧拉角，而是旋转向量。它的长度等于旋转角度，方向等于旋转轴。OpenCV 内部会用 Rodrigues 把它转成旋转矩阵。

第二，tvec 不是世界坐标本身，而是对象坐标系原点在相机坐标系中的位置表达。也就是说，rvec 和 tvec 一起描述了“对象到相机”的变换。

当 useExtrinsicGuess 为 false 时，OpenCV 会自己从头估计位姿；当它为 true 时，函数会把你提供的初值当作优化起点继续迭代。

5. 参数含义解析

参数	说明	初学者要点
`objectPoints`	三维对象点	每个点都必须和 `imagePoints` 一一对应
`imagePoints`	二维图像点	单位是像素，不是归一化坐标
`cameraMatrix`	相机内参	必须是 3x3 矩阵
`distCoeffs`	畸变系数	没有畸变时可传全 0
`rvec`	输出旋转向量	求解结果写到这里
`tvec`	输出平移向量	求解结果写到这里
`useExtrinsicGuess`	是否使用初值	没有初值时设为 `false`
`flags`	求解方法	入门阶段通常先用 `Iterative`

6. 应用场景列表

增强现实中，把虚拟物体稳定叠加到真实画面上。
工业视觉里，根据标记点求工件姿态。
机器人抓取时，估计目标零件相对相机的位姿。
相机标定完成后，用已知板点回测标定结果是否合理。
视觉教学中，用来理解“3D 点如何对应到 2D 像素”。

7. 函数使用示例

下面的 Console 代码会先合成一组三维点和二维点，再调用 SolvePnP，最后把估计位姿重新投影回图像中，计算平均误差和 RMS 误差。

using System;
using System.Globalization;
using OpenCvSharp;

namespace Demo;

internal static class Program
{
    private static void Main()
    {
        // 让中文在控制台里正常显示。
        Console.OutputEncoding = System.Text.Encoding.UTF8;

        // 准备三维对象点，这里故意使用非平面点，便于演示一般 PnP 场景。
        var objectPoints = new[]
        {
            new Point3f(-40, -40, 0),
            new Point3f( 40, -40, 0),
            new Point3f( 40,  40, 0),
            new Point3f(-40,  40, 0),
            new Point3f(-20, -20, 40),
            new Point3f( 20, -20, 40),
            new Point3f( 20,  20, 40),
            new Point3f(-20,  20, 40),
            new Point3f(  0,   0, 80),
            new Point3f(-60,  10, 30),
            new Point3f( 60,  10, 30),
            new Point3f(  0, -60, 20),
        };

        // 真实位姿只用于合成测试数据，实际业务里通常不会提前知道它。
        var groundTruthRvec = new[] { 0.20, -0.14, 0.28 };
        var groundTruthTvec = new[] { 18.0, -16.0, 660.0 };

        // 相机内参矩阵，fx/fy 是焦距，cx/cy 是主点坐标。
        var cameraMatrix = new double[,]
        {
            { 760.0,   0.0, 400.0 },
            {   0.0, 758.0, 260.0 },
            {   0.0,   0.0,   1.0 },
        };

        // 本示例先不考虑畸变，直接传全 0。
        var distCoeffs = new[] { 0.0, 0.0, 0.0, 0.0, 0.0 };

        // 把三维点按真实位姿投影到图像平面，作为后续求解的输入观测值。
        Cv2.ProjectPoints(objectPoints, groundTruthRvec, groundTruthTvec, cameraMatrix, distCoeffs, out Point2f[] imagePoints, out _);

        // 把数组包装成 OpenCvSharp 的输入输出容器。
        using var objectMat = Mat.FromArray(objectPoints);
        using var imageMat = Mat.FromArray(imagePoints);
        using var cameraMat = Mat.FromArray(cameraMatrix);
        using var distMat = Mat.FromArray(distCoeffs);
        using var rvecMat = new Mat();
        using var tvecMat = new Mat();

        using var objectInput = InputArray.Create((Mat)objectMat);
        using var imageInput = InputArray.Create((Mat)imageMat);
        using var cameraInput = InputArray.Create((Mat)cameraMat);
        using var distInput = InputArray.Create((Mat)distMat);
        using var rvecOutput = OutputArray.Create(rvecMat);
        using var tvecOutput = OutputArray.Create(tvecMat);

        // 直接调用 SolvePnP，让 OpenCV 估计位姿。
        Cv2.SolvePnP(objectInput, imageInput, cameraInput, distInput, rvecOutput, tvecOutput, false, SolvePnPMethod.Iterative);

        // 从 Mat 里读回 3 个元素，方便控制台输出。
        var estimatedRvec = ReadVector3(rvecMat);
        var estimatedTvec = ReadVector3(tvecMat);

        // 再次投影，用来检查估计位姿和观测点之间的误差。
        Cv2.ProjectPoints(objectPoints, estimatedRvec, estimatedTvec, cameraMatrix, distCoeffs, out Point2f[] reprojectionPoints, out _);

        // 统计误差，帮助判断求解结果是否稳定。
        var meanError = ComputeMeanError(imagePoints, reprojectionPoints);
        var rmsError = ComputeRmsError(imagePoints, reprojectionPoints);

        Console.WriteLine("==== SolvePnP(InputArray) 结果 ====");
        Console.WriteLine($"真实 rvec = {FormatVector(groundTruthRvec)}");
        Console.WriteLine($"估计 rvec = {FormatVector(estimatedRvec)}");
        Console.WriteLine($"真实 tvec = {FormatVector(groundTruthTvec)}");
        Console.WriteLine($"估计 tvec = {FormatVector(estimatedTvec)}");
        Console.WriteLine($"平均重投影误差 = {meanError:F4} px");
        Console.WriteLine($"RMS 重投影误差 = {rmsError:F4} px");
        Console.WriteLine();

        // 打印前几组点，方便初学者对照观察。
        PrintPointPairs(objectPoints, imagePoints, reprojectionPoints, 6);
    }

    /// <summary>
    /// 从 Mat 中读取一个 3 维向量。
    /// </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 ComputeMeanError(Point2f[] expectedPoints, Point2f[] actualPoints)
    {
        var count = Math.Min(expectedPoints.Length, actualPoints.Length);
        var sum = 0.0;
        for (var index = 0; index < count; index++)
        {
            var dx = expectedPoints[index].X - actualPoints[index].X;
            var dy = expectedPoints[index].Y - actualPoints[index].Y;
            sum += Math.Sqrt(dx * dx + dy * dy);
        }

        return sum / count;
    }

    /// <summary>
    /// 计算 RMS 误差。
    /// </summary>
    private static double ComputeRmsError(Point2f[] expectedPoints, Point2f[] actualPoints)
    {
        var count = Math.Min(expectedPoints.Length, actualPoints.Length);
        var sum = 0.0;
        for (var index = 0; index < count; index++)
        {
            var dx = expectedPoints[index].X - actualPoints[index].X;
            var dy = expectedPoints[index].Y - actualPoints[index].Y;
            sum += dx * dx + dy * dy;
        }

        return Math.Sqrt(sum / count);
    }

    /// <summary>
    /// 格式化向量，方便输出。
    /// </summary>
    private static string FormatVector(double[] values)
    {
        return $"[{string.Join(", ", values.Select(value => value.ToString("F6", CultureInfo.InvariantCulture)))}]";
    }

    /// <summary>
    /// 打印若干组对应点，帮助核对输入是否正确。
    /// </summary>
    private static void PrintPointPairs(Point3f[] objectPoints, Point2f[] imagePoints, Point2f[] reprojectionPoints, int maxCount)
    {
        var count = Math.Min(Math.Min(objectPoints.Length, imagePoints.Length), reprojectionPoints.Length);
        count = Math.Min(count, maxCount);

        for (var index = 0; index < count; index++)
        {
            Console.WriteLine($"P{index + 1}: 3D=({objectPoints[index].X:F1}, {objectPoints[index].Y:F1}, {objectPoints[index].Z:F1})  " +
                              $"观测=({imagePoints[index].X:F2}, {imagePoints[index].Y:F2})  " +
                              $"重投影=({reprojectionPoints[index].X:F2}, {reprojectionPoints[index].Y:F2})");
        }
    }
}