CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_map: add ability to pass number of detected markers to Pose covariance field

Browse Source
This commit is contained in:
Oleg Kalachev
2023-07-30 14:04:59 +07:00
parent 6fb84ae584
commit 2f8915ce31
1 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
aruco_pose/src/aruco_map.cpp
View File

@@ -83,7 +83,7 @@ private:
visualization_msgs::MarkerArray vis_array_;
std::string known_vertical_, map_, markers_frame_, markers_parent_frame_;
int image_width_, image_height_, image_margin_;
bool flip_vertical_, auto_flip_, image_axis_;
bool flip_vertical_, auto_flip_, image_axis_, put_markers_count_to_covariance_;
public:
virtual void onInit()
@@ -111,6 +111,7 @@ public:
image_height_ = nh_priv_.param("image_height", 2000);
image_margin_ = nh_priv_.param("image_margin", 200);
image_axis_ = nh_priv_.param("image_axis", true);
put_markers_count_to_covariance_ = nh_priv_.param("put_markers_count_to_covariance", false);
markers_parent_frame_ = nh_priv_.param<std::string>("markers/frame_id", transform_.child_frame_id);
markers_frame_ = nh_priv_.param<std::string>("markers/child_frame_id_prefix", "");
@@ -178,6 +179,11 @@ public:
corners.push_back(marker_corners);
}
if (put_markers_count_to_covariance_) {
// HACK: pass markers count using covariance field
pose_.pose.covariance[0] = markers->markers.size();
}
if (known_vertical_.empty()) {
// simple estimation
valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,