pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

document the aruco_detector

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-28 15:17:49 +01:00
parent f200e2e61c
commit 9d12747e14
1 changed files with 18 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/aruco_detector.cpp
View File

@@ -12,6 +12,17 @@
#include <aruco/aruco.h> #include <aruco/aruco.h>
#include <teleoperation/utils.hpp> #include <teleoperation/utils.hpp>
/*
* This is the ROS Node for detecting the ArUco markers.
*
* Node: aruco_detector
*
* It subscribes to a webcam, detects the markers, transforms them
* into the correct coordinate frame (z-up) and publishes on TF
* relatively to the odom frame
*
*/
static const std::string ARUCO_WINDOW = "Aruco window"; static const std::string ARUCO_WINDOW = "Aruco window";
class ImageConverter { class ImageConverter {
@@ -41,6 +52,7 @@ class ImageConverter {
} }
void imageCb(const sensor_msgs::ImageConstPtr& msg) { void imageCb(const sensor_msgs::ImageConstPtr& msg) {
// This function gets called when an image arrives
cv_bridge::CvImageConstPtr cv_ptr; cv_bridge::CvImageConstPtr cv_ptr;
try try
{ {
@@ -54,7 +66,7 @@ class ImageConverter {
return; return;
} }
// Do Aruco // Define parameters of markers and camera
cv::Mat cam_mat = (cv::Mat_<double>(3, 3) << cv::Mat cam_mat = (cv::Mat_<double>(3, 3) <<
690., 0.0, 320, 690., 0.0, 320,
0.0, 690., 225, 0.0, 690., 225,
@@ -70,7 +82,7 @@ class ImageConverter {
float m_size = 0.15; float m_size = 0.15;
detector.detect(cv_ptr->image, markers, cam, m_size); detector.detect(cv_ptr->image, markers, cam, m_size);
// Draw and print aruco // Draw all aruco into the window and publish on tf
cv::Mat aruco_demo = cv_ptr->image.clone(); cv::Mat aruco_demo = cv_ptr->image.clone();
for (int i = 0; i < markers.size(); i++) { for (int i = 0; i < markers.size(); i++) {
@@ -79,6 +91,8 @@ class ImageConverter {
cv::Mat rot_cv; cv::Mat rot_cv;
cv::Rodrigues(markers[i].Rvec, rot_cv); cv::Rodrigues(markers[i].Rvec, rot_cv);
// Transform the coordinates of the aruco markers so that they
// are in the Z-up, X-forward frame
tf::Matrix3x3 rot( tf::Matrix3x3 rot(
rot_cv.at<float>(0, 0), rot_cv.at<float>(0, 0),
rot_cv.at<float>(0, 1), rot_cv.at<float>(0, 1),
@@ -106,7 +120,7 @@ class ImageConverter {
0, -1, 0 0, -1, 0
); );
rot = hmat * rot * another; // This all somehow works as expected rot = hmat * rot * another; // This transforms the coords
trans = hmat * trans; trans = hmat * trans;
tf::Transform aruco_tf(rot, trans); tf::Transform aruco_tf(rot, trans);
@@ -115,6 +129,7 @@ class ImageConverter {
id += stuff_to_str(markers[i].id); id += stuff_to_str(markers[i].id);
id += "_frame"; id += "_frame";
// Publish the transform
this->aruco_pub.sendTransform(tf::StampedTransform( this->aruco_pub.sendTransform(tf::StampedTransform(
aruco_tf, ros::Time::now(), "odom", aruco_tf, ros::Time::now(), "odom",
id.c_str())); id.c_str()));