基于QT&C++的线激光标定+测距模型

基于QT&C++，参考CSDN大神发帖，做了一套线激光标定+测距模型。直接上代码，可参考注释自由复制粘贴。

原理方面，主要应用线激光刀面的几何约束，并应用ransac方法优化标定线激光点的拟合平面。

硬件方面：图便宜采用了可见650nm的红色线激光和USB免驱摄像头，具体可根据自己需求自行选择。标定纸可自行打印。

特别感谢这两篇帖子给我的启发：

 一文了解单线激光扫描系统的标定与成像原理_天琴lyra的博客-CSDN博客_线激光标定
张正友相机标定法原理与实现_Eating Lee的博客-CSDN博客_张正友标定法原理
 

界面如下。

widget.h

#ifndef WIDGET_H
#define WIDGET_H

#include 
#include "opencv2/opencv.hpp"
#include "opencv2/highgui.hpp"
#include 
#include 
#include "opencv2/imgproc/types_c.h"
#include "qt_windows.h"
using namespace std;
using namespace cv;

namespace Ui {
class Widget;
}

class Widget : public QWidget
{
    Q_OBJECT

public:
    explicit Widget(QWidget *parent = nullptr);
    ~Widget();

public slots:
    void readFarme();

private slots:
    void on_pushButton_clicked();
    void on_pushButton_2_clicked();
    void on_pushButton_4_clicked();
    void on_pushButton_3_clicked();
    void on_pushButton_5_clicked();
    void on_pushButton_6_clicked();
    void on_spinBox_valueChanged(int arg1);
    void on_spinBox_2_valueChanged(int arg1);
    void on_doubleSpinBox_2_valueChanged(double arg1);
    void on_spinBox_3_valueChanged(int arg1);
    void on_spinBox_4_valueChanged(int arg1);
    void on_spinBox_5_valueChanged(int arg1);

private:
    Ui::Widget *ui;
    QTimer    *timer;
    Mat srcImge,OperImage,RGBIntrMat, RGBDistCoeff;
    VideoCapture *videocap;
    vector channels,RgbMatVec,GrayMatVec,GrayMatVecUndis,RGBRotVec, RGBTranVec,realworld;
    vector count;
    vector> RGBcornerV;
    vector> realCornerV;
    Size chessSize = Size(9, 13);
    float gridLength = 18; //unit mm
    Size RGBcamResolution = Size(640, 480);    //resolution of color RGB camera
    int imgAmount=0,pn=0;   //the images used to calibrate
    int expo=-6,pointthre=170,camerano=1;
    double PA=0.0, PB, PC, PD;
    bool Caldoneflag=false;
};

#endif // WIDGET_H

main.cpp

#include "widget.h"

#include 

int main(int argc, char *argv[])
{
    QApplication a(argc, argv);
    Widget w;
    w.show();
    return a.exec();
}

widget.cpp

#include "widget.h"
#include "ui_widget.h"
#include 
#define _USE_MATH_DEFINES
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

Widget::Widget(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::Widget)
{
    ui->setupUi(this);
    timer = new QTimer(this);
    connect(timer, SIGNAL(timeout()), this, SLOT(readFarme()));
    ui->pushButton_2->setEnabled(false);
    ui->pushButton_5->setEnabled(false);
    ui->pushButton_6->setEnabled(false);
}

void Widget::readFarme()
{
    videocap = new cv::VideoCapture(camerano);// Get the camera.
    videocap->set(cv::CAP_PROP_EXPOSURE,expo);
    videocap->read(srcImge);//Get one frame.
    cvtColor(srcImge,srcImge,COLOR_BGR2RGB);
    cv::GaussianBlur(srcImge, OperImage,cvSize(5, 5),0, 0 );//GaussianBlur.
    split(OperImage,channels);//Separate the channels，and value the variety.
    OperImage=channels.at(0);//0 for red(RGB).
    cv::threshold(OperImage,OperImage, pointthre, 255, 0);//The third number is the threthod, value smaller than the threthod shall be 0.
    //Canny(OperImage,OperImage,50,200,3);//Get the edge,restored.
    if(!Caldoneflag)//Show the original images.
    {
        QImage image1=QImage((const unsigned char*)srcImge.data,srcImge.cols,srcImge.rows,QImage::Format_RGB888);
        ui->label_2->setPixmap(QPixmap::fromImage(image1));
        for(int j=0;jlabel->setPixmap(QPixmap::fromImage(image2));
    }
    else //Show the undistorted images.
    {
        Mat rr,gg;
        cv::undistort(srcImge, rr, RGBIntrMat, RGBDistCoeff);
        cv::undistort(OperImage, gg, RGBIntrMat, RGBDistCoeff);
        for(int j=0;jlabel_2->setPixmap(QPixmap::fromImage(image1));
        QImage image2=QImage((const unsigned char*)gg.data,gg.cols,gg.rows,QImage::Format_Grayscale8);
        ui->label->setPixmap(QPixmap::fromImage(image2));
        if(PA!=0) //Calculate and show the distance if the extrinsticts are get.
        {
            double compare=10000;
            for(int j=0;j(3, 1) << k, j, 1);
                        Mat cccc=RGBIntrMat.inv()*ii;
                        double cx=cccc.at(0,0);
                        double cy=cccc.at(1,0);
                        double curpointdis=-PD/(PA*cx+PB*cy+PC);
                        if (curpointdistextBrowser_2->append("Out of range!Adjust the device!");
                ui->textBrowser_2->moveCursor(QTextCursor::End);
            }
            else
            {
                ui->textBrowser_2->append("Current distance is : "+QString::number(compare));
                ui->textBrowser_2->moveCursor(QTextCursor::End);
            }
        }
    }
}

Widget::~Widget()
{
    videocap->release();
    delete ui;
}

void Widget::on_pushButton_clicked()//Start the timer.
{
    QMessageBox msg(this);
    msg.setWindowTitle("Window Title");
    msg.setText("Chessboard type is "+QString::number(chessSize.width)+" * "+QString::number(chessSize.height)+"\n"+
                "Chessboard type is "+QString::number(gridLength)+" mm"+"\n"+
                "Camera opened is No:"+QString::number(camerano)+"\n");
    msg.setIcon(QMessageBox::Information);//Set the icon type.
    msg.setStandardButtons(QMessageBox::Ok | QMessageBox:: Cancel);//Button settings.
    if((msg.exec() == QMessageBox::Ok))//Module callback.
    {
       ui->textBrowser->append("Show images!");
       ui->textBrowser->moveCursor(QTextCursor::End);
       timer->start(100);
       ui->spinBox->setEnabled(false);
       ui->spinBox_2->setEnabled(false);
       ui->spinBox_5->setEnabled(false);
       ui->doubleSpinBox_2->setEnabled(false);
       ui->pushButton->setEnabled(false);
       ui->pushButton_2->setEnabled(true);
    }
}

void Widget::on_pushButton_2_clicked()//Stop the timer.
{
    timer->stop();
    ui->textBrowser->append("Paused!");
    ui->textBrowser->moveCursor(QTextCursor::End);
    ui->pushButton_2->setEnabled(false);
    ui->pushButton->setEnabled(true);
}

void Widget::on_pushButton_3_clicked()//Shut the widget.
{
    Widget::~Widget();
}

void Widget::on_pushButton_4_clicked()//Capture the images if the corners can be found.
{
    mkdir("saveData");
    vector RGBcorner;
    vector realCorner;
    for (int i = 0; i < chessSize.height; i++)
    {
        for (int k = 0; k < chessSize.width; k++)
        {
            realCorner.push_back(Point3f(k*gridLength, i*gridLength, 0));
        }
    }
    Mat RGBgrayImg=srcImge;
    cvtColor(RGBgrayImg, RGBgrayImg, CV_BGR2GRAY);
    bool findRGBCorner = cv::findChessboardCorners(RGBgrayImg, chessSize, RGBcorner,
        cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);
    if(findRGBCorner)
    {
        RgbMatVec.push_back(srcImge.clone());//Save RGB&GRAY images and show.
        string rgbFileName="./saveData/rgb("+std::to_string(imgAmount)+").jpg";
        cvtColor(srcImge,srcImge,COLOR_BGR2RGB);
        imwrite(rgbFileName,srcImge);
        ui->textBrowser->moveCursor(QTextCursor::End);
        GrayMatVec.push_back(OperImage.clone());
        string grayFileName="./saveData/gray("+std::to_string(imgAmount)+").jpg";
        imwrite(grayFileName,OperImage);
        ui->textBrowser->append("NO "+QString::number(imgAmount+1)+" image's corners are found and captured!");
        ui->textBrowser->moveCursor(QTextCursor::End);
        cornerSubPix(RGBgrayImg, RGBcorner, cv::Size(5, 5), cv::Size(-1, -1), //Optimize the corners.
            cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 1000, 0.0001));
        RGBcornerV.push_back(RGBcorner);
        realCornerV.push_back(realCorner);
        imgAmount++;//Show the right if images > 6.
        if(imgAmount>6)
        {
            ui->pushButton_5->setEnabled(true);
            ui->textBrowser->append("You can calibrate now!");
            ui->textBrowser->moveCursor(QTextCursor::End);
        }
        waitKey(500);
    }
    else if(!findRGBCorner)//Warn if cannot find the images' corners.
    {
        ui->textBrowser->append("No corners are found, images captured failed!");
        ui->textBrowser->moveCursor(QTextCursor::End);
    }
}

void Widget::on_pushButton_5_clicked() //Start to Calibrate and get the intrinsics.
{
    ui->pushButton_4->setEnabled(false);
    ui->pushButton_5->setEnabled(false);
    calibrateCamera(realCornerV, RGBcornerV, RGBcamResolution, RGBIntrMat, RGBDistCoeff, RGBRotVec, RGBTranVec, 0);
    stringstream ri,disco;
    ri<textBrowser->append("RGB camera Intrinsics are:"+QString::fromStdString(ri.str()));
    ui->textBrowser->append("RGB camera distCoeffs are:"+QString::fromStdString(disco.str()));
    ui->textBrowser->append("You can calculate the Extrinsics!");
    ui->textBrowser->moveCursor(QTextCursor::End);
    ui->pushButton_6->setEnabled(true);
}

void Widget::on_pushButton_6_clicked() //Calculate the Extrinsics.
{
    ui->textBrowser->append("Staring to extract 3D point!");
    ui->textBrowser->moveCursor(QTextCursor::End);

    for(int i=0;i(0,2);
        b=temp.at(1,2);
        c=temp.at(2,2);
        Mat n=(cv::Mat_(1, 3) << a, b, c);
        Mat D=n*RGBTranVec[i];
        double d=-1*D.at(0,0);
        vector realv;
        for(int j=0;j(3, 1) << k, j, 1);//Core scentence.
                    Mat cccc=RGBIntrMat.inv()*ii;
                    double cx=cccc.at(0,0);
                    double cy=cccc.at(1,0);
                    Point3f real;
                    real.x = -d/(a*cx+b*cy+c)*cx,
                    real.y = -d/(a*cx+b*cy+c)*cy,
                    real.z = -d/(a*cx+b*cy+c);
                    realv.push_back(real);
                    pn++;
                }
            }
        realworld.push_back(realv);
        ui->textBrowser->append("Finish No "+QString::number(i+1)+" image's checking!");
        ui->textBrowser->moveCursor(QTextCursor::End);
    }
    ui->textBrowser->append("Points amount is "+QString::number(pn));

    //Ransac method for fitting the plane.    
    int size_old = 3;
    double para = size_old/pn;
    vector pts_3d;
    for(int i=0;i index;
            for (int k=0; k<3;k++)
            {
                index.push_back(rand()%imgAmount);
            }
            auto idx = index.begin();
            int a=rand()%realworld[*idx].size();
            double x1 = realworld[*idx].at(a).x, y1 = realworld[*idx].at(a).y, z1 = realworld[*idx].at(a).z;
            ++idx;

            int b=rand()%realworld[*idx].size();
            double x2 = realworld[*idx].at(b).x, y2 = realworld[*idx].at(b).y, z2 = realworld[*idx].at(b).z;
            ++idx;

            int c=rand()%realworld[*idx].size();
            double x3 = realworld[*idx].at(c).x, y3 = realworld[*idx].at(c).y, z3 = realworld[*idx].at(c).z;
            PA = (y2 - y1)*(z3 - z1) - (z2 - z1)*(y3 - y1);
            PB = (z2 - z1)*(x3 - x1) - (x2 - x1)*(z3 - z1);
            PC = (x2 - x1)*(y3 - y1) - (y2 - y1)*(x3 - x1);
            PD = -(PA*x1 + PB*y1 + PC*z1);
            for (auto iter = pts_3d.begin(); iter != pts_3d.end(); ++iter)
            {
                double dis = fabs(PA*iter->x + PB*iter->y + PC*iter->z + PD) / sqrt(PA*PA + PB*PB + PC*PC);//点到平面的距离公式
                if (dis < limit)
                    index.push_back(iter - pts_3d.begin());
            }
            //Update the group.
            if (index.size() > size_old)
            {
                size_old = index.size();
            }
            index.clear();
        }
    cout << PA << " " << PB << " " << PC << " " << PD << endl;
    cout << "Points Total account is: "<< pn <textBrowser->append("Parameter A is "+QString::number(PA));
    ui->textBrowser->append("Parameter B is "+QString::number(PB));
    ui->textBrowser->append("Parameter C is "+QString::number(PC));
    ui->textBrowser->append("Parameter D is "+QString::number(PD));
    ui->textBrowser->moveCursor(QTextCursor::End);
    realworld.clear();
    pn=0;
    Caldoneflag=true;
}

void Widget::on_spinBox_valueChanged(int arg1)
{
    ui->textBrowser->append("ChessBoard Row account is "+QString::number(arg1));
    ui->textBrowser->moveCursor(QTextCursor::End);
    chessSize.width=arg1;
}

void Widget::on_spinBox_2_valueChanged(int arg1)
{
    ui->textBrowser->append("ChessBoard Column account is "+QString::number(arg1));
    ui->textBrowser->moveCursor(QTextCursor::End);
    chessSize.height=arg1;
}

void Widget::on_doubleSpinBox_2_valueChanged(double arg1)
{
    ui->textBrowser->append("ChessBoard Column size is "+QString::number(arg1));
    ui->textBrowser->moveCursor(QTextCursor::End);
    gridLength=arg1;
}

void Widget::on_spinBox_3_valueChanged(int arg1)
{
    ui->textBrowser->append("Exposure time value is "+QString::number(arg1));
    ui->textBrowser->moveCursor(QTextCursor::End);
    expo=arg1;
}

void Widget::on_spinBox_4_valueChanged(int arg1)
{
    ui->textBrowser->append("Point lower threthod is "+QString::number(arg1));
    ui->textBrowser->moveCursor(QTextCursor::End);
    pointthre=arg1;
}

void Widget::on_spinBox_5_valueChanged(int arg1)
{
    ui->textBrowser->append("Camera opened is "+QString::number(arg1));
    ui->textBrowser->moveCursor(QTextCursor::End);
    camerano=arg1;
}