C#实现物体尺寸测量（利用坐标转换）

由于需要实现一个物体的测量，但是已有QT程序，最后的整体功能需要在C#集成实现。

首先有两个方案：（1）利用已有的QT程序以及界面，直接在C#中调用QT，或者C++程序，但是经过尝试，发现两者之间进行调用不是那么的简单，涉及到许多变量定义的不用以及数据结构的不同。因此决定方案（2），在C#里重新实现该功能。

由于也是第一次接触相机的使用，因此就借此记录一下。

一、首先是相机的标定，这个很简单，也有大量的相关参考：

相机标定(一)——内参标定与程序实现_相机内参标定_white_Learner的博客-CSDN博客相机标定(一)——内参标定与程序实现相机标定(二)——图像坐标与世界坐标转换相机标定(三)——手眼标定一、张正友标定算法实现流程1.1 准备棋盘格备注：棋盘格黑白间距已知，可采用打印纸或者购买黑白棋盘标定板（精度要求高）1.2 针对棋盘格拍摄若干张图片此处分两种情况（1）标定畸变系数和相机内参，拍摄照片需要包含完整棋盘，同时需要不同距离，不同方位...https://blog.csdn.net/Kalenee/article/details/80672785

OpenCvSharp 棋盘格标定助手_opencvsharp 标定_YT - Chow的博客-CSDN博客使用的是VS调用OpenCvSharp资源库进行一个Winform操作界面编写，网上找了很多开源的程序，发现根本用不了的，用的时候还需要你配置各种电脑系统变量，显得好麻烦。现在弄了个简单的标定助手，可以完美运行，带有棋盘格图像生成工具，操作简单，源码也不复杂。使用了OpenCvSharp资源开发包，在VS下做了一个棋盘格图像下的相机标定助手小Demo，显然，C#也可以用OpenCv了。这是一个比较好的案例，可以参考下。鄙人不才，也用它做了一个SFM三维重建的Demo，这里就不放了。using Op.https://blog.csdn.net/Yoto_Jo/article/details/117574528?utm_medium=distribute.pc_feed_404.none-task-blog-2~default~BlogCommendFromBaidu~Rate-11-117574528-blog-null.pc_404_mixedpudn&depth_1-utm_source=distribute.pc_feed_404.none-task-blog-2~default~BlogCommendFromBaidu~Rate-11-117574528-blog-null.pc_404_mixedpud

因为OpencvSharp本来也是由Opencv封装成的C#的动态链接库，因此本质上使用的方法和基本的函数实现是差不多的，可以直接参考C++版本的相机标定。只需要注意个人使用的时候修改标定格的具体物理大小和标定格的个数。

二、像素坐标转世界坐标

当标定好相机后，就需要实现如何通过像素坐标转到世界坐标，理论和实现过程可以参考C++版本

相机标定(二)——图像坐标与世界坐标转换_标定将图像坐标转换成世界坐标_white_Learner的博客-CSDN博客因本文存在错误与模糊之处，为此进行重写修改，但因CSDN不支持富文本转换Markdown，所以重写发布新的博文：https://blog.csdn.net/Kalenee/article/details/99207102一、坐标变换详解1.1 坐标关系相机中有四个坐标系，分别为world，camera，image，pixelworld为世界坐标系，可以任意指定轴和轴，...https://blog.csdn.net/Kalenee/article/details/80659489

  public static Point3d GetWorldPoints(Point2f inPoints)
        {
            double s;
            int zConst = 0;
            Mat r = new Mat(3, 3, MatType.CV_64FC1);
            Mat t = new Mat(3, 1, MatType.CV_64FC1);
            using (var fs1 = new FileStorage("RT.yaml", FileStorage.Mode.Read))
            {

                r = (Mat)fs1["R"];
                t = (Mat)fs1["t"];

            }
            Mat R_invert = new Mat(3, 3, MatType.CV_64FC1);
            Mat cameraMatrix_invert = new Mat(3, 3, MatType.CV_64FC1);
            Mat imagePoint = new Mat(3, 1, MatType.CV_64FC1);

            imagePoint.At<double>(0, 0) = inPoints.X;
            imagePoint.At<double>(1, 0) = inPoints.Y;
            imagePoint.At<double>(2, 0) = 1;


            Mat cameraMatrix = new Mat(3, 3, MatType.CV_64FC1, new double[3, 3] { 
              { 1473.819, 0, 615.859 },
              { 0, 1474.14, 467.697 },
              { 0, 0, 1 } });
           
            Cv2.Invert(r, R_invert, DecompTypes.SVD);
            Cv2.Invert(cameraMatrix, cameraMatrix_invert, DecompTypes.SVD);
          
            Mat tempMat = R_invert * cameraMatrix_invert * imagePoint;
            Mat tempMat2 = R_invert * t;
            s = zConst + tempMat2.At<double>(2, 0);
            s /= tempMat.At<double>(2, 0);

            //计算世界坐标
            Mat wcPoint;
            wcPoint = R_invert * ( cameraMatrix_invert*s * imagePoint - t);
            Point3d world;
            world.X = wcPoint.At<double>(0, 0);
            world.Y = wcPoint.At<double>(1, 0);
            world.Z = wcPoint.At<double>(2, 0);
            //Point3f worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));
            return world;
        }

 可以看出来几乎用C++的版本就能实现。

这里需要注意的是Cv2.Invert（）矩阵求逆的函数，他的第三个参数 DecompTypes的不同，求逆的方法也不同。但是我自己的测试得到的结果却只有 DecompTypes.SVD这种方法比较准确，不知道各位有没有什么经验对于求逆的方法选择。

然而在函数写好后测试像素坐标到世界坐标却始终达不到要求，经过师兄指导是因为自己这里用到的外参（旋转矩阵和平移向量）是不对的，原因在于我是直接用的Cv2.CalibrateCamera（）求解得到的外参进行计算的，而我在实验的时候拍摄条件已经变了，当时的外参已经不适用了。因而在求解之前还需要得到当前相机的外参。犯这个错误也是没有整体掌握好对于整个实现过程的理解，没有对相机坐标转换理论的理解，导致外参与物体的对应有问题，因此下面就是求解当前相机的外参。

三、外参求解

由于需要通过已知的物体像素坐标转换为世界坐标，因此就需要该相机和该物体此时的位置姿态的变化。因为求解出外参就能够反推出物体的实际世界坐标。

扫描二维码关注公众号，回复： 17205400 查看本文章

需要注意的是，我的实际环境是相机处于不动的情况，即相机始终与地面保持平行，固定相机的位置。因此若需要在相机运动或者相对位置随时发生改变的情况下使用，需要有所修改。

所用到的函数也很简单，用到的是OpencvSharp自带的SolvePnP进行求解

 从函数的参数中可以看到，需要我们准备的有objectPoints, imagePoints, Intrinsic, distCoeffs, rvec, tvec,  SolvePnPFlags，我认为这几个是比较关键的参数。

首先是objectPoints：需要提供一组世界坐标

          imagePoints：需要提供一组与世界坐标对应的像素坐标

          Intrinsic, distCoeffs：相机的内参和畸变系数

          rvec, tvec：输出一个旋转向量和平移向量，通常情况下旋转向量需要用Cv2.Rodrigues（）转换为旋转矩阵

          SolvePnPFlags：求解的方法选择，不同参数选择要求的输入点也有所要求，可以参考相关文章进行选择选择

这里对于objectPoints和imagePoints点的取值我用了两种方法。

其一是利用标定纸，通过程序寻找标定纸的角点坐标，然后在用尺子实际测出各个角点的世界坐标，这里需要注意的是对于世界坐标的X,Y轴的选取，以角点最左上角为原点，向右形成X轴，向下形成Y轴。一定要注意世界坐标点与像素坐标点的对应关系。

           

 从得到的角点可以看出，我的标定纸有横向五个角点纵向四个角点，而通过Cv2.FindChessboardCorners（）得到的角点坐标是以图片的最左上角为坐标原点，向右形成X轴，向下形成Y轴。且存放的点的顺序是以左上角第一个点开始，从左向右，从上至下，即第一排角点的索引以此为0，1，2.....，第二排开始为5。

下面是求解过程

public static void GetRvec()
        {
            double meanDistance = 0;
            double sumDistance = 0;
            int numPairs = 0;
            Mat Intrinsic = new Mat(3, 3, MatType.CV_32FC1, new double[] {  1473.81, 0, 615.85 , 0, 1474.14, 467.69 , 0, 0, 1  });
            Mat distCoeffs = new Mat(5,1, MatType.CV_32FC1, new double[] { 0.051, 0.44, -0.01, -0.009, -2.22 });
             string calibImagesPath = "标定图片/"; // 标定图片所在目录
            int boardWidth = 5; // 棋盘格宽度（内角点个数）
            int boardHeight = 4; // 棋盘格高度（内角点个数）
            float squareSize = 36.1F; // 棋盘格单个方格的边长（毫米）
            int k=0;
            Point3d[] objectPoints = new Point3d[20];
           Point2d[] ww = new Point2d[20] ;
             for (int i = 0; i < boardHeight; i++)
            {
                for (int j = 0; j < boardWidth; j++)
                {

                   // objectPoints.Add(new Point3d(j * squareSize, i * squareSize, 0));
                    objectPoints[k].X = j * squareSize;
                    objectPoints[k].Y = i * squareSize;
                    objectPoints[k].Z = 0;
                    ww[k].X = j * squareSize;
                    ww[k].Y = i * squareSize;
                    k++;

                }
            }

         
            // 提取图像中的角点
            Mat gray = new Mat();
            Cv2.CvtColor(calibImage, gray, ColorConversionCodes.BGR2GRAY);
            Point2f[] corners;
            Point2f[] imagepoint = new Point2f[4];
            bool found = Cv2.FindChessboardCorners(gray, new Size(boardWidth, boardHeight), out corners);
            if (found)
            {
                TermCriteria criteria = new TermCriteria(CriteriaType.MaxIter | CriteriaType.Eps, 30, 0.001);
                Cv2.CornerSubPix(gray, corners, new Size(11, 11), new Size(-1, -1), criteria);
                Cv2.Find4QuadCornerSubpix(gray, corners, new Size(5, 5));


            }

           
            foreach (Point2f p in corners)
            {
                Cv2.Circle(gray, (int)p.X, (int)p.Y, 5, new Scalar(0, 0, 255), 2);
            }
            Cv2.ImShow("Image with Corners", gray);


            Mat rvec = new Mat(); // 旋转向量
            Mat tvec = new Mat(); // 平移向量
            InputArray imagePoints = InputArray.Create(corners);
            
            InputArray objectPoints1 = InputArray.Create(objectPoints);
            Cv2.SolvePnP(objectPoints1, imagePoints, Intrinsic, distCoeffs, rvec, tvec, false, SolvePnPFlags.Iterative);
            

            Mat rotMatrix = new Mat(3, 3, MatType.CV_32FC1);
            Cv2.Rodrigues(rvec, rotMatrix);

            using (var fs1 = new FileStorage("test5.yaml", FileStorage.Mode.Write))
            {


                fs1.Add("rvecsMat").Add(rotMatrix);
                fs1.Add("tvecsMat").Add(tvec);

            }
           
        }

方法二是通过手动获取图片的像素坐标，这个就可以使用任何图片，通过手动点击获取坐标，世界坐标任然通过实际测量得到。

 private void button6_Click(object sender, EventArgs e)
        {
            MouseCallback draw = new MouseCallback(draw_circle);
            Mat src = Cv2.ImRead(@"你自己的图片", ImreadModes.AnyColor);
            Cv2.ImShow("src image", src);
            tempMat = new Mat(src.Size(), src.Type());
            Cv2.CopyTo(src, tempMat);
            System.Runtime.InteropServices.GCHandle handle = System.Runtime.InteropServices.GCHandle.Alloc(src);
            IntPtr ptr = System.Runtime.InteropServices.GCHandle.ToIntPtr(handle);
            Cv2.SetMouseCallback("src image", draw, ptr);
           
        }

        static Mat tempMat;
        static Point2f[] a = new Point2f[4];
        static int i = 0;
       
        public static void draw_circle(MouseEventTypes @event, int x, int y, 
             MouseEventFlags flags, IntPtr userData)
        {
            System.Runtime.InteropServices.GCHandle handle = 
            System.Runtime.InteropServices.GCHandle.FromIntPtr(userData);
            Mat src = (Mat)handle.Target;
            if (@event == MouseEventTypes.LButtonDown)
            {
               
                a[i].X = x;
                a[i].Y = y;
                i++;

              
            }
         }

上述代码是在按键触发时执行读取照片并创建鼠标的点击事件，对于鼠标的操作自己不太熟悉，因为要使用就随便找了个例程改改这是能实现基本的鼠标点击功能。这里由于采用的手动点击的方式，因此点的个数就设置得比较少，只采用了四个点，根据自己实际情况做调整，最好点得个数不少于4个。

得到了相机得外参，当我将已知像素坐标带入其中得时候，却发现并没有得到我想要得结果，经过几周的调试加计算，最后通过C++版本的求解过程一一对比，每一步我都输出结果进行比对，发现在SolvePnP（）这儿出现了问题，在带入参数之前，一切都与C++版本的结果已知，但是通过SolvePnP（）求解的外参就对不上，尝试改变输入的数据格式，以及数据的个数，还有求解方法，始终与C++版本对不上，于是我用C++求得的外参带入我自己的后续程序中，最后能够比较准确的得到世界坐标。说明就是SolvePnP（）函数的问题。

但是很遗憾，我仍然没有找到解决的方法，不知道各位大佬有没有什么办法或者我哪里出现了问题。

为了实现这一步，我最后采用的是用C++来调用C++版本的SolvePnP（）函数，将其封装为Dill动态连接库，让C#进行调用。

四、C++封装DILL给C#调用

参考其他文章的C++封装过程，基本上跟着步骤来就没啥大问题。

C#调用OpenCV（C++原版）思路和实现方法（小白教程）_c# opencv_SteveDraw的博客-CSDN博客为什么要本地安装呢？因为既然调用那么必须是要获得相应OpenCV接口的调头文件或者C++文件！c#和C++虽然两者衍生自C语言爸爸，两者更是有多个类似的地方，但是终究语言环境的差异，这两者并不能互通，但是做好接口和生成和调用.dll（动态链接库）就可以无缝连接，这也是目前C#做视觉应用的一个常用点！既然要用到第一种方法那么就要建立一个C++空项目来生成.dll文件！点击头文件夹，右键点击添加->添加新建项（或者点击头文件后，快捷键Ctrl+Shift+A）接着添加demo.h内容：2.添加cpp文https://blog.csdn.net/SteveZhou212/article/details/125103432 

#include <iostream>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgcodecs/legacy/constants_c.h"
#include <opencv2/opencv.hpp>
#include <time.h>
#include"getRT.h"  //这里对应你新建的那个头文件
#include <vector>

using namespace std;
using namespace cv;

void toCV()
{
    vector<Point2d> imagepoint;
    Mat imagepoint1 = Mat(4, 2, CV_64FC1);
    Mat objectpoint1 = Mat(4, 3, CV_64FC1);
    cv::FileStorage fs("point.yaml", FileStorage::READ);
    fs.open("point.yaml", cv::FileStorage::READ);
    fs["point"]>>imagepoint1;
    fs["worldpoint"] >> objectpoint1;
   
    fs.release();

   
    //imagepoint1.
   /* vector<Point3d> objP;
   
    objP.clear();
    objP.push_back(Point3d(0, 0, 0));
    objP.push_back(Point3d(212.0f, 0, 0));
    objP.push_back(Point3d(212.0f, 298.3f, 0));
    objP.push_back(Point3d(0, 298.3f, 0));
    */

    
    

    Mat intrinsic = (Mat_<double>(3, 3) << 1473.819, 0, 615.859 ,
         0, 1474.14, 467.697,
         0, 0, 1 );
    Mat dis = (Mat_<double>(5,1) << 0.051, 0.44, -0.01, -0.009, -2.22);
    Mat rvec = Mat(3, 1, CV_64FC1, Scalar::all(0));
    Mat tvec = Mat(3, 1, CV_64FC1, Scalar::all(0));
    Mat rotM = Mat(3, 3, CV_64FC1, Scalar::all(0));
    solvePnP(objectpoint1, imagepoint1, intrinsic, dis, rvec, tvec,false,SOLVEPNP_EPNP );
    Rodrigues(rvec, rotM);  //将旋转向量变换成旋转矩阵
  
    cv::FileStorage fd("RT.yaml", FileStorage::WRITE);
    fd << "R" << rotM;
    fd << "t" << tvec;
    fd << "point" << imagepoint1;
    fd.release();
}
void main()
{
    toCV();
}

只需要在C#工程中引用该函数就行，记得添加这两行代码

[DllImport("getRT.dll")]
private extern static void toCV();

最后在需要用到PNP求解的地方调用toCV就行。

下面是最后实现的效果，首先是对物体进行检测，找到最小外接圆，得到圆心与半径

 最后通过计算得到我的手机对角线长为166mm，通过屏幕尺寸6.4英寸换算为162.56mm,考虑到手机并不是全面屏，上下巴加上一点，误差应该在5mm以内。

第一次接触C#，可能很多地方表达不太清楚，也是顺便记录一下整个过程，希望能够帮助到有需要的人。

另外就是SolvePnP（）求解的问题，不知道有没有大佬知道为啥我的求解始终不对。文章有不足或者不清楚的地方希望大家指正。