ArUco----一个微型现实增强库的介绍及视觉应用（二）

ArUco----一个微型现实增强库的介绍及视觉应用（二）

一、第一个ArUco的视觉应用

　　首先介绍第一个视觉应用的Demo，这个应用场景比较简单，下面具体介绍：

1. 应用场景

　　主线程：通过摄像头检测环境中的视觉标志，看到ID为100的标志后在图像中圈出标志，在标志上绘制坐标系，得到视觉标志相对于相机坐标系的位置和姿态参数；

　　子线程：将得到的视觉标志进一步转换成需要的数据类型并发送给机器人。

2. 编程环境

　　Ubuntu14.04（安装有OpenCV以及ArUco）

3. 编译工具

　　Cmake

4. 源码下载地址

　　https://github.com/Zhanggx0102/Aruco_Blog_Demo.git

5. 源码处理

　　下载完成后重新编译即可。

　　cd Aruco_Blog_Demo-master

　　rm -r build/

　　mkdir build

　　cd build

　　cmake ..

　　make

二、源码解读

源码中已经做了比较详细的注释，这里主要讲解程序框架。

程序流程图如下所示：

程序流程图

执行后的效果如下图所示：

下面是源码截取的两个主要的函数。

/*******************************************************************************************************************

main function

********************************************************************************************************************/

int main(int argc,char **argv)

{

    int thread_return;

    pthread_t Message_Send_Thread_ID;

    //init thread lock

    pthread_mutex_init(&IK_Solver_Lock, NULL);

    //creat new thread

    thread_return = pthread_create(&Message_Send_Thread_ID,NULL,Thread_Func_Message_Send,NULL);

    //import the camera param (CameraMatrix)

    float camera_matrix_array[9] = { 1.0078520005023535e+003, 0., 6.3950000000000000e+002,

                                  0.0, 1.0078520005023535e+003, 3.5950000000000000e+002,

                                  0.0, 0.0, 1.0 };

    cv::Mat Camera_Matrix(3,3,CV_32FC1);

    InitMat(Camera_Matrix,camera_matrix_array);

    cout << "Camera_Matrix = " << endl << "" << Camera_Matrix << endl ;

    //import the camera param (Distorsion)

    float Distorsion_array[5] = {-4.9694653328469340e-002, 2.3886698343464000e-001, 0., 0.,-2.1783942538569392e-001};

    cv::Mat Distorsion_M(1,5,CV_32FC1);

    InitMat(Distorsion_M,Distorsion_array);

    cout << "Distorsion_M = " << endl << "" << Distorsion_M << endl ;

    CameraParameters LogiC170Param;

    //LogiC170Param.readFromXMLFile("LogitchC170_Param.yml");

    LogiC170Param.CameraMatrix = Camera_Matrix.clone();

    LogiC170Param.Distorsion = Distorsion_M.clone();

    LogiC170Param.CamSize.width = 1280;

    LogiC170Param.CamSize.height = 720;

    float MarkerSize = 0.04;

    int Marker_ID;

    MarkerDetector MDetector;

    MDetector.setThresholdParams(7, 7);

    MDetector.setThresholdParamRange(2, 0);

    CvDrawingUtils MDraw;

    //read the input image

    VideoCapture cap(0); // open the default camera

     if(!cap.isOpened())  // check if we succeeded

        return -1;

    cv::Mat frame;

    cv::Mat Rvec;//rotational vector

    CvMat Rvec_Matrix;//temp matrix

    CvMat R_Matrix;//rotational matrixs

    cv::Mat Tvec;//translation vector

    cap>>frame;//get first frame

    //LogiC170Param.resize(frame.size());

    printf("%f, %f\n",cap.get(CV_CAP_PROP_FRAME_WIDTH),cap.get(CV_CAP_PROP_FRAME_HEIGHT));

    cap.set(CV_CAP_PROP_FRAME_WIDTH, 1280);

    cap.set(CV_CAP_PROP_FRAME_HEIGHT, 720);

    //cap.set(CV_CAP_PROP_FPS, 10);

    printf("%f, %f\n",cap.get(CV_CAP_PROP_FRAME_WIDTH),cap.get(CV_CAP_PROP_FRAME_HEIGHT));   

    while(1)

    {

        //get current frame

        cap>>frame;

        //Ok, let's detect

        vector< Marker >  Markers=MDetector.detect(frame, LogiC170Param, MarkerSize);

        //printf("marker count:%d \n",(int)(Markers.size()));

        //for each marker, estimate its ID and if it is  100 draw info and its boundaries in the image

        for (unsigned int j=0;j<Markers.size();j++)

        {

            //marker ID test

            Marker_ID = Markers[j].id;

            printf("Marker ID = %d \n",Marker_ID);

            if(Marker_ID == 100)

            {

                //cout<<Markers[j]<<endl;

                Markers[j].draw(frame,Scalar(0,0,255),2);

                Markers[j].calculateExtrinsics(MarkerSize, LogiC170Param, false);

                //calculate rotational vector

                Rvec = Markers[j].Rvec;

                cout << "Rvec = " << endl << "" << Rvec << endl ;

                //calculate transformation vector

                Tvec = Markers[j].Tvec;

                cout << "Tvec = " << endl << "" << Tvec << endl ;

                //lock to update global variables: Rotat_Vec_Arr[3]  Rotat_M[9]  Trans_M[3]

                pthread_mutex_lock(&IK_Solver_Lock);

                //save rotational vector to float array

                for (int r = 0; r < Rvec.rows; r++)

                {

                    for (int c = 0; c < Rvec.cols; c++)

                    {

                        //cout<< Rvec.at<float>(r,c)<<endl;

                        Rotat_Vec_Arr[r] = Rvec.at<float>(r,c);

                    }

                }

                printf("Rotat_Vec_Arr[3] = [%f, %f, %f] \n",Rotat_Vec_Arr[0],Rotat_Vec_Arr[1],Rotat_Vec_Arr[2]);

                //save array data to CvMat and convert rotational vector to rotational matrix

                cvInitMatHeader(&Rvec_Matrix,1,3,CV_32FC1,Rotat_Vec_Arr,CV_AUTOSTEP);//init Rvec_Matrix

                cvInitMatHeader(&R_Matrix,3,3,CV_32FC1,Rotat_M,CV_AUTOSTEP);//init R_Matrix and Rotat_M

                cvRodrigues2(&Rvec_Matrix, &R_Matrix,0);

                printf("Rotat_M = \n[%f, %f, %f, \n  %f, %f, %f, \n  %f, %f, %f] \n",Rotat_M[0],Rotat_M[1],Rotat_M[2],Rotat_M[3],Rotat_M[4],Rotat_M[5],Rotat_M[6],Rotat_M[7],Rotat_M[8]);

                //save transformation vector to float array

                for (int r = 0; r < Tvec.rows; r++)

                {

                    for (int c = 0; c < Tvec.cols; c++)

                    {

                        Trans_M[r] = Tvec.at<float>(r,c);

                    }

                }

                printf("Trans_M[3] = [%f, %f, %f] \n",Trans_M[0],Trans_M[1],Trans_M[2]);

                //unlock

                pthread_mutex_unlock(&IK_Solver_Lock);

                // draw a 3d cube in each marker if there is 3d info

                if (LogiC170Param.isValid() && MarkerSize != -1)

                {

                    MDraw.draw3dAxis(frame,LogiC170Param,Rvec,Tvec,0.04);

                }

            }

        }

        //*/

        cv::waitKey(150);//wait for key to be pressed

        cv::imshow("Frame",frame);

    }

    //wait for the IK solver thread close and recover resources

    pthread_join(Message_Send_Thread_ID,NULL);

    pthread_mutex_destroy(&IK_Solver_Lock); //destroy the thread lock

    return 0

}

/**********************************************************

function: new thread to send messages

input: void

return :null

***********************************************************/

void * Thread_Func_Message_Send(void *arg)

{

    printf("IK solver thread is running!\n");

    //original pose and position

    float P_original[4];

    float N_original[4];

    float O_original[4];

    float A_original[4];

    //final pose and position

    float P[3];

    float N[3];

    float O[3];

    float A[3];

    P_original[3] = 1;

    N_original[3] = 0;

    O_original[3] = 0;

    A_original[3] = 0;

    while (1)

    {

        //get the spacial pose

        pthread_mutex_lock(&IK_Solver_Lock);

        //memcpy(P_original, Trans_M, sizeof(Trans_M));

        for(int i=0;i<3;i++)

        {

            P_original[i] = Trans_M[i];

            N_original[i] = Rotat_M[3*i];

            O_original[i] = Rotat_M[3*i+1];

            A_original[i] = Rotat_M[3*i+2];

        }

        pthread_mutex_unlock(&IK_Solver_Lock);

        //debug printf

        ///*

        printf("N_original[4] = [%f, %f, %f, %f]  \n",N_original[0],N_original[1],N_original[2],N_original[3]);

        printf("O_original[4] = [%f, %f, %f, %f]  \n",O_original[0],O_original[1],O_original[2],O_original[3]);

        printf("A_original[4] = [%f, %f, %f, %f]  \n",A_original[0],A_original[1],A_original[2],A_original[3]);

        printf("P_original[4] = [%f, %f, %f, %f]  \n",P_original[0],P_original[1],P_original[2],P_original[3]);

        //*/

        printf("I send the message to robot here! \n");

        /*

        add message send function here!

        */

        //uodate every 5 s

        sleep(5);

    }

    //kill the message send thread

    pthread_exit(0);

}

　

<-- 本篇完-->

欢迎留言、私信、邮箱、微信等任何形式的技术交流。

作者信息：

名称：Shawn

邮箱：zhanggx0102@163.com

微信二维码：↓

ArUco----一个微型现实增强库的介绍及视觉应用（二）的相关教程结束。