在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，也就是角度值，最初的时候我很是纳闷，怎么会用角度值来表示一个点的坐标的，如下图示例：

通过角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的时候经常逛大神的博客，代码就直接拷贝然后修改参数