在NAO中定位目标图像的空间坐标

来实验室有一段时间了,最近一直在做小球的空间坐标定位,其实思路还是蛮简单的。

完成目标:根据获取摄像头图像中的目标图像的信息,来将其定位到nao的world_space中,计算出目标图像在nao空间中的坐标。这里以识别红色小球RedBall为例。我这里是下载 nao官网上的 c++ sdk,在visual stdio 2010 编程环境下,做了一个remote module 提供给指令盒进行调用。

第一步 获取nao的视觉图像,这里就不多做介绍了,帮助文档中有例子,直接拿过来用就好了。

第二步 在获取的图像中,将RedBall识别出来,并能够获取一些基本信息。

之前有过一个粗糙版的思路:获取图像之后,将其他无关项置为黑色,将小球置为白色,然后获取其灰度图像,进行简单滤波,然后应用霍夫变化,识别出图像中的圆,代码如下:

//去除干扰项,使得图像尽可能简洁
	CvScalar pixel= {};  

	for (int i=0;i<imgHeader->height;i++)  
	{  
		for (int j=0;j<imgHeader->width;j++)  
		{  
			pixel=cvGet2D(imgHeader,i,j);  


			if (pixel.val[0]<120&&pixel.val[1]>150&&pixel.val[2]>150)//将背景及无关项直接置黑色 
			{  
				//目标图像置白     
				pixel.val[0]=255;  
				pixel.val[1]=255;  
				pixel.val[2]=255;  
				cvSet2D(imgHeader, i, j, pixel);  
			}  
			else //将无关项置为白色 
			{  
				pixel.val[0] = 0;  
				pixel.val[1] = 0;  
				pixel.val[2] = 0;  
				cvSet2D(imgHeader, i, j, pixel);  
			}  
		}  
	}

	//cvShowImage("images2", imgHeader);
	//获取灰度图像,以方便后面的霍夫变换
	IplImage* gray = cvCreateImage( cvGetSize(imgHeader), 8, 1 );  
	CvMemStorage* storage = cvCreateMemStorage(0);  
	cvCvtColor( imgHeader, gray, CV_BGR2GRAY );  
	cvSmooth( gray, gray, CV_GAUSSIAN, 9,9); // smooth it, otherwise a lot of false circles may be detected  
	//cvShowImage("gray",gray);
	//霍夫变换 识别圆形
	
	CvSeq* circles = cvHoughCircles( gray, storage, CV_HOUGH_GRADIENT, 2, gray->height/4, 200, 30,1,60);  

这样的效果并不好,因为是手动调整像素值,结果很是粗糙
有一个改良版,在这里把思路稍微说一下,其他的内容并没有多少变化,只是在进行干扰项的屏蔽上,将图像转化为hsv格式,这样只需要对h通道的内容进行判定就好了,事先将小球的h值的范围获取,再在程序中将不是这个范围的内容屏蔽掉。这样效果应该是要好一些。
对于霍夫变换函数cvHoughCircles 返回值的内容,我们需要的信息不过是,所识别出的圆的圆心坐标和半径。
cvHoughCircles 的返回值类型为 cvSeq类型,显然我们只需要他的第一项内容来帮助我们完成之后的工作。
float* p = (float*)cvGetSeqElem( circles, 0 );
cvCircle( imgHeader, cvPoint(cvRound(p[0]),cvRound(p[1])), cvRound(p[2]),CV_RGB(255,0,0), 3, 8, 0 );
这里获取的半径实际上是像素个数,而圆心也是相对于左上角的像素个数(获取的图像大小是320*240),而在naoqi提供的一些函数里面,要用的是angleSize,也就是角度值,最初的时候我很是纳闷,怎么会用角度值来表示一个点的坐标的,如下图示例:

在NAO中定位目标图像的空间坐标_第1张图片

通过角1,角2,便可以表示出(x,y)的坐标位置,有一个形象一点的比喻:假设当前nao的头位于初始位置(视觉的中心位置位于(0,0)),若想要将视觉的中心位置移至(x,y),必然是是要将头部向左摆动角1 弧度,再向上摆动角2弧度。
通过camProxy.getAngPosFromImgPos()函数,我们可以直接得到所需要的弧度值。下面gc来了

第三步,通过求得的小球的半径,圆心坐标的弧度值来进行空间坐标的定位

这里是NAO论坛上的一个回复贴,空间定位的思路也是从这里找到的点击打开链接

简单来说就是利用矩阵变换,通过矩阵连乘,进而得到目标物体在WORLD_SPACE下的空间坐标位置。

在NAO中可以利用平移矩阵来表示一个物体在空间中的位置,通过平移矩阵作乘,可以在几个局部空间坐标系下进行坐标变换。

已有信息:小球的实际尺寸大小;小球图像的半径及圆心坐标的弧度值表示;

这里我们的目标是得到世界物体在NAO的world_SPACE中的空间坐标,

首先我们利用函数getTransform()得到摄像头的空间位置,这里得到的也是一个transform;之后我们又通过小球的圆心弧度值,得到摄像头到小球图像的transform;

再之后根据小球的实际尺寸大小,和小球图像中的半径弧度值大小,利用三角函数,得到小球图像到真实小球的transform,再将三者连乘,即得到真实小球在NAO空间中的空间坐标。


naoToRedBall = naoToCamera * cameraToRedBallImage * redBallImageToRedBal

std::vector<float> HelloWorld::getPosition()
{
	//获取naoToCameraTop的transform
	
	AL::ALMotionProxy motion(robotIP);

	//std::vector<float> angles = getAngles(); 该句删除的原因是假设能在isNewData中调用getAngles,并将新的值赋给angles
	// Example showing how to get the end of the right arm as a transform
	// represented in torso space.
	const float centX = angles[0];
	const float centY = angles[1];

	std::string name  = "CameraTop";
	int space  = 2; // TORSO_SPACE
	bool useSensorValues  = false;
	std::vector<float> naoToCameraVector = motion.getTransform(name, space, useSensorValues);
	AL::Math::Transform naoToCamera = AL::Math::Transform(naoToCameraVector);
	//std::cout << "Transform of naoToCamera" << std::endl;
	//outPutTransForm(naoToCamera.toVector());

	/*获取cameraToredBallInImage的tansform,根据图像中获取的centX centY的值,
	构造rotation,进而构造transform*/
	//AL::Math::Rotation rotation = AL::Math::Rotation::from3DRotation(0,centY,centX);
	AL::Math::Transform cameraToImage = AL::Math::transformFrom3DRotation(0,centY,centX);
	//std::cout << "Transform of CameratoImage" << std::endl;
	//outPutTransForm(cameraToImage.toVector());

	/*构造BallInImageToReal的transform,在这次转换中,只是沿X轴向前平移distance段距离
	(这里的distance是camera到ball的距离,可以通过三角函数求的)*/
	float distance = RADIUS_BALL / sinf(angles[2]);
	AL::Math::Transform imageToReal = AL::Math::transformFromPosition(distance,0.0,0.0,0.0,0.0,0.0);
	//std::cout << "Transform of imageToReal" << std::endl;
	//outPutTransForm(imageToReal.toVector());

	
	/*构造naoToRealball的transform,其中的部分内容即为Ball在NAO_SPACE中的三维坐标*/
	AL::Math::Transform naoToRealBall = naoToCamera*cameraToImage*imageToReal;
	//outPutTransForm(naoToRealBall.toVector());
	std::vector<float> naotoRealBallVector = naoToRealBall.toVector();

	std::vector<float> position;
	position.push_back(naotoRealBallVector[3]);
	position.push_back(naotoRealBallVector[7]);
	position.push_back(naotoRealBallVector[11]);
	return position;
}

发现把自己做的东西再清晰地展示出来还不是那么容易的一件事情,只能说继续努力了。

另外感谢一下liuying_1001,在刚接触NAO的时候经常逛大神的博客,代码就直接拷贝然后修改参数大笑


你可能感兴趣的:(Module,distance)