42 static cv::Mat
getRTMatrix(
const cv::Mat &_rvec,
const cv::Mat &_tvec) {
45 cv::Mat m = cv::Mat::eye(4, 4, CV_32FC1);
46 cv::Mat R33 = cv::Mat(m, cv::Rect(0, 0, 3, 3));
47 cv::Rodrigues(_rvec, R33);
48 for (
int i = 0; i < 3; i++)
49 m.at<
float>(i, 3) = _tvec.ptr<
float>(0)[i];
54 cv::Mat &camera_k, cv::Mat &camera_d,
55 std::vector<MarkerPose> &markerPoses) {
56 for (
auto &marker:markers) {
61 cv::solvePnP(marker.object_points, marker.image_points, camera_k, camera_d, rv, tv);
64 rv.convertTo(rvec, CV_32F);
65 tv.convertTo(tvec, CV_32F);
67 MarkerPose pose(marker.ids, marker.ids_confidence);
71 markerPoses.push_back(pose);
PoseEstimationParameters & getParameters()
PoseEstimationParameters params_
void estimatePose(std::vector< MarkerFiducials > &markers, cv::Mat &camera_k, cv::Mat &camera_d, std::vector< MarkerPose > &markerPoses)
static cv::Mat getRTMatrix(const cv::Mat &_rvec, const cv::Mat &_tvec)