How to compute CamtoTag pose , I have get 3D feature Point( chessboard )

[Fireforce123]

In my project, the 3D camera is on the top floor , and i use this handeye_calib_camodocal project to finish my handeye calibration。But the error is large, and result is clearly an error，so i doubt the CamtoTag is wrong . I have get 3D Checkerboard corner points(x,y,z) ,and here is the codes i used to compute CamtoTag :

Eigen::Matrix4f getPlanePose(pcl::PointCloud::Ptr board_corner_cloud) { pcl::SACSegmentation seg; seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.1);

seg.setInputCloud(board_corner_cloud);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

seg.segment(*inliers, *coefficients);

if (inliers->indices.size() == 0)
{
    PCL_ERROR("Could not estimate a planar model for the given dataset.");
    //return -1;
}

//// 平面方程: ax + by + cz + d = 0
float a = coefficients->values[0];
float b = coefficients->values[1];
float c = coefficients->values[2];
float d = coefficients->values[3];

// 计算平面法线并规范化
Eigen::Vector3f plane_normal(a, b, c);
plane_normal.normalize();

// 计算标定板的一个已知参考点的坐标（例如，可以是标定板的一个角点或中心点）
// 这里需要你根据实际情况来提供这个参考点的坐标
Eigen::Vector3f reference_point_on_plane(board_corner_cloud->points[36].x, board_corner_cloud->points[36].y, board_corner_cloud->points[36].z); // 假设已经得到了参考点的坐标

// 计算旋转矩阵，使得平面法线对齐到z轴
Eigen::Vector3f z_axis(0, 0, 1);
Eigen::Quaternionf quat = Eigen::Quaternionf::FromTwoVectors(plane_normal, z_axis);

// 计算标定板的位姿矩阵
Eigen::Matrix4f calibration_board_pose = Eigen::Matrix4f::Identity();
calibration_board_pose.block<3, 3>(0, 0) = quat.toRotationMatrix();
calibration_board_pose.block<3, 1>(0, 3) = reference_point_on_plane;
return calibration_board_pose;

}