CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-06-04 00:39:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_pose: known_tilt => known_vertical, add flip_vertical parameter (#476)

Browse Source 
* aruco_pose: rename parameter known_tilt to known_vertical

* More clean variable names

* aruco_pose: add flip_vertical parameter and get rid of map_flipped

* selfcheck.py: support flip_vertical parameter

* aruco_pose: document flip_vertical parameter

* selfcheck.py: fix known_vertical description

* Fix editorconfig
This commit is contained in:
Oleg Kalachev
2022-11-12 01:33:15 +06:00
committed by GitHub
parent 94a95b28b3
commit 95db8ba1b1
6 changed files with 62 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
aruco_pose/src/aruco_map.cpp
View File

@@ -81,9 +81,9 @@ private:
bool enabled_ = true;
std::string type_;
visualization_msgs::MarkerArray vis_array_;
std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
std::string known_vertical_, map_, markers_frame_, markers_parent_frame_;
int image_width_, image_height_, image_margin_;
bool auto_flip_, image_axis_;
bool flip_vertical_, auto_flip_, image_axis_;
public:
virtual void onInit()
@@ -104,7 +104,8 @@ public:
type_ = nh_priv_.param<std::string>("type", "map");
transform_.child_frame_id = nh_priv_.param<std::string>("frame_id", "aruco_map");
known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
known_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name
flip_vertical_ = nh_priv_.param<bool>("flip_vertical", false);
auto_flip_ = nh_priv_.param("auto_flip", false);
image_width_ = nh_priv_.param("image_width" , 2000);
image_height_ = nh_priv_.param("image_height", 2000);
@@ -177,7 +178,7 @@ public:
corners.push_back(marker_corners);
}
if (known_tilt_.empty()) {
if (known_vertical_.empty()) {
// simple estimation
valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
rvec, tvec, false);
@@ -191,7 +192,7 @@ public:
} else {
Mat obj_points, img_points;
// estimation with "snapping"
// estimation with known vertical
cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
if (obj_points.empty()) goto publish_debug;
@@ -203,11 +204,11 @@ public:
fillTransform(transform_.transform, rvec, tvec);
try {
geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
known_tilt_, markers->header.stamp, ros::Duration(0.02));
snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
geometry_msgs::TransformStamped vertical = tf_buffer_.lookupTransform(markers->header.frame_id,
known_vertical_, markers->header.stamp, ros::Duration(0.02));
applyVertical(transform_.transform.rotation, vertical.transform.rotation, flip_vertical_, auto_flip_);
} catch (const tf2::TransformException& e) {
NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
NODELET_WARN_THROTTLE(1, "can't retrieve known vertical: %s", e.what());
}
geometry_msgs::TransformStamped shift;