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----一个微型现实增强库的介绍及视觉应用(二)的更多相关文章

  1. ArUco----一个微型现实增强库的介绍及视觉应用(一)

    ArUco----一个微型现实增强库的介绍及视觉应用(一) 一.ArUco简介 ArUco是一个开源的微型的现实增强库,目前好像已经集成在OpenCV3.0以上的版本内了,它除了用于现实增强,还很用于 ...

  2. 用 Lua 实现一个微型虚拟机-基本篇

    用 Lua 实现一个微型虚拟机-基本篇 目录 介绍 机器指令模拟 最终核心代码 虚拟机内部状态可视化 完整项目代码 后续计划 参考 介绍 在网上看到一篇文章 使用 C 语言实现一个虚拟机, 这里是他的 ...

  3. 介绍一个python视频处理库:moviepy

    由于博客园的插件和我自己博客的插件不一致,代码以及视频插入转换很麻烦,所以还是我原来博客的地址查看吧. 介绍一个python视频处理库:moviepy

  4. dotweb——go语言的一个微型web框架(一)

    dotweb是16年正式托管到github的一个开源项目,go语言的web框架目前也有很多,出名的有bee和echo.它们都是很优秀的框架,但是我们喜欢更轻.更小的东西,经历一些之后我们更青睐微服务这 ...

  5. 【Tool】Augmentor和imgaug——python图像数据增强库

    Augmentor和imgaug--python图像数据增强库 Tags: ComputerVision Python 介绍两个图像增强库:Augmentor和imgaug,Augmentor使用比较 ...

  6. Python常用的库简单介绍一下

    Python常用的库简单介绍一下fuzzywuzzy ,字符串模糊匹配. esmre ,正则表达式的加速器. colorama 主要用来给文本添加各种颜色,并且非常简单易用. Prettytable ...

  7. Go语言标准库log介绍

    Go语言标准库log介绍 无论是软件开发的调试阶段还是软件上线之后的运行阶段,日志一直都是非常重要的一个环节,我们也应该养成在程序中记录日志的好习惯. log Go语言内置的log包实现了简单的日志服 ...

  8. C 封装一个简单二叉树基库

    引文 今天分享一个喜欢佩服的伟人,应该算人类文明极大突破者.收藏过一张纸币类型如下 那我们继续科普一段关于他的简介 '高斯有些孤傲,但令人惊奇的是,他春风得意地度过了中产阶级的一生,而  没有遭受到冷 ...

  9. JavaFX的扩展控件库ControlsFX介绍

    声明:   本博客文章原创类别的均为个人原创,版权所有.转载请注明出处: http://blog.csdn.net/ml3947,另外本人的个人博客:http://www.wjfxgame.com. ...

随机推荐

  1. Spark核心技术原理透视一(Spark运行原理)

    在大数据领域,只有深挖数据科学领域,走在学术前沿,才能在底层算法和模型方面走在前面,从而占据领先地位. Spark的这种学术基因,使得它从一开始就在大数据领域建立了一定优势.无论是性能,还是方案的统一 ...

  2. 【Python】 配置文件相对路径&软件自动执行的工作目录

    今天对监控脚本做了一些变更,然后突然发现监控全部都失效了..排查了半天问题仍然不知所踪.最终发现居然是一个踩过好几次的老坑.. 就是脚本内写的配置文件为了调试方便写成了相对路径,但是在上线时没有意识到 ...

  3. Algorithm --> 并查集

    并查集 主要解决图的连通性问题,比如: 1.随意给你两个点,让你判断它们是否连通: 2.问你整幅图一共有几个连通分支: 初始化: void init(int size) { ; i < size ...

  4. java 定义泛型方法

    1 class Demo{ 2 public <T> T fun(T t){ 3 return t; 4 } 5 } 6 public class GenericsDemo { 7 pub ...

  5. sqlite语句主页

    因为现在android手机用sqlite数据,但是sql语句很多和sqlserver不同..所以还是把官网记下以便开发:http://www.sqlite.org/lang.html

  6. IDEA 中配置JDK

    提前安装jdk,配置环境变量 一.配置jdk 1.依次点开File -->Project Structure,点击左侧标签页,点击SDKs 2.点击+号,选SDK 3.在弹出框选择jdk路径(我 ...

  7. 从零部署Spring boot项目到云服务器(准备工作)

    自己的博客终于成功部署上线了,回过头来总结记录一下整个项目的部署过程! 测试地址:47.94.154.205:8084 注:文末有福利! 一.Linux下应用Shell通过SSH连接云服务器 //ss ...

  8. python内置函数 divmod()

    先来看一下builtins.py中的代码: def divmod(x, y): # known case of builtins.divmod """ Return th ...

  9. java1.8版本的HashMap源码剖析

    一.摘要 以下分析内容均是基于JDK1.8产生的,同时也和JDK1.7版本的hashmap做了一些比较.在1.7版本中,HashMap的实现是基于数组+链表的形式,而在1.8版本中则引入了红黑树,但其 ...

  10. Docker深入浅出系列教程——Docker简介

    我是架构师张飞洪,钻进浩瀚代码,十年有余,人不堪其累,吾不改其乐.如果你和我的看法不一样,请关注我的头条号,我们一起奇闻共赏,疑义相析. 本节属于入门简介,从三个小方面进行简单介绍Docker. Do ...