37 : params_(), markerMapConfig_(markerMapConfig) {
48 static cv::Mat
getRTMatrix(
const cv::Mat &_rvec,
const cv::Mat &_tvec) {
51 cv::Mat m = cv::Mat::eye(4, 4, CV_32FC1);
52 cv::Mat R33 = cv::Mat(m, cv::Rect(0, 0, 3, 3));
53 cv::Rodrigues(_rvec, R33);
54 for (
int i = 0; i < 3; i++)
55 m.at<
float>(i, 3) = _tvec.ptr<
float>(0)[i];
60 cv::Mat &camera_k, cv::Mat &camera_d,
61 std::vector<MarkerPose> &markerPoses) {
63 estimator.estimatePose(markerFiducials, camera_k, camera_d, markerPoses);
std::vector< MarkerMapDetails > markerMaps
std::vector< MarkerMapEstimator > estimators_
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)
~PoseEstimationMarkerMapBase()
PoseEstimationMarkerMapParameters & getParameters()
MarkerMapConfig markerMapConfig_
PoseEstimationMarkerMapParameters params_
PoseEstimationMarkerMapBase(MarkerMapConfig markerMapConfig)