相机3D坐标到机械臂坐标的转换

本程序原理简单，就是RX+T，为了测试方便记录于此

已知

1、3D相机和机械臂进行了标定，得到3*4的刚体矩阵;

2、使用3D相机对物体进行拍摄，取其中1个坐标点P（x,y,z）;

求 ： 该坐标点P在机械臂世界坐标的位置 编程如下

其中circle.txt 中的圆心P点坐标为：

-21.5134 -10.5050 93.5234

RT.txt 中的机械臂标定后的刚体矩阵为：

0.02132  0.02397  0.99949  -91.46600
-0.00744  -0.99968  0.02414  -270.98030
0.99975  -0.00795  -0.02114  263.78220

结论：转换后的机械臂坐标 误差为20微米

#include <iostream>
#include <Eigen/Dense>
#include <fstream>
#include <string>

using namespace std;

typedef Eigen::Matrix<float, 3, 1>  Matrix31f;
//读取txt中的刚体矩阵
void readMatrix4f(string strPath,Eigen::Matrix3f &rotation,Matrix31f &transform)
{
	rotation = Eigen::Matrix3f::Identity();
	ifstream myfile(strPath.c_str(), std::ifstream::in);
	if (!myfile.is_open())
	{
		cout << "can not open this file" << strPath << endl;
		return ;
	}
	else{

		for (int i = 0;i < 3;i++)
			for (int j = 0; j < 4; j++)
			{
				if (j==3)
				{
					myfile >> transform(i, 0);
				}
				else
					myfile >> rotation(i, j);
			}

	}
	myfile.close();

	
	
}



//读取圆心和向量
void readCirclecenterAndVector(string strPath, Matrix31f &circle  )
{
	ifstream myfile(strPath.c_str(), std::ifstream::in);
	if (!myfile.is_open())
	{
		cout << "can not open this file "<< strPath << endl;
		return;
	}
	myfile >> circle(0, 0);
	myfile >> circle(1, 0);
	myfile >> circle(2, 0);


	myfile.close();
}
//success 进行相机世界坐标系到机械臂坐标系的转换
int main()
{
	//1、读取刚体矩阵
	Eigen::Matrix3f rotation;
	Matrix31f transform1={ 0,0,0 };
	string strPath_Maxtrix = "RT.txt";
	readMatrix4f(strPath_Maxtrix, rotation, transform1);
	cout << "rotation:" << endl << rotation << endl << endl;
	cout <<"transform1:"<<endl<< transform1 << endl << endl;
	//2、读取圆心和向量
	string strPath_vector = "circle.txt"; //保存圆心点，空格隔开
	Matrix31f  circle = {0,0,0};
	readCirclecenterAndVector(strPath_vector, circle);
	cout << "circle center: " <<endl<< circle << endl << endl;
	//计算
	Matrix31f result = rotation*circle + transform1;
	cout << "result: " << endl << result << endl << endl;


	system("pause");
	return 1;
}