一、第一个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!
        */<br>
        //uodate every 5 s
        sleep(5);
    }
    //kill the message send thread
    pthread_exit(0); 
}

  <– 本篇完–>