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

aruco_detect: add length_override parameter for overriding individual marker’s length

Browse Source
This commit is contained in:
Oleg Kalachev
2019-02-22 11:11:19 +03:00
parent 70e1d6e5fd
commit 306185aafe
1 changed files with 45 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
aruco_pose/src/aruco_detect.cpp
View File

@@ -17,6 +17,8 @@
#include <math.h>
#include <vector>
#include <string>
#include <map>
#include <unordered_map>
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
@@ -62,6 +64,7 @@ private:
ros::Publisher markers_pub_, vis_markers_pub_;
bool estimate_poses_, send_tf_;
double length_;
std::unordered_map<int, double> length_override_;
std::string frame_id_prefix_, snap_orientation_;
Mat camera_matrix_, dist_coeffs_;
aruco_pose::MarkerArray array_;
@@ -81,6 +84,8 @@ public:
ROS_FATAL("aruco_detect: can't estimate marker's poses as ~length parameter is not defined");
ros::shutdown();
}
readLengthOverride();
nh_priv_.param<std::string>("snap_orientation", snap_orientation_, "");
nh_priv_.param<std::string>("frame_id_prefix", frame_id_prefix_, "aruco_");
@@ -128,6 +133,24 @@ private:
cv::aruco::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_,
rvecs, tvecs);
// process length override, TODO: efficiency
if (!length_override_.empty()) {
for (unsigned int i = 0; i < ids.size(); i++) {
int id = ids[i];
auto item = length_override_.find(id);
if (item != length_override_.end()) { // found override
vector<cv::Vec3d> rvecs_current, tvecs_current;
vector<vector<cv::Point2f>> corners_current;
corners_current.push_back(corners[i]);
cv::aruco::estimatePoseSingleMarkers(corners_current, item->second,
camera_matrix_, dist_coeffs_,
rvecs_current, tvecs_current);
rvecs[i] = rvecs_current[0];
tvecs[i] = tvecs_current[0];
}
}
}
if (!snap_orientation_.empty()) {
try {
snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, snap_orientation_,
@@ -181,7 +204,7 @@ private:
for (unsigned int i = 0; i < ids.size(); i++)
pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose.pose,
length_, ids[i], i);
getMarkerLength(ids[i]), ids[i], i);
vis_markers_pub_.publish(vis_array_);
}
@@ -192,7 +215,8 @@ private:
cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
if (estimate_poses_)
for (unsigned int i = 0; i < ids.size(); i++)
cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], length_);
cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_,
rvecs[i], tvecs[i], getMarkerLength(ids[i]));
cv_bridge::CvImage out_msg;
out_msg.header.frame_id = msg->header.frame_id;
@@ -279,6 +303,25 @@ private:
{
return frame_id_prefix_ + std::to_string(id);
}
void readLengthOverride()
{
std::map<std::string, double> length_override;
nh_priv_.getParam("length_override", length_override);
for (auto const& item : length_override) {
length_override_[std::stoi(item.first)] = item.second;
}
}
inline double getMarkerLength(int id)
{
auto item = length_override_.find(id);
if (item != length_override_.end()) {
return item->second;
} else {
return length_;
}
}
};
PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet)