tcpjkhk

#include “mainwindow.h”
#include
#include
#include
#include <eigen3/Eigen/Geometry>
void get_xyz(Eigen::Vector3d P1,
Eigen::Vector3d P2,
Eigen::Vector3d P3,
Eigen::Vector3d P4,
double &x, double &y, double &z)//空间四点确定球心坐标(克莱姆法则)
{
double x1=P1(0); double y1=P1(1); double z1=P1(2);
double x2=P2(0); double y2=P2(1); double z2=P2(2);
double x3=P3(0); double y3=P3(1); double z3=P3(2);
double x4=P4(0); double y4=P4(1); double z4=P4(2);
double a11,a12,a13,a21,a22,a23,a31,a32,a33,b1,b2,b3,d,d1,d2,d3;
a11=2*(x2-x1); a12=2*(y2-y1); a13=2*(z2-z1);
a21=2*(x3-x2); a22=2*(y3-y2); a23=2*(z3-z2);
a31=2*(x4-x3); a32=2*(y4-y3); a33=2*(z4-z3);
b1=x2x2-x1x1+y2y2-y1y1+z2z2-z1z1;
b2=x3x3-x2x2+y3y3-y2y2+z3z3-z2z2;
b3=x4x4-x3x3+y4y4-y3y3+z4z4-z3z3;
d=a11a22a33+a12a23a31+a13a21a32-a11a23a32-a12a21a33-a13a22a31;
d1=b1a22a33+a12a23b3+a13b2a32-b1a23a32-a12b2a33-a13a22b3;
d2=a11b2a33+b1a23a31+a13a21b3-a11a23b3-b1a21a33-a13b2a31;
d3=a11a22b3+a12b2a31+b1a21a32-a11b2a32-a12a21b3-b1a22a31;
x=d1/d;
y=d2/d;
z=d3/d;

}
void TCP_geometric_calibration(double Tx[4][16],double T_tool[16]){
int pointNum=4;
Eigen::Matrix3d RotationMatrix[4];
Eigen::Vector3d P_be[4];
Eigen::Vector3d P_bt;
Eigen::Vector3d P_et[4];
for(int i=0;i<pointNum;i++)
{
for(int m=0;m<3;m++)
{
for(int n=0;n<3;n++)
{
RotationMatrixi = Tx[i][m4+n];
}
P_bei = Tx[i][m4+3];
}
}
double x,y,z;
get_xyz(P_be[0],P_be[1],P_be[2],P_be[3],x,y,z);
P_bt(0)=x;
P_bt(1)=y;
P_bt(2)=z;
for (int i=0; i<4; i++){
P_et[i]=RotationMatrix[i].inverse()*(P_bt-P_be[i]);
qDebug()<<P_eti<<P_eti<<P_eti;
}

T_tool[3]=(P_et[0](0)+P_et[1](0)+P_et[2](0)+P_et[3](0))/4;
T_tool[7]=(P_et[0](0)+P_et[1](0)+P_et[2](0)+P_et[3](0))/4;
T_tool[11]=(P_et[0](0)+P_et[1](0)+P_et[2](0)+P_et[3](0))/4;

}
int TCP_point_evaluation(double Tx[4][16]){
int pointNum = 6;
for(int i=0; i<pointNum-3; i++){
for(int j>i; j<pointNum-2; j++){
for(int k>j; k<pointNum-1; k++){
for(int l>k; l<pointNum; l++){

        }
    }
}

}
}
int main(int argc, char argv[])
{
// double x0=rand();
// double y0=rand();
// double z0=rand();
// double r=rand();
// qDebug()<<x0<<y0<<z0<<r;
// double theta1=rand();
// double phi1=rand();
// double x1=x0+rsin(theta1)cos(phi1);
// double y1=y0+rsin(theta1)sin(phi1);
// double z1=z0+rcos(theta1);
// double theta2=rand();
// double phi2=rand();
// double x2=x0+rsin(theta2)cos(phi2);
// double y2=y0+rsin(theta2)sin(phi2);
// double z2=z0+rcos(theta2);
// double theta3=rand();
// double phi3=rand();
// double x3=x0+rsin(theta3)cos(phi3);
// double y3=y0+rsin(theta3)sin(phi3);
// double z3=z0+rcos(theta3);
// double theta4=rand();
// double phi4=rand();
// double x4=x0+r*sin(theta4)cos(phi4);
// double y4=y0+rsin(theta4)sin(phi4);
// double z4=z0+rcos(theta4);
// double x,y,z;
// //get_xyz(x1,y1,z1,x2,y2,z2,x3,y3,z3,x4,y4,z4,x,y,z);
// qDebug()<<x<<y<<z;
double Tx[4][16]={0.121146, 0.905133, 0.407503, -0.350761, 0.968256, -0.0173308, -0.249357, -0.0694317, -0.218639, 0.424776, -0.8785, 0.299594, 0, 0, 0, 1,
-0.549799, 0.78906, 0.274054, -0.334246, 0.766886, 0.346789, 0.540022, -0.168092, 0.33107, 0.507071, -0.795783, 0.288264, 0, 0, 0, 1,
-0.155993, 0.879464, -0.449676, -0.249111, 0.915521, 0.299629, 0.268411, -0.133688, 0.370794, -0.369818, -0.851908, 0.305591, 0, 0, 0, 1,
0.0623122, 0.887835, -0.455923, -0.245275, 0.958953, -0.179874, -0.219213, -0.0703058, -0.276634, -0.423549, -0.8626, 0.310602, 0, 0, 0, 1};

double T_tool[16];
TCP_geometric_calibration(Tx,T_tool);
qDebug()<<"sd"<<T_tool[3]<<T_tool[7]<<T_tool[11];
QApplication a(argc, argv);
MainWindow w;
w.show();
return a.exec();

}