SLAM学习-应用齐次坐标实现矩形平移

任务

了解齐次坐标在三维空间的优势,以及在计算机图形学中的应用后,进行实战练习:
利用键盘读取方向输入,控制矩形平移,其中矩形的坐标以齐次坐标形式表达.

齐次坐标

优势

1. 便于表达某点是否在直线上
2. 表达直线和直线交点，平面和平面交线
3. 区分向量和点
4. 能表示无穷远
5. 欧式变换中的旋转和平移：由加法到乘法

注意问题

旋转和平移先后顺序有区别

经过了坐标变换之后：

• 缩放变换不改变坐标轴的走向，也不改变原点的位置，所以两个坐标系仍然重合。
• 旋转变换改变坐标轴的走向，但不改变原点的位置，所以两个坐标系坐标轴不再处于相同走向。
• 平移变换不改变坐标轴走向，但改变原点位置，两个坐标系原点不再重合。

坐标变换的顺序必须是： 缩放->旋转->平移

实际演练

完整代码

#include<iostream>
#include<opencv2/opencv.hpp>
#include<Eigen/Dense>
using namespace std;
using namespace cv;
using namespace Eigen;

void drawSquare(Mat img,Point2i center,int length,Scalar color)
{
		Point2i p1(center.x-length/2,center.y-length/2);
		Point2i p2(center.x-length/2,center.y+length/2);
		Point2i p3(center.x+length/2,center.y+length/2);
		Point2i p4(center.x+length/2,center.y-length/2);
		line(img,p1,p2,color);
		line(img,p2,p3,color);
		line(img,p3,p4,color);
		line(img,p4,p1,color);
}
int translate(Vector2d&v)
{
		int key = waitKey(2000);
		int scale=10;
		key = key % 255;
		Vector3d homoV,tHomoV;
		homoV<<v,1;
		Matrix3d t=Matrix3d::Identity();
		cout<<"v:\n"<<v<<endl;	
		cout<<"t:\n"<<t<<endl;
		switch(key){
		case 82: //up
			t(1,2)=-scale;
			cout<<"up"<<endl;
		break;
		case 84: //down
			t(1,2)=scale;
			cout<<"down"<<endl;
		break;
		case 81://left
			t(0,2)=-scale;
			cout<<"left"<<endl;
		break;
		case 83://right
			t(0,2)=scale;
			cout<<"right"<<endl;
		break;
		default:
			cout<<"key="<<key<<endl;
			cout<<"other keys"<<endl;
			break;		
	}
	tHomoV=t*homoV;
	cout<<tHomoV<<endl;
	v(0,0)=tHomoV(0,0);
	v(1,0)=tHomoV(1,0);
	return key;
}

Point2i cvtVector2Point(Vector2d v)
{
		return Point2i(int(v(0,0)),int(v(1,0)));
}
int main(void)
{
	Mat bg=Mat(400,800,CV_8UC3,Scalar(0,0,0));
	int waitQ;
	imshow("img",bg);
	waitQ=waitKey(0);
	Vector2d o;
	o<<200,300;
	while(waitQ!=113)
	{
		waitQ=translate(o);
		drawSquare(bg,cvtVector2Point(o),40,Scalar(0,0,255));
		imshow("img",bg);
		bg=Scalar(0,0,0);
	}
	return 0;
}	

实现效果

参考文章

1. 从零开始一起学习SLAM | 为什么要用齐次坐标？
2. 齐次坐标的理解
3. 2D Translation in Computer Graphics | Definition | Examples
4. Eigen学习（六）高级初始化
5. 【OpenCv】cvWaitKey获取键盘值
6. OpenCV doc of waitKey()