六自由度JAKA机器人基于视觉的多颜色方块抓取（色块颜色识别、坐标提取、旋转角度提取）_jaka机器人编程

目录

一、任务描述

二、视觉处理方案（重点）

2.1 图像标定与单应性矩阵计算

2.2 目标坐标与旋转角度提取

三、运动控制方案

四、结果讨论

五、完整代码（无控制底层）

---

一、任务描述

        基于JAKA ZU3机器人平台与固定位置的彩色相机（非垂直），完成不同颜色木块的识别与抓取。编写图像处理算法获取图像中方块的位姿和颜色，并用机器人抓取。

图1：实验主要对象

二、视觉处理方案（重点）

        本次实验中，视觉处理主要分为两部分，即图像坐标系同机器人 XY 平面坐标系之间的单应矩阵计算及坐标转换；方块图像采集、处理，最终提取图像中所有方块的属性，包括位置、角度和颜色。

2.1 图像标定与单应性矩阵计算

        单应性矩阵的计算原理同“文档矫正”类似，但本实验中要注意三维平面的映射关系。如下图2，定义世界坐标系的原点在Jaka机器人底座，经验证，Jaka坐标系与世界坐标系之间无旋转映射，仅有Z坐标中的差距，两坐标系之间的刚体变换关系如下：

图2：平面机器人视觉系统坐标关系

        因此，仅需要求出图像像素坐标系与世界坐标系之间的单应性矩阵，就能得出识别到的物块点在Jaka坐标系下的值。如下图2所示，使用目标相机得到照片，分别取标定点1-4的坐标值作为图像中已知点，而后使用平板采集四个点对应的孔的X坐标与Y坐标，作为世界坐标系已知点的X坐标与Y坐标。注意，​​​​​​本篇中容易忽视高度带来的单应性矩阵误差，误差造成原因如下图3所示，解决方法为采集图像点与标定时均选择物块上表面的点。

图3：高度带来的单应性矩阵误差图

        将两个点组对应的代入进去，利用OpenCV库中findHomography()函数即可求得单应性矩阵，代码如下图4所示。

图4：标定点图

求解单应性矩阵代码：

/*
*函数名称：calculate_Homo
*功能：计算单应性矩阵
*输入参数：要得到的单应性矩阵
*/
void calculate_Homo(Mat* Homo) {
	vector<Point2f> ori;//三个目标点世界坐标,z:100,单位mm，rx=180,ry=0,rz=0
	ori.push_back(Point(294.923, 48.434));
	ori.push_back(Point(418.584, 32.481));
	ori.push_back(Point(435.834, 155.045));
	ori.push_back(Point(310.961, 172.286));
 
	vector<Point2f> dst;//三个目标点图像坐标
	dst.push_back(Point(708, 22));
	dst.push_back(Point(446, 104));
	dst.push_back(Point(595, 377));
	dst.push_back(Point(908, 246));
	*Homo = findHomography(dst, ori);//求得单应性矩阵
}

        单应性矩阵求得后，尝试给定一点进行验证。已知靶标图像中所有特征点的坐标 ；相应的特征点在机器人坐标系中的点，上述算法得到的单应性矩阵H，其三者满足。因此，撰写代码对下面要求得的坐标进行求解，代码如下所示。

坐标变换函数

/*
*函数名称：turn_coordinate
*功能：将识别得到的图像坐标转换为机械臂的x，y，z世界坐标
*输入参数1：识别到的点在图像坐标系下的坐标点集（vector）<Point2f>*
*输入参数2：识别到的点在世界坐标系下的坐标点集（vector）<Point2f>*
*输入参数3：单应性矩阵（Mat）
*/
void turn_coordinate(vector<Point2f>* obj_image, Mat* Homo, vector<Point2f> &world_image) {
	short number = 5;
	Mat temp;
	for (int i = 0; i < number; i++) {
		temp = (Mat_<double>(3, 1) << (*obj_image)[i].x, (*obj_image)[i].y, 1);//图像坐标点坐标强行转换成1*3
		temp = *Homo * temp;
		temp = temp / temp.at<double>(2, 0);//得到世界坐标系值
		cout << "obj_world[" << i  << "]=" << temp<< endl;//输出监控
		world_image.push_back(Point(temp.at<double>(0, 0), temp.at<double>(1, 0)));//推入world坐标系，供后续使用
	}
	cout << "*********************************************" << endl;
}

2.2 目标坐标与旋转角度提取

        要得到目标坐标值与旋转角度，最简单的方法就是利用阈值分割。坐标点与旋转角度的提取思路如下图5所示：

图5：图像识别思路

1. 二值化。简单粗暴的对每个颜色的HSV阈值进行列出（取阈值的方法使用可软件得到，或使用滑动条脚本），限制只有五个物块，且五个物块目标颜色不出现重复，取物块上表面的阈值，使用循环依次对五个色块进行遍历，使用switch对阈值范围进行判定，并分别进行二值化。
2. 找最大外围轮廓。得到二值化后的图像后可简单进行形态学操作以确保后续操作的准确性，但本实验中没有采用，目的是为了减少腐蚀带来的目标形态学变化而导致的坐标点偏移，但这对阈值的获取有着更高的要求。同时，为了防止杂点的干扰，使用OpenCV中contourArea()函数对轮廓面积进行判定，只取最大的外围轮廓。
3. 利用轮廓找到中心点的位置。得到轮廓后，可以对轮廓求一阶矩，得到其质心，质心点经过单应性变换后得到的点集即目标物块的坐标值，至此，求得目标物块的X坐标与Y坐标。
4. 利用轮廓矩求取矩形角点。对得到的轮廓进行多边形拟合或最小外接四边形近似，得到每个轮廓的最小外接矩形，经过多次实验，可以确定外接矩形的第一个和第三个顶点能够较为准确的落在物块的对角顶点上。
5. 利用求得的角点求得旋转角度。将上述求得的对角点利用单应性矩阵映射到世界坐标系中，求得世界坐标系下的对应角点，连接两角点求得斜率，进而解出直线倾斜角，使用直线倾斜角相加45°得到物块上表面边的角度，求绝对值后减去90°得到角度差值，而后定义机器人初始角度为90°，每次的旋转角度就是初始角度减去差值。至此，旋转角度已确定。

        最终识别效果如下图6所示。

图6：最终识别效果

三、运动控制方案

        上述识别过程中，根据得到的坐标点与旋转角度，完成机器人运动控制，如图7所示，为最终识别到的物块坐标与旋转角度。

图7：物块坐标与旋转角度

        运动控制选用封装好的moveP()函数方式运动，即输入目标位姿自动求逆解而后运动。运动之前，先执行一遍气爪松开的函数以防气爪异常，根据识别到的世界坐标系坐标组（vector容器）obj_world与旋转角度差值angle_obj来对位姿数组中每个物块的信息进行赋值，采用“门”运动的方式，并定义抓取物块的Z坐标值为105，安全高度为200。定义要搬运的位置坐标组obj_jaka，逐步完成物块搬运，并设置相应断点供操控。

四、结果讨论

        图像识别中，最终识别到的效果如上图6所示，绿色边框为轮廓绘制，蓝色边框为轮廓外接矩形。蓝色数字为识别到的角点序号，黑色字体表示识别到的颜色。

        输出监控中得到的中心坐标点在世界坐标系下如下图8所示。

图 8 输出监控：中心坐标

        输出监控中得到的对角点组的坐标值在世界坐标系下如下图9所示。

图 9 输出监控：对角点组

        输出监控中得到在世界坐标系下角度差值如下图10所示。

图 10 输出监控：角度差值

        如上图所示，基本能够完成识别。但使用最小外接矩形来计算对角点时，橙色的识别没有特别准确。而此时它的二值化图像如下图11所示，经过分析可推测其最小外接矩形在拟合时并没有将点1和点3拟合正确，反而拟合到了点2和点4为准确的角点。

图 11 橙色二值化图像

对此问题，提出两种解决方法：

1. 更改算法。这是最有效的方法，可以使用多边形近似来完成，如果想要更准确，可以使用角点检测，但是角点检测要先设法消除操作平台中孔洞的影响。
2. 改进算法。目前已知在阈值合理的前提下，多边形拟合能够得到两个较为正确的点，但正确的点出现次序是随机的。可对该点的位置进行判定，代入到二值化后的图像中判断是否在图像“白点”内或是在边缘上，如果是那该点必为正确的角点。相比方法1，该方法显然更简便

五、完整代码（无控制底层）

1.source.cpp  存放编写的各个函数的实现

#include "stdafx.h"
#include "source.h"
#include <iostream>
#include <opencv2/opencv.hpp>
#include<vector>
 
using namespace std;
using namespace cv;
#define PI 3.1415926
 
/*
*函数名称：calculate_Homo
*功能：计算单应性矩阵
*输入参数：要得到的单应性矩阵
*/
void calculate_Homo(Mat* Homo) {
	vector<Point2f> ori;//三个目标点世界坐标,z:100,单位mm，rx=180,ry=0,rz=0
	ori.push_back(Point(294.923, 48.434));
	ori.push_back(Point(418.584, 32.481));
	ori.push_back(Point(435.834, 155.045));
	ori.push_back(Point(310.961, 172.286));
 
	vector<Point2f> dst;//三个目标点图像坐标
	dst.push_back(Point(708, 22));
	dst.push_back(Point(446, 104));
	dst.push_back(Point(595, 377));
	dst.push_back(Point(908, 246));
	*Homo = findHomography(dst, ori);//求得单应性矩阵
}
 
/*
*函数名称：turn_coordinate
*功能：将识别得到的图像坐标转换为机械臂的x，y，z世界坐标
*输入参数1：识别到的点在图像坐标系下的坐标点集（vector）<Point2f>*
*输入参数2：识别到的点在世界坐标系下的坐标点集（vector）<Point2f>*
*输入参数3：单应性矩阵（Mat）
*/
void turn_coordinate(vector<Point2f>* obj_image, Mat* Homo, vector<Point2f> &world_image) {
	short number = 5;
	Mat temp;
	for (int i = 0; i < number; i++) {
		temp = (Mat_<double>(3, 1) << (*obj_image)[i].x, (*obj_image)[i].y, 1);//图像坐标点坐标强行转换成1*3
		temp = *Homo * temp;
		temp = temp / temp.at<double>(2, 0);//得到世界坐标系值
		cout << "obj_world[" << i  << "]=" << temp<< endl;//输出监控
		world_image.push_back(Point(temp.at<double>(0, 0), temp.at<double>(1, 0)));//推入world坐标系，供后续使用
	}
	cout << "*********************************************" << endl;
}
 
 
/*
*函数名称：pre_image
*功能：图像预处理、包括重设大小、改变颜色空间、均值化
*输入参数1：初始图象
*输入参数2：要得到的最终图像
*/
void pre_image(Mat* InputImage, Mat* OutputImage) {
	//重设大小
	int h = (*InputImage).rows;
	int w = (*InputImage).cols;
	resize(*InputImage, *InputImage, Size(w / 2, h / 2), 0, 0, INTER_LINEAR);//对照片进行缩小
 
	//色彩空间处理
	cvtColor(*InputImage, *OutputImage, COLOR_BGR2HSV); //转成hsv空间
 
}
 
 
/*
*函数名称：color_find
*功能：颜色识别、返回图像坐标
*输入参数1：坐标集合
*输入参数2：输入图像
* 输入参数3：输入图像的处理后的图像
* 输入参数4：要得到的斜率点集的起始点的集合
* 输入参数5：要得到的斜率点集的末尾点的集合
*/
void color_find(vector<Point2f>& obj_image, Mat& image, Mat& image_hsv, vector<Point2f>& first_image, vector<Point2f>& third_image) {
	Mat image_copy = image.clone();//绘制用，copy
	//imshow("image", image);
	string  color_string[5] = { "purple","yellow","red","blue","orange" };//按序显示字体使用
	//颜色识别
	Mat imgThresholded;
	for (int i = 0; i <= 4; i++) {
		//依次二值化
		switch (i) {
		case 0: 
			inRange(image_hsv, Scalar(128, 7, 130), Scalar(179, 82, 161), imgThresholded); //紫色
			break;
		case 1: 
			inRange(image_hsv, Scalar(20, 119, 208), Scalar(82, 255, 255), imgThresholded); //黄色
			break;
		case 2: 
			inRange(image_hsv, Scalar(0, 168, 242), Scalar(7, 212, 255), imgThresholded); //红色
			break;
		case 3: 
			inRange(image_hsv, Scalar(82, 157, 0), Scalar(104, 255, 197), imgThresholded); //蓝色
			break;
		case 4: 
			inRange(image_hsv, Scalar(11, 168, 245), Scalar(14, 189, 255), imgThresholded); //橙色
			break;
		}
 
		//形态学操作   ，根据情况选择是否要进行形态学操作
		Mat kernel = getStructuringElement(MORPH_RECT, Size(2, 2));
		Mat image_erode= imgThresholded;
		//morphologyEx(imgThresholded, image_erode, MORPH_CLOSE, kernel);
		//erode(image_erode, image_erode, kernel);
 
		//找最大轮廓、
		vector <vector<Point>> contours;//输出轮廓图像，是一个向量，向量的每个元素都是一个轮廓
		vector<Vec4i> hierachy;//各个轮廓的继承关系
		findContours(image_erode, contours, hierachy, RETR_EXTERNAL, CHAIN_APPROX_NONE, Point());
		int c_max_index=0;//最大轮廓的下标
		find_max_area(contours, c_max_index);//得到最大下标用于绘制
		drawContours(image_copy, contours, c_max_index, Scalar(0,255,0), 1, 8);
 
		//得到对应颜色外接矩形
		Rect rect = boundingRect(Mat(contours[c_max_index]));//被轮廓包围的矩形
		//rectangle(image_copy, rect, (0, 0, 0), 2, 8, 0);//绘制它
 
		//找到中心点位置，
		Moments mu = moments(contours[c_max_index]);
		Point centerPos = cv::Point(mu.m10 / mu.m00, mu.m01 / mu.m00);//轮廓矩求质心
		circle(image_copy, centerPos, 1, cv::Scalar(0, 0, 255),2);
		obj_image.push_back(centerPos);//推入图像坐标组中
 
		//绘制，做提示识别到了哪种颜色
		putText(image_copy, color_string[i], Point(rect.x + rect.width, rect.y+40), FONT_HERSHEY_COMPLEX, 0.5, (0, 255, 0), 1, LINE_8);
 
		//求最小外接矩形 青色
		vector<string> v_num;//绘制用 字符串容器
		v_num.push_back("1");
		v_num.push_back("2");
		v_num.push_back("3");
		v_num.push_back("4");
		RotatedRect rotateRect = minAreaRect(contours[c_max_index]);//最小外接矩形
		vector<Point2f> boxPts(4);
		rotateRect.points(boxPts.data());
 
		//利用最小外接矩形求得边角点
		for (int j = 0; j < 4; j++)
		{
			cv::line(image_copy, boxPts[j], boxPts[(j + 1) % 4], cv::Scalar(128, 128, 0), 1, 8);  //绘制最小外接矩形每条边
			//绘制，做提示得到点的位置，发现1和3总是对应的
			putText(image_copy, v_num[j], boxPts[j], FONT_HERSHEY_COMPLEX, 0.4, (0, 255, 255), 1, LINE_8);
		}
		first_image.push_back(boxPts[0]);
		third_image.push_back(boxPts[2]);//将点1和3分别推入其中
 
		//打印查看点是否正确
		//for (vector<Point2f>::iterator it = first_image.begin(); it != first_image.end(); it++) {
		//	cout << "第一个点x=" << (*it).x << "    第一个点y=" << (*it).y << endl;
		//}
		//for (vector<Point2f>::iterator it = third_image.begin(); it != third_image.end(); it++) {
		//	cout << "第三个点x=" << (*it).x << "    第三个点y=" << (*it).y << endl;
		//}
 
		//循环 等待按键Esc按下
		while (true) {
			imshow("image_erode", image_erode);
			imshow("image", image_copy);
			if (waitKey(0) == 27)
				break;
		}
 
	}
}
 
/*函数名称：寻找最大面积
* 输入：当前图像求得的轮廓，要得到的最大面积下标
* 功能：根据寻找到的轮廓，得到最大的轮廓
*/
void find_max_area(vector <vector<Point>>& contours, int& max_index) {
	vector<double> g_dConArea(contours.size());
	for (int i = 0; i < contours.size(); i++)//计算面积
	{
		g_dConArea[i] = contourArea(contours[i]);
		//cout << "【用轮廓面积计算函数计算出来的第" << i << "个轮廓的面积为：】" << g_dConArea[i] << endl;
	}
	for (int i = 1; i < contours.size(); i++) {//遍历数组，返回最大下标
		if (g_dConArea[i] > g_dConArea[max_index]) {
			max_index = i;
		}
	}
	//cout <<"当前最大轮廓下标："<< max_index<<endl;
}
 
 
/*函数名称：输入点得到要旋转的角度
* 输入参数1：起始点组
* 输入参数2：结束点组
* 输入参数3：要得到的角度组vector<double>
* 输入参数4：单应性矩阵Homo
*/
void get_angle(vector<Point2f>& first_image, vector<Point2f>& third_image, vector<double> &angle_obj, Mat *Homo) {
	short number = 5;
	Mat temp,temp_re;
	//vector<Point3f> p;
	vector<Point2f> first_world;
	vector<Point2f> third_world;
 
	//得到起始和末尾在世界坐标系下的点组
	for (int i = 0; i < number; i++) {
		//图像坐标点坐标强行转换成1*3
		temp = (Mat_<double>(3, 1) << first_image[i].x, first_image[i].y, 1);//first组操作
		temp_re = (Mat_<double>(3, 1) << third_image[i].x, third_image[i].y, 1);//third组操作
 
		//单应性变化
		temp = (*Homo) * temp;//first组操作
		temp_re = (*Homo) * temp_re;//third组操作
 
		//得到世界坐标系值
		temp = temp / temp.at<double>(2, 0);//first组操作
		cout << "first_world[" << i + 1 << "]=" << temp << endl;
		temp_re = temp_re / temp_re.at<double>(2, 0);//third组操作
		cout << "third_world[" << i + 1 << "]=" << temp_re << endl;
		//推入world组中
		first_world.push_back(Point(temp.at<double>(0, 0), temp.at<double>(1, 0)));
		third_world.push_back(Point(temp_re.at<double>(0, 0), temp_re.at<double>(1, 0)));
	}
	cout << "*********************************************" << endl;
 
	//得到斜率与角度
	double k_temp;//定义临时斜率值
	double angle_temp;//定义临时角度
	for (int i = 0; i < number; i++) {
		k_temp = (first_world[i].y - third_world[i].y) / (first_world[i].x - third_world[i].x);
		angle_temp = abs(atan(k_temp) * 180 / PI)+45;//解出角度
		angle_temp = angle_temp - 90.0;//得到角度差值
		cout << "angle ["<<i <<"] - 90° =  " << angle_temp << endl;//45是自给的，
		angle_obj.push_back(angle_temp);//推入目标组中
	}
}
 
 

2.source.h 存放编写的函数声明

#pragma once
#include <iostream>
#include <opencv2/opencv.hpp>
#include<cmath>
using namespace std;
using namespace cv;
void calculate_Homo(Mat* Homo);//根据现已知的点得到单应性矩阵
void turn_coordinate(vector<Point2f>* obj_image, Mat* Homo, vector<Point2f>& world_image);//将识别得到的图像坐标转换为机械臂的x，y，z世界坐标
 
void pre_image(Mat* InputImage, Mat* OutputImage);
void color_find(vector<Point2f>& obj_image, Mat& image, Mat& image_hsv, vector<Point2f>& first_image, vector<Point2f>& third_image);
void find_max_area(vector <vector<Point>>& contours, int& max_index);
void get_angle(vector<Point2f>& first_image, vector<Point2f>& third_image, vector<double>& angle_obj, Mat* Homo);

3. main.cpp 完成运动控制
 

// JAKAAPI.cpp : 定义控制台应用程序的入口点。
//
 
#include "stdafx.h"
#include <iostream>
#include <fstream>  
#include "JAKARobot_API.h"
#include "source.h"
#include<string.h>
#include <opencv2/opencv.hpp>
 
struct array_obj {
	int array[6];
};
 
 
 
 
using namespace std;
using namespace cv;
 
#define JAKA_Work 1//正常跑用这个
#define test 0//测试用
 
 
#if defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#elif defined(_MSC_VER)
#pragma warning(disable : 4996)
#endif
 
int main()
{
 
#if JAKA_Work
	//读取图片
	Mat image = imread("C://opencv_picture//3.jpg");
	Mat image_hsv;
	pre_image(&image, &image_hsv);//预处理
	vector<Point2f> obj_image;//图像坐标集,颜色依次为：紫、黄、绿 、蓝、红
	vector<Point2f> obj_world;//世界坐标系坐标
 
	vector<Point2f> first_image, third_image;//定义斜率点在图像坐标系中的起始点
	vector<double> angle_obj;//定义旋转角度组
	
	//颜色处理，得出坐标
	color_find(obj_image, image, image_hsv, first_image, third_image);
 
	//坐标变换
	Mat Homo;
	calculate_Homo(&Homo);//计算单应性矩阵
	turn_coordinate(&obj_image, &Homo,obj_world);//输入图像五个坐标求JAKA坐标系下的坐标 
 
	//得到每个目标的旋转角度,并打印输出
	get_angle(first_image, third_image, angle_obj, &Homo);//得到每个目标的旋转角度      angle=0-90°
 
	cout << endl << endl << endl;
	system("pause");
 
	//定义要搬运到的点组
	vector<Point2f> obj_jaka;//世界坐标系坐标
	obj_jaka.push_back(Point(365.573, -105.260));
	obj_jaka.push_back(Point(295.559, -171.921));
	obj_jaka.push_back(Point(365.573, -150.260));
	obj_jaka.push_back(Point(315.573, -150.260));
	obj_jaka.push_back(Point(315.573, -105.260));
 
	//下面是机器人操作
	cout << "下面是机器人操作"<<endl;
	JAKAZuRobot jakaRob;
	int is_enable = 0, is_powered = 0;
	int ret = jakaRob.login_in("192.168.1.100");
	if (ret) printf("%s\n", jakaRob.m_errMsg);
	jakaRob.getState(is_enable, is_powered);
	if (!is_powered) {
		ret = jakaRob.power(1);
		Sleep(6);
	}
	if (!is_enable) {
		ret = jakaRob.enable(1);
		Sleep(6);
	}
 
	cout << "等待机器人初始化" << endl;
	jakaRob.initSetting();
	Sleep(10);
	double jointpos1[6], jointpos2[6];
	jakaRob.getJoints(jointpos1);
	double jointsHome[6] = { 0, 0, 0, -180, 0, 90 };
	cout << "开始搬运各个色块" << endl;
 
	ret = jakaRob.setDout(7, 1);
	cout << "气爪松开" << endl;//气爪松开
 
	for (int k = 0; k < 5; k++) {//循环完成五个点的搬运
 
 
		jointsHome[0] = obj_world[k].x;
		jointsHome[1] = obj_world[k].y;
		jointsHome[2] = 200;
		ret = jakaRob.moveP(jointsHome, 10.0);//移动到目标点上方
		ret = jakaRob.waitEndMove();
		Sleep(200);
 
		jointsHome[5] = 90-angle_obj[k];//旋转、这里是“-”是根据得到的差值来进行机器人旋转的补偿
		ret = jakaRob.moveP(jointsHome, 10.0);//移动到目标点上方
		ret = jakaRob.waitEndMove();
		Sleep(500);
 
		jointsHome[2] = 120;//移动到色块处
		ret = jakaRob.moveP(jointsHome, 10.0);
		ret = jakaRob.waitEndMove();
 
		system("pause");//断点式可控
 
		jointsHome[2] = 105;//移动到色块处
		ret = jakaRob.moveP(jointsHome, 10.0);
		ret = jakaRob.waitEndMove();
		Sleep(100);
 
		ret = jakaRob.setDout(7, 0);
		cout << "气爪合上" << endl;//气爪合
 
		Sleep(500);
		jointsHome[2] = 200;//抬起来
		ret = jakaRob.moveP(jointsHome, 10.0);
		ret = jakaRob.waitEndMove();
 
		Sleep(500);
		jointsHome[0] = obj_jaka[k].x;
		jointsHome[1] = obj_jaka[k].y;//移动到要放置的点的上方
		ret = jakaRob.moveP(jointsHome, 10.0);
		ret = jakaRob.waitEndMove();
 
		Sleep(100);
		jointsHome[2] = 108;//移动到要放置的点
		ret = jakaRob.moveP(jointsHome, 10.0);
		ret = jakaRob.waitEndMove();
		Sleep(100);
		ret = jakaRob.setDout(7, 1);
		cout << "气爪松开" << endl;//气爪松开
		Sleep(200);
		jointsHome[2] = 200;//抬起来
		ret = jakaRob.moveP(jointsHome, 10.0);
		ret = jakaRob.waitEndMove();
		cout << "第" << k+1 << "个色块已经放置完毕" << endl;
 
 
		system("pause");//断点式可控
 
	}
 
	jakaRob.getJoints(jointpos2);
 
	jakaRob.login_out();
#endif
 
#if test
	//读取图片
	Mat image = imread("C://opencv_picture//5.jpg");
	Mat image_hsv;
	pre_image(&image, &image_hsv);//预处理
	vector<Point2f> obj_image;//图像坐标集,颜色依次为：紫、黄、绿 、蓝、红
	vector<Point2f> obj_world;//世界坐标系坐标
 
	vector<Point2f> first_image, third_image;//定义斜率点在图像坐标系中的起始点
	vector<double> angle_obj;//定义旋转角度组
 
	color_find(obj_image, image, image_hsv, first_image, third_image);
 
	//坐标变换
	Mat Homo;
	calculate_Homo(&Homo);//计算单应性矩阵
	turn_coordinate(&obj_image, &Homo, obj_world);//输入图像五个坐标求JAKA坐标系下的坐标 
 
	get_angle(first_image, third_image, angle_obj, &Homo);//得到每个目标的旋转角度      angle=0-90°
 
	
 
#endif
    return 0;
}