本程序原理简单,就是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;
}