Migrate to tf2 and add utilities

pal-robotics · Jan 21, 2020 · b91bb24 · b91bb24
1 parent f81fe0c
commit b91bb24
Show file tree

Hide file tree

Showing 2 changed files with 84 additions and 4 deletions.
diff --git a/aruco_ros/include/aruco_ros/aruco_ros_utils.h b/aruco_ros/include/aruco_ros/aruco_ros_utils.h
@@ -3,7 +3,11 @@
 
 #include <aruco/aruco.h>
 #include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/Image.h>
 #include <tf/transform_datatypes.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/transform_datatypes.h>
+#include <visualization_msgs/Marker.h>
 
 namespace aruco_ros
 {
@@ -18,8 +22,17 @@ namespace aruco_ros
 aruco::CameraParameters rosCameraInfo2ArucoCamParams(const sensor_msgs::CameraInfo& cam_info,
                                                      bool useRectifiedParameters);
 
-//FIXME: make parameter const as soon as the used function is also const
 tf::Transform arucoMarker2Tf(const aruco::Marker& marker);
+tf2::Transform arucoMarker2Tf2(const aruco::Marker& marker);
+
+std::vector<aruco::Marker> detectMarkers(const cv::Mat& img,
+                                         const aruco::CameraParameters& cam_params,
+                                         float marker_size,
+                                         aruco::MarkerDetector* detector = nullptr,
+                                         bool normalize_ilumination = false, bool correct_fisheye = false);
+
+visualization_msgs::Marker visMarkerFromPose(const geometry_msgs::PoseStamped& pose,
+                                             double marker_size, int marker_id = 1);
 
 }
 #endif // ARUCO_ROS_UTILS_H
diff --git a/aruco_ros/src/aruco_ros_utils.cpp b/aruco_ros/src/aruco_ros_utils.cpp
@@ -54,6 +54,18 @@ aruco::CameraParameters aruco_ros::rosCameraInfo2ArucoCamParams(const sensor_msg
 }
 
 tf::Transform aruco_ros::arucoMarker2Tf(const aruco::Marker &marker)
+{
+  tf2::Transform tf2_tf = arucoMarker2Tf2(marker);
+  return tf::Transform(
+        tf::Matrix3x3(tf::Quaternion(tf2_tf.getRotation().x(),
+                                     tf2_tf.getRotation().y(),
+                                     tf2_tf.getRotation().z(),
+                                     tf2_tf.getRotation().w())),
+                      tf::Vector3(tf2_tf.getOrigin().x(),
+                                  tf2_tf.getOrigin().y(),
+                                  tf2_tf.getOrigin().z()));
+}
+tf2::Transform aruco_ros::arucoMarker2Tf2(const aruco::Marker &marker)
 {
   cv::Mat rot(3, 3, CV_64FC1);
   cv::Mat Rvec64;
@@ -62,11 +74,66 @@ tf::Transform aruco_ros::arucoMarker2Tf(const aruco::Marker &marker)
   cv::Mat tran64;
   marker.Tvec.convertTo(tran64, CV_64FC1);
 
-  tf::Matrix3x3 tf_rot(rot.at<double>(0, 0), rot.at<double>(0, 1), rot.at<double>(0, 2), rot.at<double>(1, 0),
+  tf2::Matrix3x3 tf_rot(rot.at<double>(0, 0), rot.at<double>(0, 1), rot.at<double>(0, 2), rot.at<double>(1, 0),
                        rot.at<double>(1, 1), rot.at<double>(1, 2), rot.at<double>(2, 0), rot.at<double>(2, 1),
                        rot.at<double>(2, 2));
 
-  tf::Vector3 tf_orig(tran64.at<double>(0, 0), tran64.at<double>(1, 0), tran64.at<double>(2, 0));
+  tf2::Vector3 tf_orig(tran64.at<double>(0, 0), tran64.at<double>(1, 0), tran64.at<double>(2, 0));
+
+  return tf2::Transform(tf_rot, tf_orig);
+}
+
 
-  return tf::Transform(tf_rot, tf_orig);
+std::vector<aruco::Marker> aruco_ros::detectMarkers(const cv::Mat &img, const aruco::CameraParameters &cam_params, float marker_size, aruco::MarkerDetector *detector, bool normalize_ilumination, bool correct_fisheye)
+{
+  std::vector<aruco::Marker> markers;
+  try
+  {
+    if (normalize_ilumination)
+    {
+      ROS_WARN("normalizeImageIllumination is unimplemented!");
+      //        cv::Mat inImageNorm;
+      //        pal_vision_util::dctNormalization(inImage, inImageNorm,
+      //        dctComponentsToRemove); inImage = inImageNorm;
+    }
+
+    // detection results will go into "markers"
+    markers.clear();
+    // ok, let's detect 
+    if (detector)
+    {
+      detector->detect(img, markers, cam_params, marker_size, false, correct_fisheye);
+    }
+    else
+    {
+      aruco::MarkerDetector default_detector;
+      default_detector.detect(img, markers, cam_params, marker_size, false, correct_fisheye);
+    }
+    return markers;
+  }
+  catch (cv_bridge::Exception& e)
+  {
+    ROS_ERROR("cv_bridge exception: %s", e.what());
+    return markers;
+  }
+}
+
+
+visualization_msgs::Marker aruco_ros::visMarkerFromPose(const geometry_msgs::PoseStamped &pose, double marker_size, int marker_id)
+{
+  visualization_msgs::Marker visMarker;
+  visMarker.header = pose.header;
+  visMarker.id = marker_id;
+  visMarker.type = visualization_msgs::Marker::CUBE;
+  visMarker.action = visualization_msgs::Marker::ADD;
+  visMarker.pose = pose.pose;
+  visMarker.scale.x = marker_size;
+  visMarker.scale.y = marker_size;
+  visMarker.scale.z = 0.001;
+  visMarker.color.r = 1.0;
+  visMarker.color.g = 0;
+  visMarker.color.b = 0;
+  visMarker.color.a = 1.0;
+  visMarker.lifetime = ros::Duration(3.0);
+  return visMarker;
 }