基于RM比赛的飞镖检测总结（三维构建）

最新推荐文章于 2024-03-03 12:41:22 发布

心如坚石

最新推荐文章于 2024-03-03 12:41:22 发布

阅读量1.3k

点赞数 1

分类专栏： opencv RM 文章标签：计算机视觉

本文链接：https://blog.csdn.net/qq_32740315/article/details/106824344

版权

opencv 同时被 2 个专栏收录

6 篇文章 1 订阅

订阅专栏

5 篇文章 0 订阅

订阅专栏

目标需求与前期准备

比赛的场地是16m*28m,雷达放置在己方基地的高3m处，检测到飞镖，得到他的三维坐标，将三维坐标发给nuc。

因为单目相机的测距虽然可以采用solvepnp的方法，但是因为你对运动的飞镖进行二值化形成的区域大小很容易受到外部环境的影响，所以完全无法提供准确的坐标。因此我们必须采用双目相机才能提供准确的坐标，提高哨兵的拦截准确度。而实验室只有6mm、8mm、12mm三个镜头的相机，参考了大疆的各种参数的选型，所以我根据自己实验室的已有相机。

几个参数选取

相机型号：大华5000系列USB3面阵工业相机 A5131M/CU210

帧率：最大可达到210FPS

焦距：8mm

图像分辨率： 1280*1080

基线：暂定

单目相机标定

直接而在MATLAB的Command Window里面输入cameraCalibrator即可调用标定应用。

首先先把之前照好的图像添加进去，这时出现：

undefined

我的标定板是30mm的，所以这里我选择的是30mm。

在这里有人推荐使用2个参数,但是我觉得这个最好视情况而定。

undefined

在OpenCV中的畸变系数的排列（这点一定要注意k1，k2，p1，p2，k3）。

RadialDistortion对应k1，k2，k3设置为0了。
TangentialDistortion对应p1，p2。
IntrinsicMatrix对应内参，注意这个和OpenCV中是转置的关系，注意不要搞错。

使用下面这段代码可以查看一下自己标定的效果。

#include "opencv2/opencv.hpp"
#include <iostream>

using namespace cv;
using namespace std;

int main()
{
    VideoCapture inputVideo(0);
    if (!inputVideo.isOpened())
    {
        cout << "Could not open the input video: " << endl;
        return -1;
    }
    Mat frame;
    Mat frameCalibration;

    inputVideo >> frame;
    Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
    cameraMatrix.at<double>(0, 0) = 4.450537506243416e+02;
    cameraMatrix.at<double>(0, 1) = 0.192095145445498;
    cameraMatrix.at<double>(0, 2) = 3.271489590204837e+02;
    cameraMatrix.at<double>(1, 1) = 4.473690628394497e+02;
    cameraMatrix.at<double>(1, 2) = 2.442734958206504e+02;

    Mat distCoeffs = Mat::zeros(5, 1, CV_64F);
    distCoeffs.at<double>(0, 0) = -0.320311439187776;
    distCoeffs.at<double>(1, 0) = 0.117708464407889;
    distCoeffs.at<double>(2, 0) = -0.00548954846049678;
    distCoeffs.at<double>(3, 0) = 0.00141925006352090;
    distCoeffs.at<double>(4, 0) = 0;

    Mat view, rview, map1, map2;
    Size imageSize;
    imageSize = frame.size();
    initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
        getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
        imageSize, CV_16SC2, map1, map2);


    while (1) //Show the image captured in the window and repeat
    {
        inputVideo >> frame;              // read
        if (frame.empty()) break;         // check if at end
        remap(frame, frameCalibration, map1, map2, INTER_LINEAR);
        imshow("Origianl", frame);
        imshow("Calibration", frameCalibration);
        char key = waitKey(1);
        if (key == 27 || key == 'q' || key == 'Q')break;
    }
    return 0;
}

双目标定

虽然opencv提供了StereoCalibrate函数，按照网上说法效果不是很好，我没有验证过，所以还是选择matlab工具箱标定。

获得的结果参数

内参	外参
左右相机矩阵	相机之间的旋转矩阵
畸变系数	二者之间的平移向量

matlab标定

在命令行输入stereoCameraCalibrator，出现如下界面：

将上面的“Skew”、“Tangential Distortion”以及“3 Coefficients或者2 Coefficients”等选项选上如下：

下面就是加载图像（copy别的地方的博客图片，因为我在Ubuntu下面使用）

undefined

在这里camera1表示左相机，camera2表示右相机。图片后面补充。

输出参数意思

TranslationOfCamera2：相机2相对于相机1的偏移矩阵，可以直接使用。也就是相机2变换到相机下面坐标。
RotationOfCamera2：相机2相对于相机1的旋转矩阵，需要转置之后才能使用。

CameraParameters1与CameraParameters2为左右摄像头的单独标定参数，与上面的单目相机一致。

致谢

驱动双目相机

对于自制的双目相机,实际上只要使用i/o ionput1,和接地的i/o grand,因为实验室的大华相机的fps为210，所以的板子采用的频率为210hz。此外工业相机的软件部分应当将连续采图模式转变为下降沿或者上升沿触发。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-lpBVbV0H-1592445790399)(/home/demon/.config/Typora/typora-user-images/image-20200507183319084.png)]

如果使用桌面应用程序的化，大华相机的客户端在linux和ubuntu下面有区别的，windows下面可以驱动两个相机，linux下面只能一个

在这里主要修改红色方框内的三个地方，将triggermode模式打开，将triggersource设置为外部触发，选择上升沿触发模式。

致谢

三维空间定位问题（光轴平行模型）

双目校正

首先，先看看视差的定义，三维世界的物点落在左右两眼的视网膜上的几何投影位于不同的位置，即在视网膜上存在差异，通常我们把这种差异称作视差。为了计算视差，我们需要在左右视图上将两个像素点匹配起来，然而，在二维空间上匹配对应点是非常耗时的，为了减少匹配搜索范围，我们可以利用极线约束使得对应点的匹配由二维搜索降为一维搜索。而双目校正的作用就是要把消除畸变后的两幅图像严格地行对应，使得两幅图像的对极线恰好在同一水平线上，这样一幅图像上任意一点与其在另一幅图像上的对应点就必然具有相同的行号，只需在该行进行一维搜索即可匹配到对应点。

undefined

在这幅图里面，PO1O2组成的平面是极平面；

O1O2为基线，e1、e2为极点；p1e1和p2e2为极线。

立体校正（双目校正）就是，把实际中非共面行对准的两幅图像，校正成共面行对准，如上图右下角所示，这样就可以让极线在同一条直线上面，比如说在左图出现的一点，其匹配点一定在右图的极线上。

下面这幅图就是我对两幅图的立体校正。可以看到两幅图的每个点都在一条直线上。

双目校正.png

立体匹配

双目相机测距的原理，我在此放一张slam书籍上一页说明。

双目测距原理.png

以下显示的是三维深度图

三维深度图.png

代码显示方法

下面代码里面，我提供了两种匹配算法，分别是sgbm匹配算法和bm算法。

/******************************
      立体匹配和测距
/******************************/


#include <opencv2/opencv.hpp>
#include <iostream>
#include "opencv2/imgcodecs.hpp"
#include <pretreatment.hpp>
#include <radar_main.hpp>
#include <shm.hpp>

using namespace std;
using namespace cv;


cv:: Mat img;

const int imageWidth = 1280;                             //摄像头的分辨率
const int imageHeight = 1024;
Size imageSize = Size(imageWidth, imageHeight);

Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;

Rect validROIL;//图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域
Rect validROIR;

Mat mapLx, mapLy, mapRx, mapRy;     //映射表
Mat Rl, Rr, Pl, Pr, Q;              //校正旋转矩阵R，投影矩阵P 重投影矩阵Q
Mat xyz;              //三维坐标

Point origin;         //鼠标按下的起始点
Rect selection;      //定义矩形选框
bool selectObject = false;    //是否选择对象

int blockSize = 0, uniquenessRatio =0, numDisparities=0;
Ptr<StereoBM> bm = StereoBM::create(16, 9);


cv::Ptr<cv::StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);
int numberOfDisparities = ((imageSize.width / 8) + 15) & -16;


/*
事先标定好的相机的参数
fx 0 cx
0 fy cy
0 0  1
*/

Mat cameraMatrixL = (Mat_<double>(3, 3) << 1705.8,-1.1,730.4,
        0,1712.5,542.2,
        0, 0, 1);
Mat distCoeffL = (Mat_<double>(5, 1) << -0.0441, -0.3753, 0.0024, 0.0115, 0);

Mat cameraMatrixR = (Mat_<double>(3, 3) << 1709.2,-1.6,730.6,
        0,1711.6,484.2,
        0, 0, 1);
Mat distCoeffR = (Mat_<double>(5, 1) << -0.0837, 0.1398, -0.0058, 0.0112, 0);

Mat T = (Mat_<double>(3, 1) << -281.0872,-6.1517,-13.0867);//T平移向量

Mat R = (Mat_<double>(3, 3) << 0.9989,-0.0065,-0.0454,
        0.0053,0.9996,-0.0271,
        0.0456,0.0268,0.9986);//R 旋转矩阵



/*****立体匹配*****/
//bm算法
void stereo_match(int,void*)
{
    bm->setBlockSize(2*blockSize+5);     //SAD窗口大小，5~21之间为宜
    bm->setROI1(validROIL);
    bm->setROI2(validROIR);
    bm->setPreFilterCap(29);
    bm->setMinDisparity(0);  //最小视差，默认值为0, 可以是负值，int型
    bm->setNumDisparities(numDisparities*16+16);//视差窗口，即最大视差值与最小视差值之差,窗口大小必须是16的整数倍，int型
    bm->setTextureThreshold(10);
    bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(-1);
    Mat disp, disp8;
    bm->compute(rectifyImageL, rectifyImageR, disp);//输入图像必须为灰度图
    disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式
    reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
    xyz = xyz * 16;
    Point p;
    p.x =600,p.y=500;
    cout<<p<< "in world coordinate: " << xyz.at<Vec3f>(p)*16 <<disp8.size()<<endl;
    imshow("disparity", disp8);
}

//sgbm算法
void stereo_match_sgbm(int, void*)
{
    sgbm->setPreFilterCap(32);
    int SADWindowSize = 9;
    int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
    sgbm->setBlockSize(sgbmWinSize);
    int cn = rectifyImageL.channels();
    sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
    sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
    sgbm->setMinDisparity(0);
    sgbm->setNumDisparities(numberOfDisparities);
    sgbm->setUniquenessRatio(10);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setSpeckleRange(32);
    sgbm->setDisp12MaxDiff(1);
    sgbm->setMode(cv::StereoSGBM::MODE_SGBM);

    Mat disp, disp8;
    sgbm->compute(rectifyImageL, rectifyImageR, disp);//输入图像必须为灰度图

    disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式
    reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
    xyz = xyz * 16;
    imshow("disparity", disp8);
}

/*****描述：鼠标操作回调*****/

static void onMouse(int event, int x, int y, int, void*)
{
    if (selectObject)
    {
        selection.x = MIN(x, origin.x);
        selection.y = MIN(y, origin.y);
        selection.width = std::abs(x - origin.x);
        selection.height = std::abs(y - origin.y);
    }

    switch (event)
    {
        case EVENT_LBUTTONDOWN:   //鼠标左按钮按下的事件
            origin = Point(x, y);
            selection = Rect(x, y, 0, 0);
            selectObject = true;
            cout << origin <<"in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
            break;
        case EVENT_LBUTTONUP:    //鼠标左按钮释放的事件
            selectObject = false;
            if (selection.width > 0 && selection.height > 0)
                break;
    }
}


/*****主函数*****/

int main() {

//    立体校正
    //Rodrigues(rec, R); //Rodrigues变换
    stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q,
                  CALIB_ZERO_DISPARITY,
                  0, imageSize, &validROIL, &validROIR);
    initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
    initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);


//    读取图片
    rgbImageL = imread("/home/demon/CLionProjects/two_camera/cmake-build-debug/1L.bmp");
    rgbImageR = imread("/home/demon/CLionProjects/two_camera/cmake-build-debug/1R.bmp");

    cvtColor(rgbImageL, grayImageL, COLOR_BGR2GRAY);
    cvtColor(rgbImageR, grayImageR, COLOR_BGR2GRAY);

    imshow("ImageL Before Rectify", grayImageL);
    imshow("ImageR Before Rectify", grayImageR);

//    经过remap之后，左右相机的图像已经共面并且行对准了

    remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
    remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

//    把校正结果显示出来

    Mat rgbRectifyImageL, rgbRectifyImageR;
    cvtColor(rectifyImageL, rgbRectifyImageL, COLOR_GRAY2BGR);  //伪彩色图
    cvtColor(rectifyImageR, rgbRectifyImageR, COLOR_GRAY2BGR);

    //单独显示
    //rectangle(rgbRectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);
    //rectangle(rgbRectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);
    imshow("ImageL After Rectify", rgbRectifyImageL);
    imshow("ImageR After Rectify", rgbRectifyImageR);

    //显示在同一张图上
    Mat canvas;
    double sf;
    int w, h;
    sf = 600. / MAX(imageSize.width, imageSize.height);
    w = cvRound(imageSize.width * sf);
    h = cvRound(imageSize.height * sf);
    canvas.create(h, w * 2, CV_8UC3);   //注意通道

    //左图像画到画布上
    Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到画布的一部分
    resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);     //把图像缩放到跟canvasPart一样大小
    Rect vroiL(cvRound(validROIL.x * sf), cvRound(validROIL.y * sf),                //获得被截取的区域
               cvRound(validROIL.width * sf), cvRound(validROIL.height * sf));
    //rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8);                      //画上一个矩形
    cout << "Painted ImageL" << endl;

    //右图像画到画布上
    canvasPart = canvas(Rect(w, 0, w, h));                                      //获得画布的另一部分
    resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
    Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y * sf),
               cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
    //rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);
    cout << "Painted ImageR" << endl;

    //画上对应的线条
    for (int i = 0; i < canvas.rows; i += 16)
        line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
    imshow("rectified", canvas);

//    立体匹配
    namedWindow("disparity", WINDOW_AUTOSIZE);
    // 创建SAD窗口 Trackbar
    createTrackbar("BlockSize:\n", "disparity", &blockSize, 8, stereo_match_sgbm);
    // 创建视差唯一性百分比窗口 Trackbar
    createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match_sgbm);
    // 创建视差窗口 Trackbar
    createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match_sgbm);
    //鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数，一般取0)
    setMouseCallback("disparity", onMouse, 0);
    stereo_match_sgbm(0, 0);

    waitKey(0);
    return 0;

}

函数单独解释

以下单独拿出几个重要函数说明一下（顺便自我复习一下）

1,罗德里格斯变换Rodrigues(rec, R); 虽然我没有用到这个函数，但是实际上是将旋转向量(rec)变为旋转矩阵®。

2,立体校正函数void stereoRectify(InputArray cameraMatrix1, InputArray distCoeffs1, 　　　　　　　　　　　InputArray cameraMatrix2,InputArray distCoeffs2, Size imageSize, 　　　　　　　　　　　InputArray R, InputArray T,OutputArray R1, OutputArray R2, OutputArray P1, 　　　　　　　　　　　OutputArray P2, OutputArray Q, int flags=CALIB_ZERO_DISPARITY, double alpha=-1, 　　　　　　　　　　　Size newImageSize=Size(), Rect* validPixROI1=0, Rect* validPixROI2=0 )

cameraMatrix1-第一个摄像机的摄像机矩阵、

distCoeffs1-第一个摄像机的畸变向量、同理第二个相机

imageSize-图像大小、

R与T- matlab标定求得的R矩阵和T、

R1和R2-输出矩阵，第一与第二个个摄像机的校正变换矩阵（旋转变换）

P1和P2-输出矩阵，第一和第二个摄像机在新坐标系下的投影矩阵、

Q-4*4的深度差异映射矩阵、

flags-可选的标志有两种零或者 CV_CALIB_ZERO_DISPARITY ,如果设置 CV_CALIB_ZERO_DISPARITY 的话，该函数会让两幅校正后的图像的主点有相同的像素坐标。否则该函数会水平或垂直的移动图像，以使得其有用的范围最大
alpha-拉伸参数。如果设置为负或忽略，将不进行拉伸。如果设置为0，那么校正后图像只有有效的部分会被显示（没有黑色的部分），如果设置为1，那么就会显示整个图像。设置为0~1之间的某个值，其效果也居于两者之间。
newImageSize-校正后的图像分辨率，默认为原分辨率大小。
validPixROI1-可选的输出参数，Rect型数据。其内部的所有像素都有效
validPixROI2-可选的输出参数，Rect型数据。其内部的所有像素都有效

3，计算畸变矫正和立体校正的映射变换void initUndistortRectifyMap(InputArray cameraMatrix, InputArray distCoeffs, InputArray R,InputArray newCameraMatrix, Size size, int m1type, OutputArray map1, OutputArray map2)

cameraMatrix-摄像机参数矩阵
distCoeffs-畸变参数矩阵
R- stereoRectify求得的R矩阵
newCameraMatrix-矫正后的摄像机矩阵（可省略）
Size-没有矫正图像的分辨率
m1type-第一个输出映射的数据类型，可以为 CV_32FC1 或 CV_16SC2
map1-输出的第一个映射变换
map2-输出的第二个映射变换

4, 几何变换函数 void remap(InputArray src, OutputArray dst, InputArray map1, InputArray 　　　　　　map2, int interpolation,int borderMode=BORDER_CONSTANT, const Scalar& 　　　　　　borderValue=Scalar())

src-原图像
dst-几何变换后的图像
map1-第一个映射，无论是点(x,y)或者单纯x的值都需要是CV_16SC2 ,CV_32FC1 , 或 CV_32FC2类型
map2-第二个映射，y需要是CV_16UC1 , CV_32FC1类型。或者当map1是点(x,y)时，map2为空。
interpolation-插值方法，但是不支持最近邻插值

第六个参数，int类型的borderMode，边界模式，有默认值BORDER_CONSTANT，表示目标图像中“离群点（outliers）”的像素值不会被此函数修改。

第七个参数，const Scalar&类型的borderValue，当有常数边界时使用的值，其有默认值Scalar( )，即默认值为0。

5，根据一组差异图像构建3D空间 voidreprojectImageTo3D(InputArray disparity,OutputArray _3dImage,InputArray Q, bool handleMissingValues=false, int ddepth=-1 )

disparity 视差图像。可以是8位无符号，16位有符号或者32位有符号的浮点图像。
_3dImage和视差图同样大小的3通道浮点图像。_3dImage(x,y)位置（也是视察图的(x,y)）包含3D坐标点。
Q 透视变换矩阵。可以通过stereoRectify()获得。
handleMissingValues是否处理缺失值（即点差距不计算）。如果handleMissingValues=true，则具有最小视差对应的异常值（见StereoBM::operator符（）），否则，具有非常大的Z值（目前设置为10000）转化为三维点的像素。
ddepth可选的输出数组的深度。如果是-1，输出图像将有深度CV_32F。也可以设置ddepth的CV_16S，的CV_32S或CV_32F。

致谢

此方法的另外一种思路

实际上

三维重建问题2（三角测量方法）

虽然有了上面的构建的三维图像，但是实际上在比赛中不会这么使用的，在实际比赛中双目相机检测到飞镖，那么取检测到的飞镖的中心点，就可以把他们当成匹配点来计算，因为我们使用的是工业相机，畸变系数其实很小，我们没有必要对整个图片进行矫正，只要对匹配点进行矫正就好了。

畸变矫正

单个点畸变矫正，公式原理可以参考网上（一大堆）。

cv::Point2f DoubleMeasurement::pixelDistortionCorrection(cv::Point2f &connor,Eigen::Matrix3d cameraMatrix,Eigen::Matrix<double, 5, 1> distCoeffs)
{
    Eigen::Vector3d pixel;
    Eigen::Vector3d P_cam, P_distortion;  //3行1列，distortion畸变
    pixel << connor.x, connor.y, 1;  //像素坐标


    P_cam = cameraMatrix.inverse() * pixel; //inverse是进行逆变换，求出Xc/Zc,即归一化后的坐标,内参矩阵

    double r2 = pow(P_cam.x(), 2) + pow(P_cam.y(), 2);//pow是求x的y次方
//公示后求出畸变坐标点
    P_distortion.x() = P_cam.x() * ( 1 + distCoeffs[0] * pow( r2, 1 ) + distCoeffs[1] * pow( r2, 2 ) + distCoeffs[2] * pow( r2, 3 ) )
                       + 2 * distCoeffs[3] * P_cam.x() * P_cam.y() + distCoeffs[4] * (r2 + 2 * pow(P_cam.x(), 2 ) );

    P_distortion.y() = P_cam.y() * ( 1 + distCoeffs[0] * pow( r2, 1 ) + distCoeffs[1] * pow( r2, 2 ) + distCoeffs[2] * pow( r2, 3 ) )
                       + distCoeffs[3] * ( r2 + 2 * pow( P_cam.y(), 2 ) ) + 2 * distCoeffs[4] * P_cam.x() * P_cam.y();
    P_distortion.z() = 1;

    cv::Point2f P;  //矫正后的点
    P.x=P_distortion.x()+connor.x;
    P.y=P_distortion.y()+connor.y;
    return P;
}

公式推导

三维变换.png

上面就是我对检测的原理推导，其实就是解两个矩阵组，这个矩阵方程就是像素坐标到实际坐标的变换，分别是左右相机。

上面的四个方程完全可以变成求解A*X=B;可以采用最小二乘法。

代码

在这套代码里面，是在左相机上建立世界坐标系

#include <opencv2/opencv.hpp>
#include <fstream>
#include <iostream>
 
using namespace std;
using namespace cv;
 
Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3]);
Point3f uv2xyz(Point2f uvLeft,Point2f uvRight);
 
//图片对数量
int PicNum = 14;
 
//左相机内参数矩阵
float leftIntrinsic[3][3] = {4037.82450,			 0,		947.65449,
									  0,	3969.79038,		455.48718,
									  0,			 0,				1};
//左相机畸变系数
float leftDistortion[1][5] = {0.18962, -4.05566, -0.00510, 0.02895, 0};
//左相机旋转矩阵  
float leftRotation[3][3] = {1,0,0,  0,1,0,  0,0,1 };
//左相机平移向量  
float leftTranslation[1][3] = {0,0,0};
 
//右相机内参数矩阵
float rightIntrinsic[3][3] = {3765.83307,			 0,		339.31958,
										0,	3808.08469,		660.05543,
										0,			 0,				1};
//右相机畸变系数
float rightDistortion[1][5] = {-0.24195, 5.97763, -0.02057, -0.01429, 0};
//右相机旋转矩阵
float rightRotation[3][3] = {-0.134947,		 0.989568,		-0.050442, 
							  0.752355,		 0.069205,		-0.655113, 
							 -0.644788,		-0.126356,		-0.753845};
//右相机平移向量
float rightTranslation[1][3] = {50.877397, -99.796492, 1507.312197};
 
 
int main()
{
	//已知空间坐标求成像坐标
	Point3f point(700,220,530);
	cout<<"左相机中坐标："<<endl;
	cout<<xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation)<<endl;
	cout<<"右相机中坐标："<<endl;
	cout<<xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation)<<endl;
 
	//已知左右相机成像坐标求空间坐标
	Point2f l = xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation);
	Point2f r = xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation);
	Point3f worldPoint;
	worldPoint = uv2xyz(l,r);
	cout<<"空间坐标为:"<<endl<<uv2xyz(l,r)<<endl;
 
	system("pause");
 
	return 0;
}
 
 
//************************************
// Description: 根据左右相机中成像坐标求解空间坐标

//************************************
Point3f uv2xyz(Point2f uvLeft,Point2f uvRight)
{
	//  [u1]      |X|					  [u2]      |X|
	//Z*|v1| = Ml*|Y|					Z*|v2| = Mr*|Y|
	//  [ 1]      |Z|					  [ 1]      |Z|
	//			  |1|								|1|
	Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);
	Mat mLeftTranslation = Mat(3,1,CV_32F,leftTranslation);
	Mat mLeftRT = Mat(3,4,CV_32F);//左相机M矩阵
	hconcat(mLeftRotation,mLeftTranslation,mLeftRT);
	Mat mLeftIntrinsic = Mat(3,3,CV_32F,leftIntrinsic);
	Mat mLeftM = mLeftIntrinsic * mLeftRT;
	//cout<<"左相机M矩阵 = "<<endl<<mLeftM<<endl;
 
	Mat mRightRotation = Mat(3,3,CV_32F,rightRotation);
	Mat mRightTranslation = Mat(3,1,CV_32F,rightTranslation);
	Mat mRightRT = Mat(3,4,CV_32F);//右相机M矩阵
	hconcat(mRightRotation,mRightTranslation,mRightRT);
	Mat mRightIntrinsic = Mat(3,3,CV_32F,rightIntrinsic);
	Mat mRightM = mRightIntrinsic * mRightRT;
	//cout<<"右相机M矩阵 = "<<endl<<mRightM<<endl;
 
	//最小二乘法A矩阵
	Mat A = Mat(4,3,CV_32F);
	A.at<float>(0,0) = uvLeft.x * mLeftM.at<float>(2,0) - mLeftM.at<float>(0,0);
	A.at<float>(0,1) = uvLeft.x * mLeftM.at<float>(2,1) - mLeftM.at<float>(0,1);
	A.at<float>(0,2) = uvLeft.x * mLeftM.at<float>(2,2) - mLeftM.at<float>(0,2);
 
	A.at<float>(1,0) = uvLeft.y * mLeftM.at<float>(2,0) - mLeftM.at<float>(1,0);
	A.at<float>(1,1) = uvLeft.y * mLeftM.at<float>(2,1) - mLeftM.at<float>(1,1);
	A.at<float>(1,2) = uvLeft.y * mLeftM.at<float>(2,2) - mLeftM.at<float>(1,2);
 
	A.at<float>(2,0) = uvRight.x * mRightM.at<float>(2,0) - mRightM.at<float>(0,0);
	A.at<float>(2,1) = uvRight.x * mRightM.at<float>(2,1) - mRightM.at<float>(0,1);
	A.at<float>(2,2) = uvRight.x * mRightM.at<float>(2,2) - mRightM.at<float>(0,2);
 
	A.at<float>(3,0) = uvRight.y * mRightM.at<float>(2,0) - mRightM.at<float>(1,0);
	A.at<float>(3,1) = uvRight.y * mRightM.at<float>(2,1) - mRightM.at<float>(1,1);
	A.at<float>(3,2) = uvRight.y * mRightM.at<float>(2,2) - mRightM.at<float>(1,2);
 
	//最小二乘法B矩阵
	Mat B = Mat(4,1,CV_32F);
	B.at<float>(0,0) = mLeftM.at<float>(0,3) - uvLeft.x * mLeftM.at<float>(2,3);
	B.at<float>(1,0) = mLeftM.at<float>(1,3) - uvLeft.y * mLeftM.at<float>(2,3);
	B.at<float>(2,0) = mRightM.at<float>(0,3) - uvRight.x * mRightM.at<float>(2,3);
	B.at<float>(3,0) = mRightM.at<float>(1,3) - uvRight.y * mRightM.at<float>(2,3);
 
	Mat XYZ = Mat(3,1,CV_32F);
	//采用SVD最小二乘法求解XYZ
	solve(A,B,XYZ,DECOMP_SVD);
 
	//cout<<"空间坐标为 = "<<endl<<XYZ<<endl;
 
	//世界坐标系中坐标
	Point3f world;
	world.x = XYZ.at<float>(0,0);
	world.y = XYZ.at<float>(1,0);
	world.z = XYZ.at<float>(2,0);
 
	return world;
}
 
//************************************
// Description: 将世界坐标系中的点投影到左右相机成像坐标系中

//************************************
Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3])
{
	//    [fx s x0]							[Xc]		[Xw]		[u]	  1		[Xc]
	//K = |0 fy y0|       TEMP = [R T]		|Yc| = TEMP*|Yw|		| | = —*K *|Yc|
	//    [ 0 0 1 ]							[Zc]		|Zw|		[v]	  Zc	[Zc]
	//													[1 ]
	Point3f c;
	c.x = rotation[0][0]*worldPoint.x + rotation[0][1]*worldPoint.y + rotation[0][2]*worldPoint.z + translation[0][0]*1;
	c.y = rotation[1][0]*worldPoint.x + rotation[1][1]*worldPoint.y + rotation[1][2]*worldPoint.z + translation[0][1]*1;
	c.z = rotation[2][0]*worldPoint.x + rotation[2][1]*worldPoint.y + rotation[2][2]*worldPoint.z + translation[0][2]*1;
 
	Point2f uv;
	uv.x = (intrinsic[0][0]*c.x + intrinsic[0][1]*c.y + intrinsic[0][2]*c.z)/c.z;
	uv.y = (intrinsic[1][0]*c.x + intrinsic[1][1]*c.y + intrinsic[1][2]*c.z)/c.z;
 
	return uv;
}

致谢

apriltag 标签的使用

参考本人的另外一篇博客

至此可以说，本篇文档已经总结结束了，对于今年的雷达站，我已经做到了实现基本功能，后面就要靠调参以及如何提高准确度了。

心如坚石

关注

1
点赞
踩
11

收藏

觉得还不错? 一键收藏
0
评论
基于RM比赛的飞镖检测总结（三维构建）

目标需求与前期准备比赛的场地是16m*28m,雷达放置在己方基地的高3m处，检测到飞镖，得到他的三维坐标，将三维坐标发给nuc。因为单目相机的测距虽然可以采用solvepnp的方法，但是因为你对运动的飞镖进行二值化形成的区域大小很容易受到外部环境的影响，所以完全无法提供准确的坐标。因此我们必须采用双目相机才能提供准确的坐标，提高哨兵的拦截准确度。而实验室只有6mm、8mm、12mm三个镜头的相机，参考了大疆的各种参数的选型，所以我根据自己实验室的已有相机。几个参数选取相
复制链接

扫一扫

专栏目录