Skip to content

Commit

Permalink
aruco_pose nodelet cleanup (#239)
Browse files Browse the repository at this point in the history 
* aruco_pose: Unhardcode contour refinement

Besides, this was basically a no-op anyway, since dynamic parameters
overwrote that anyway.

* aruco_pose: Late-construct objects that use ROS

* aruco_map: Don't create/store node handle

* aruco_pose: Don't assume dist_coeffs size

* aruco_pose: more const == more better

* aruco_pose: Be more obvious about changing variables

* aruco_pose: Fix building for Kinetic

* aruco_pose: Remove global add_definitions
  • Loading branch information
@sfalexrog
sfalexrog authored May 29, 2020
1 parent c5e954b commit 9987960
Show file tree
Hide file tree
Showing 4 changed files with 43 additions and 48 deletions.
2 changes: 0 additions & 2 deletions aruco_pose/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,6 @@
cmake_minimum_required(VERSION 3.0)
project(aruco_pose)

add_definitions(-std=c++11 -Wall -g)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)

Expand Down
41 changes: 21 additions & 20 deletions aruco_pose/src/aruco_detect.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,10 +58,9 @@ using cv::Mat;

class ArucoDetect : public nodelet::Nodelet {
private:
ros::NodeHandle nh_, nh_priv_;
tf2_ros::TransformBroadcaster br_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_{tf_buffer_};
std::unique_ptr<tf2_ros::TransformBroadcaster> br_;
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>> dyn_srv_;
cv::Ptr<cv::aruco::Dictionary> dictionary_;
cv::Ptr<cv::aruco::DetectorParameters> parameters_;
Expand All @@ -81,30 +80,32 @@ class ArucoDetect : public nodelet::Nodelet {
public:
virtual void onInit()
{
nh_ = getNodeHandle();
nh_priv_ = getPrivateNodeHandle();
ros::NodeHandle& nh_ = getNodeHandle();
ros::NodeHandle& nh_priv_ = getPrivateNodeHandle();

br_.reset(new tf2_ros::TransformBroadcaster());
tf_buffer_.reset(new tf2_ros::Buffer());
tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_, nh_));

int dictionary;
nh_priv_.param("dictionary", dictionary, 2);
nh_priv_.param("estimate_poses", estimate_poses_, true);
nh_priv_.param("send_tf", send_tf_, true);
dictionary = nh_priv_.param("dictionary", 2);
estimate_poses_ = nh_priv_.param("estimate_poses", true);
send_tf_ = nh_priv_.param("send_tf", true);
if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined");
ros::shutdown();
}
readLengthOverride();
readLengthOverride(nh_priv_);

nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
nh_priv_.param("auto_flip", auto_flip_, false);
known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
auto_flip_ = nh_priv_.param("auto_flip", false);

nh_priv_.param<std::string>("frame_id_prefix", frame_id_prefix_, "aruco_");
frame_id_prefix_ = nh_priv_.param<std::string>("frame_id_prefix", "aruco_");

camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);

dictionary_ = cv::aruco::getPredefinedDictionary(static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(dictionary));
parameters_ = cv::aruco::DetectorParameters::create();
parameters_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;

image_transport::ImageTransport it(nh_);
image_transport::ImageTransport it_priv(nh_priv_);
Expand Down Expand Up @@ -170,8 +171,8 @@ class ArucoDetect : public nodelet::Nodelet {

if (!known_tilt_.empty()) {
try {
snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, known_tilt_,
msg->header.stamp, ros::Duration(0.02));
snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
msg->header.stamp, ros::Duration(0.02));
} catch (const tf2::TransformException& e) {
NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
}
Expand Down Expand Up @@ -205,7 +206,7 @@ class ArucoDetect : public nodelet::Nodelet {
if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
transform.transform.rotation = marker.pose.orientation;
fillTranslation(transform.transform.translation, tvecs[i]);
br_.sendTransform(transform);
br_->sendTransform(transform);
}
}
}
Expand Down Expand Up @@ -326,10 +327,10 @@ class ArucoDetect : public nodelet::Nodelet {
return frame_id_prefix_ + std::to_string(id);
}

void readLengthOverride()
void readLengthOverride(ros::NodeHandle& nh)
{
std::map<std::string, double> length_override;
nh_priv_.getParam("length_override", length_override);
nh.getParam("length_override", length_override);
for (auto const& item : length_override) {
length_override_[std::stoi(item.first)] = item.second;
}
Expand Down
44 changes: 21 additions & 23 deletions aruco_pose/src/aruco_map.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,6 @@ typedef message_filters::sync_policies::ExactTime<Image, CameraInfo, MarkerArray

class ArucoMap : public nodelet::Nodelet {
private:
ros::NodeHandle nh_, nh_priv_;
ros::Publisher img_pub_, pose_pub_, markers_pub_, vis_markers_pub_;
image_transport::Publisher debug_pub_;
message_filters::Subscriber<Image> image_sub_;
Expand All @@ -83,8 +82,8 @@ class ArucoMap : public nodelet::Nodelet {
public:
virtual void onInit()
{
nh_ = getNodeHandle();
nh_priv_ = getPrivateNodeHandle();
ros::NodeHandle &nh_ = getNodeHandle();
ros::NodeHandle &nh_priv_ = getPrivateNodeHandle();

image_transport::ImageTransport it_priv(nh_priv_);

Expand All @@ -96,27 +95,26 @@ class ArucoMap : public nodelet::Nodelet {
board_->dictionary = cv::aruco::getPredefinedDictionary(
static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);

std::string type, map;
nh_priv_.param<std::string>("type", type, "map");
nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
nh_priv_.param("auto_flip", auto_flip_, false);
nh_priv_.param("image_width", image_width_, 2000);
nh_priv_.param("image_height", image_height_, 2000);
nh_priv_.param("image_margin", image_margin_, 200);
nh_priv_.param("image_axis", image_axis_, true);
nh_priv_.param<std::string>("markers/frame_id", markers_parent_frame_, transform_.child_frame_id);
nh_priv_.param<std::string>("markers/child_frame_id_prefix", markers_frame_, "");
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", "");
auto_flip_ = nh_priv_.param("auto_flip", false);
image_width_ = nh_priv_.param("image_width" , 2000);
image_height_ = nh_priv_.param("image_height", 2000);
image_margin_ = nh_priv_.param("image_margin", 200);
image_axis_ = nh_priv_.param("image_axis", true);
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", "");

// createStripLine();

if (type == "map") {
param(nh_priv_, "map", map);
loadMap(map);
} else if (type == "gridboard") {
createGridBoard();
createGridBoard(nh_priv_);
} else {
NODELET_FATAL("unknown type: %s", type.c_str());
ros::shutdown();
Expand Down Expand Up @@ -331,23 +329,23 @@ class ArucoMap : public nodelet::Nodelet {
NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
}

void createGridBoard()
void createGridBoard(ros::NodeHandle& nh)
{
NODELET_INFO("generate gridboard");
NODELET_WARN("gridboard maps are deprecated");

int markers_x, markers_y, first_marker;
double markers_side, markers_sep_x, markers_sep_y;
std::vector<int> marker_ids;
nh_priv_.param<int>("markers_x", markers_x, 10);
nh_priv_.param<int>("markers_y", markers_y, 10);
nh_priv_.param<int>("first_marker", first_marker, 0);
markers_x = nh.param("markers_x", 10);
markers_y = nh.param("markers_y", 10);
first_marker = nh.param("first_marker", 0);

param(nh_priv_, "markers_side", markers_side);
param(nh_priv_, "markers_sep_x", markers_sep_x);
param(nh_priv_, "markers_sep_y", markers_sep_y);
param(nh, "markers_side", markers_side);
param(nh, "markers_sep_x", markers_sep_x);
param(nh, "markers_sep_y", markers_sep_y);

if (nh_priv_.getParam("marker_ids", marker_ids)) {
if (nh.getParam("marker_ids", marker_ids)) {
if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
NODELET_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
ros::shutdown();
Expand Down
4 changes: 1 addition & 3 deletions aruco_pose/src/utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,9 +35,7 @@ static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Ma
for (unsigned int i = 0; i < 3; ++i)
for (unsigned int j = 0; j < 3; ++j)
matrix.at<double>(i, j) = cinfo->K[3 * i + j];

for (unsigned int k = 0; k < cinfo->D.size(); k++)
dist.at<double>(k) = cinfo->D[k];
dist = cv::Mat(cinfo->D, true);
}

inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle)
Expand Down

0 comments on commit 9987960

Please sign in to comment.