int main()
{
//通过IP端口创建协议
auto protocol = techlego::create_binary_protocol(L"localhost", 5252);
//通过协议创建客户端
auto client = techlego::h_scan3d_client::make_shared(protocol);
//坐标系1下的点坐标
std::array<techlego::pos3f, 4>points1 = {
{ {-65.4562 ,-54.4644,378.7092},
{-65.3003 ,-54.4709 ,378.7567},
{-15.7148 ,-14.9020,380.3256 },
{-65.2903 ,- 54.4111 ,378.7333} } };
//坐标系2下的点坐标
std::array<techlego::pos3f, 4>points2 = {};
// 构建绕Z轴旋转45度的旋转矩阵
Eigen::AngleAxisd angle_axis(M_PI / 4, Eigen::Vector3d(0, 0, 1));
Eigen::Matrix3d m = angle_axis.toRotationMatrix();
// 构建平移向量
Eigen::Vector3d translate_vector(1, 3, 4);
// 对坐标系1中的点应用之前构建的旋转矩阵和平移向量,得到坐标系2下的该点
for (int i = 0; i < points1.size(); i++)
{
// 用变换矩阵对坐标进行变换
// 点在坐标系1下坐标
Eigen::Vector3d point(points1[i].m_x, points1[i].m_y, points1[i].m_z);
// 变换,相当于【旋转矩阵*坐标+平移向量】
Eigen::Vector3d point_after_trans = m * point + translate_vector;
std::cout << "变换后的坐标为:" << std::endl << point_after_trans.transpose() << std::endl;
points2[i].m_x = point_after_trans(0);
points2[i].m_y = point_after_trans(1);
points2[i].m_z = point_after_trans(2);
}
//输入,配对点的数量
int total = static_cast<int>(points1.size());
//输出,旋转矩阵
//输出,平移向量
double mm[3][3] = { 0 }, t[3] = { 0 };
//由点对获取坐标系1变换到坐标系2下的旋转矩阵和平移向量
auto pairs = techlego::am::get_rt_by_pt_pairs(points2.data(), points1.data(), total, mm, t);
std::cout << "原旋转矩阵:\n" << m << "\n";
std::cout << "原平移向量:" << translate_vector.transpose() << "\n";
std::cout << "解得的旋转矩阵:";
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
std::cout << " " << mm[i][j] << '\t';
}
std::cout << std::endl;
}
std::cout << "解得的平移向量:";
for (int i = 0; i < 3; i++)
{
std::cout << t[i] << " ";
}
std::cout << "\n各个点对距离的平方和:" << pairs << std::endl;
return 0;
}