├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── data └── geniusF100.ini ├── include └── aruco_mapping.h ├── launch ├── aruco_config.rviz └── aruco_mapping.launch ├── msg └── ArucoMarker.msg ├── package.xml └── src ├── aruco_mapping.cpp └── main.cpp /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package aruco_mapping 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.4 (2015-10-11) 6 | ------------------ 7 | * msg file addopted to coding style rules 8 | * Contributors: durovsky 9 | 10 | 1.0.3 (2015-10-10) 11 | ------------------ 12 | * updated package.xml 13 | * Contributors: durovsky 14 | 15 | 1.0.2 (2015-10-10) 16 | ------------------ 17 | * added visualization_msgs dependency 18 | * Contributors: durovsky 19 | 20 | 1.0.1 (2015-10-09) 21 | ------------------ 22 | * 1.0.0 23 | * CHANGELOG 24 | * fix 25 | * adding CHANGELOG 26 | * removed pal_vision_segmentation dependency 27 | * Contributors: durovsky 28 | 29 | 1.0.0 (2015-10-09) 30 | ------------------ 31 | * removed pal_vision_segmentation dependency 32 | * Contributors: durovsky 33 | 34 | 35 | 0.1.0 (2015-10-09) 36 | ------------------ 37 | * prerelease version 38 | * version 1.0.0 39 | * Contributors: Frantisek Durovsky, durovsky 40 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aruco_mapping) 3 | 4 | find_package(OpenCV REQUIRED) 5 | find_package(aruco REQUIRED) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | message_generation 10 | image_transport 11 | cv_bridge 12 | tf 13 | aruco 14 | visualization_msgs 15 | camera_calibration_parsers) 16 | 17 | include_directories(${catkin_INCLUDE_DIRS} 18 | ${PROJECT_SOURCE_DIR}/include/) 19 | 20 | include_directories(${PROJECT_SOURCE_DIR}/src/) 21 | 22 | 23 | SET(SOURCES ${PROJECT_SOURCE_DIR}/src/main.cpp 24 | ${PROJECT_SOURCE_DIR}/src/aruco_mapping.cpp) 25 | 26 | SET(HEADERS ${PROJECT_SOURCE_DIR}/include/aruco_mapping.h) 27 | 28 | add_message_files(FILES ArucoMarker.msg) 29 | 30 | generate_messages(DEPENDENCIES 31 | std_msgs 32 | geometry_msgs) 33 | 34 | catkin_package( 35 | INCLUDE_DIRS include 36 | LIBRARIES 37 | ) 38 | 39 | add_executable(${PROJECT_NAME} ${SOURCES} ${HEADERS}) 40 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} aruco_mapping_gencpp ) 41 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${aruco_LIBS} ${ROS_LIBRARIES} ${catkin_LIBRARIES}) 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Smart Robotic Systems 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # aruco_positioning_system 2 | 3 | [ROS](http://ros.org) package 4 | 5 | Aruco mapping package, see [Wiki](http://wiki.ros.org/aruco_mapping) page for more details: 6 | 7 | * Authors: [Jan Bacik] (http://www.smartroboticsys.eu/?page_id=895&lang=en), [Smart Robotic Systems] (http://www.smartroboticsys.eu) 8 | 9 | Aruco mapping 12 | -------------------------------------------------------------------------------- /data/geniusF100.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | [image] 4 | 5 | width 6 | 640 7 | 8 | height 9 | 480 10 | 11 | [camera] 12 | 13 | camera matrix 14 | 316.172746 0 324.360871 15 | 0 315.746678 241.691715 16 | 0.000000 0.000000 1.000000 17 | 18 | distortion 19 | 0.003758 -0.026423 0.002011 -0.001394 0 20 | 21 | rectification 22 | 1.000000 0.000000 0.000000 23 | 0.000000 1.000000 0.000000 24 | 0.000000 0.000000 1.000000 25 | 26 | projection 27 | 307.211409 0 322.820704 0 28 | 0 312.895665 242.394914 0 29 | 0 0 1 0 -------------------------------------------------------------------------------- /include/aruco_mapping.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************//** 2 | * @file aruco_mapping.h 3 | * 4 | * Copyright (c) 5 | * Smart Robotic Systems 6 | * March 2015 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | ******************************************************************************/ 31 | 32 | /* Author: Jan Bacik */ 33 | 34 | #ifndef ARUCO_MAPPING_H 35 | #define ARUCO_MAPPING_H 36 | 37 | // Standard ROS libraries 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | // Aruco libraries 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | // OpenCV libraries 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | // Custom message 60 | #include 61 | 62 | /** \brief Aruco mapping namespace */ 63 | namespace aruco_mapping 64 | { 65 | 66 | /** \brief Client class for Aruco mapping */ 67 | class ArucoMapping 68 | { 69 | public: 70 | 71 | /** \brief Struct to keep marker information */ 72 | struct MarkerInfo 73 | { 74 | 75 | bool visible; // Marker visibile in actual image? 76 | int marker_id; // Marker ID 77 | int previous_marker_id; // Used for chaining markers 78 | geometry_msgs::Pose geometry_msg_to_previous; // Position with respect to previous marker 79 | geometry_msgs::Pose geometry_msg_to_world; // Position with respect to world's origin 80 | tf::StampedTransform tf_to_previous; // TF with respect to previous marker 81 | tf::StampedTransform tf_to_world; // TF with respect to world's origin 82 | geometry_msgs::Pose current_camera_pose; // Position of camera with respect to the marker 83 | tf::Transform current_camera_tf; // TF of camera with respect to the marker 84 | }; 85 | 86 | public: 87 | 88 | /** \brief Construct a client for EZN64 USB control*/ 89 | ArucoMapping(ros::NodeHandle *nh); 90 | 91 | ~ArucoMapping(); 92 | 93 | /** \brief Callback function to handle image processing*/ 94 | void imageCallback(const sensor_msgs::ImageConstPtr &original_image); 95 | 96 | private: 97 | 98 | /** \brief Function to parse data from calibration file*/ 99 | bool parseCalibrationFile(std::string filename); 100 | 101 | /** \brief Function to publish all known TFs*/ 102 | void publishTfs(bool world_option); 103 | 104 | /** \brief Function to publish all known markers for visualization purposes*/ 105 | void publishMarker(geometry_msgs::Pose markerPose, int MarkerID, int rank); 106 | 107 | /** \brief Publisher of visualization_msgs::Marker message to "aruco_markers" topic*/ 108 | ros::Publisher marker_visualization_pub_; 109 | 110 | /** \brief Publisher of aruco_mapping::ArucoMarker custom message*/ 111 | ros::Publisher marker_msg_pub_; 112 | 113 | /** \brief Compute TF from marker detector result*/ 114 | tf::Transform arucoMarker2Tf(const aruco::Marker &marker); 115 | 116 | /** \brief Process actual image, detect markers and compute poses */ 117 | bool processImage(cv::Mat input_image,cv::Mat output_image); 118 | 119 | //Launch file params 120 | std::string calib_filename_; 121 | std::string space_type_; 122 | float marker_size_; 123 | int num_of_markers_; 124 | bool roi_allowed_; 125 | int roi_x_; 126 | int roi_y_; 127 | int roi_w_; 128 | int roi_h_; 129 | 130 | /** \brief Container holding MarkerInfo data about all detected markers */ 131 | std::vector markers_; 132 | 133 | /** \brief Actual TF of camera with respect to world's origin */ 134 | tf::StampedTransform world_position_transform_; 135 | 136 | /** \brief Actual Pose of camera with respect to world's origin */ 137 | geometry_msgs::Pose world_position_geometry_msg_; 138 | 139 | aruco::CameraParameters aruco_calib_params_; 140 | 141 | int marker_counter_; 142 | int marker_counter_previous_; 143 | int closest_camera_index_; 144 | int lowest_marker_id_; 145 | bool first_marker_detected_; 146 | 147 | tf::TransformListener *listener_; 148 | tf::TransformBroadcaster broadcaster_; 149 | 150 | //Consts 151 | static const int CV_WAIT_KEY = 10; 152 | static const int CV_WINDOW_MARKER_LINE_WIDTH = 2; 153 | 154 | static const double WAIT_FOR_TRANSFORM_INTERVAL = 2.0; 155 | static const double BROADCAST_WAIT_INTERVAL = 0.0001; 156 | static const double INIT_MIN_SIZE_VALUE = 1000000; 157 | 158 | static const double RVIZ_MARKER_HEIGHT = 0.01; 159 | static const double RVIZ_MARKER_LIFETIME = 0.2; 160 | static const double RVIZ_MARKER_COLOR_R = 1.0; 161 | static const double RVIZ_MARKER_COLOR_G = 1.0; 162 | static const double RVIZ_MARKER_COLOR_B = 1.0; 163 | static const double RVIZ_MARKER_COLOR_A = 1.0; 164 | 165 | static const double THIS_IS_FIRST_MARKER = -2; 166 | 167 | }; //ArucoMapping class 168 | } //aruco_mapping namespace 169 | 170 | #endif //ARUCO_MAPPING_H 171 | -------------------------------------------------------------------------------- /launch/aruco_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /Marker1 11 | Splitter Ratio: 0.510067 12 | Tree Height: 565 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 0.2 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.03 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 100 51 | Reference Frame: 52 | Value: true 53 | - Class: rviz/Marker 54 | Enabled: true 55 | Marker Topic: /aruco_markers 56 | Name: Marker 57 | Namespaces: 58 | basic_shapes: true 59 | Queue Size: 100 60 | Value: true 61 | Enabled: true 62 | Global Options: 63 | Background Color: 0; 0; 0 64 | Fixed Frame: world 65 | Frame Rate: 30 66 | Name: root 67 | Tools: 68 | - Class: rviz/Interact 69 | Hide Inactive Objects: true 70 | - Class: rviz/MoveCamera 71 | - Class: rviz/Select 72 | - Class: rviz/FocusCamera 73 | - Class: rviz/Measure 74 | - Class: rviz/SetInitialPose 75 | Topic: /initialpose 76 | - Class: rviz/SetGoal 77 | Topic: /move_base_simple/goal 78 | - Class: rviz/PublishPoint 79 | Single click: true 80 | Topic: /clicked_point 81 | Value: true 82 | Views: 83 | Current: 84 | Class: rviz/Orbit 85 | Distance: 2.8851 86 | Enable Stereo Rendering: 87 | Stereo Eye Separation: 0.06 88 | Stereo Focal Distance: 1 89 | Swap Stereo Eyes: false 90 | Value: false 91 | Focal Point: 92 | X: -0.132655 93 | Y: 0.160501 94 | Z: 0.38816 95 | Name: Current View 96 | Near Clip Distance: 0.01 97 | Pitch: 0.820201 98 | Target Frame: 99 | Value: Orbit (rviz) 100 | Yaw: 3.98221 101 | Saved: ~ 102 | Window Geometry: 103 | Displays: 104 | collapsed: false 105 | Height: 846 106 | Hide Left Dock: false 107 | Hide Right Dock: true 108 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000036e000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 109 | Selection: 110 | collapsed: false 111 | Time: 112 | collapsed: false 113 | Tool Properties: 114 | collapsed: false 115 | Views: 116 | collapsed: true 117 | Width: 1200 118 | X: 55 119 | Y: 30 120 | -------------------------------------------------------------------------------- /launch/aruco_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | --> 26 | --> 27 | --> 28 | --> 29 | --> 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /msg/ArucoMarker.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | bool marker_visibile 3 | int32 num_of_visible_markers 4 | geometry_msgs/Pose global_camera_pose 5 | int32[] marker_ids 6 | geometry_msgs/Pose[] global_marker_poses 7 | 8 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aruco_mapping 4 | 1.0.4 5 | 6 | This package allows user to create a map of Aruco markers in 2D or 3D space 7 | and estimate full 6 DOF pose of the camera. 8 | 9 | 10 | Frantisek Durovsky 11 | BSD 12 | http://www.smartroboticsys.eu 13 | Jan Bacik 14 | 15 | catkin 16 | roscpp 17 | message_generation 18 | image_transport 19 | cv_bridge 20 | tf 21 | aruco 22 | visualization_msgs 23 | camera_calibration_parsers 24 | 25 | roscpp 26 | image_transport 27 | cv_bridge 28 | tf 29 | aruco 30 | visualization_msgs 31 | camera_calibration_parsers 32 | 33 | 34 | -------------------------------------------------------------------------------- /src/aruco_mapping.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************//** 2 | * @file aruco_mapping.cpp 3 | * 4 | * Copyright (c) 5 | * Smart Robotic Systems 6 | * March 2015 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | ******************************************************************************/ 31 | 32 | /* Author: Jan Bacik */ 33 | 34 | #ifndef ARUCO_MAPPING_CPP 35 | #define ARUCO_MAPPING_CPP 36 | 37 | #include 38 | 39 | namespace aruco_mapping 40 | { 41 | 42 | ArucoMapping::ArucoMapping(ros::NodeHandle *nh) : 43 | listener_ (new tf::TransformListener), // Initialize TF Listener 44 | num_of_markers_ (10), // Number of used markers 45 | marker_size_(0.1), // Marker size in m 46 | calib_filename_("empty"), // Calibration filepath 47 | space_type_ ("plane"), // Space type - 2D plane 48 | roi_allowed_ (false), // ROI not allowed by default 49 | first_marker_detected_(false), // First marker not detected by defualt 50 | lowest_marker_id_(-1), // Lowest marker ID 51 | marker_counter_(0), // Reset marker counter 52 | closest_camera_index_(0) // Reset closest camera index 53 | 54 | { 55 | double temp_marker_size; 56 | 57 | //Parse params from launch file 58 | nh->getParam("/aruco_mapping/calibration_file", calib_filename_); 59 | nh->getParam("/aruco_mapping/marker_size", temp_marker_size); 60 | nh->getParam("/aruco_mapping/num_of_markers", num_of_markers_); 61 | nh->getParam("/aruco_maping/space_type",space_type_); 62 | nh->getParam("/aruco_mapping/roi_allowed",roi_allowed_); 63 | nh->getParam("/aruco_mapping/roi_x",roi_x_); 64 | nh->getParam("/aruco_mapping/roi_y",roi_y_); 65 | nh->getParam("/aruco_mapping/roi_w",roi_w_); 66 | nh->getParam("/aruco_mapping/roi_h",roi_h_); 67 | 68 | // Double to float conversion 69 | marker_size_ = float(temp_marker_size); 70 | 71 | if(calib_filename_ == "empty") 72 | ROS_WARN("Calibration filename empty! Check the launch file paths"); 73 | else 74 | { 75 | ROS_INFO_STREAM("Calibration file path: " << calib_filename_ ); 76 | ROS_INFO_STREAM("Number of markers: " << num_of_markers_); 77 | ROS_INFO_STREAM("Marker Size: " << marker_size_); 78 | ROS_INFO_STREAM("Type of space: " << space_type_); 79 | ROS_INFO_STREAM("ROI allowed: " << roi_allowed_); 80 | ROS_INFO_STREAM("ROI x-coor: " << roi_x_); 81 | ROS_INFO_STREAM("ROI y-coor: " << roi_x_); 82 | ROS_INFO_STREAM("ROI width: " << roi_w_); 83 | ROS_INFO_STREAM("ROI height: " << roi_h_); 84 | } 85 | 86 | //ROS publishers 87 | marker_msg_pub_ = nh->advertise("aruco_poses",1); 88 | marker_visualization_pub_ = nh->advertise("aruco_markers",1); 89 | 90 | //Parse data from calibration file 91 | parseCalibrationFile(calib_filename_); 92 | 93 | //Initialize OpenCV window 94 | cv::namedWindow("Mono8", CV_WINDOW_AUTOSIZE); 95 | 96 | //Resize marker container 97 | markers_.resize(num_of_markers_); 98 | 99 | // Default markers_ initialization 100 | for(size_t i = 0; i < num_of_markers_;i++) 101 | { 102 | markers_[i].previous_marker_id = -1; 103 | markers_[i].visible = false; 104 | markers_[i].marker_id = -1; 105 | } 106 | } 107 | 108 | ArucoMapping::~ArucoMapping() 109 | { 110 | delete listener_; 111 | } 112 | 113 | bool 114 | ArucoMapping::parseCalibrationFile(std::string calib_filename) 115 | { 116 | sensor_msgs::CameraInfo camera_calibration_data; 117 | std::string camera_name = "camera"; 118 | 119 | camera_calibration_parsers::readCalibrationIni(calib_filename, camera_name, camera_calibration_data); 120 | 121 | // Alocation of memory for calibration data 122 | cv::Mat *intrinsics = new(cv::Mat)(3, 3, CV_64F); 123 | cv::Mat *distortion_coeff = new(cv::Mat)(5, 1, CV_64F); 124 | cv::Size *image_size = new(cv::Size); 125 | 126 | image_size->width = camera_calibration_data.width; 127 | image_size->height = camera_calibration_data.height; 128 | 129 | for(size_t i = 0; i < 3; i++) 130 | for(size_t j = 0; j < 3; j++) 131 | intrinsics->at(i,j) = camera_calibration_data.K.at(3*i+j); 132 | 133 | for(size_t i = 0; i < 5; i++) 134 | distortion_coeff->at(i,0) = camera_calibration_data.D.at(i); 135 | 136 | ROS_DEBUG_STREAM("Image width: " << image_size->width); 137 | ROS_DEBUG_STREAM("Image height: " << image_size->height); 138 | ROS_DEBUG_STREAM("Intrinsics:" << std::endl << *intrinsics); 139 | ROS_DEBUG_STREAM("Distortion: " << *distortion_coeff); 140 | 141 | 142 | //Load parameters to aruco_calib_param_ for aruco detection 143 | aruco_calib_params_.setParams(*intrinsics, *distortion_coeff, *image_size); 144 | 145 | //Simple check if calibration data meets expected values 146 | if ((intrinsics->at(2,2) == 1) && (distortion_coeff->at(0,4) == 0)) 147 | { 148 | ROS_INFO_STREAM("Calibration data loaded successfully"); 149 | return true; 150 | } 151 | else 152 | { 153 | ROS_WARN("Wrong calibration data, check calibration file and filepath"); 154 | return false; 155 | } 156 | } 157 | 158 | void 159 | ArucoMapping::imageCallback(const sensor_msgs::ImageConstPtr &original_image) 160 | { 161 | //Create cv_brigde instance 162 | cv_bridge::CvImagePtr cv_ptr; 163 | try 164 | { 165 | cv_ptr=cv_bridge::toCvCopy(original_image, sensor_msgs::image_encodings::MONO8); 166 | } 167 | catch (cv_bridge::Exception& e) 168 | { 169 | ROS_ERROR("Not able to convert sensor_msgs::Image to OpenCV::Mat format %s", e.what()); 170 | return; 171 | } 172 | 173 | // sensor_msgs::Image to OpenCV Mat structure 174 | cv::Mat I = cv_ptr->image; 175 | 176 | // region of interest 177 | if(roi_allowed_==true) 178 | I = cv_ptr->image(cv::Rect(roi_x_,roi_y_,roi_w_,roi_h_)); 179 | 180 | //Marker detection 181 | processImage(I,I); 182 | 183 | // Show image 184 | cv::imshow("Mono8", I); 185 | cv::waitKey(10); 186 | } 187 | 188 | 189 | bool 190 | ArucoMapping::processImage(cv::Mat input_image,cv::Mat output_image) 191 | { 192 | aruco::MarkerDetector Detector; 193 | std::vector temp_markers; 194 | 195 | //Set visibility flag to false for all markers 196 | for(size_t i = 0; i < num_of_markers_; i++) 197 | markers_[i].visible = false; 198 | 199 | // Save previous marker count 200 | marker_counter_previous_ = marker_counter_; 201 | 202 | // Detect markers 203 | Detector.detect(input_image,temp_markers,aruco_calib_params_,marker_size_); 204 | 205 | // If no marker found, print statement 206 | if(temp_markers.size() == 0) 207 | ROS_DEBUG("No marker found!"); 208 | 209 | //------------------------------------------------------ 210 | // FIRST MARKER DETECTED 211 | //------------------------------------------------------ 212 | if((temp_markers.size() > 0) && (first_marker_detected_ == false)) 213 | { 214 | //Set flag 215 | first_marker_detected_=true; 216 | 217 | // Detect lowest marker ID 218 | lowest_marker_id_ = temp_markers[0].id; 219 | for(size_t i = 0; i < temp_markers.size();i++) 220 | { 221 | if(temp_markers[i].id < lowest_marker_id_) 222 | lowest_marker_id_ = temp_markers[i].id; 223 | } 224 | 225 | 226 | ROS_DEBUG_STREAM("The lowest Id marker " << lowest_marker_id_ ); 227 | 228 | // Identify lowest marker ID with world's origin 229 | markers_[0].marker_id = lowest_marker_id_; 230 | 231 | markers_[0].geometry_msg_to_world.position.x = 0; 232 | markers_[0].geometry_msg_to_world.position.y = 0; 233 | markers_[0].geometry_msg_to_world.position.z = 0; 234 | 235 | markers_[0].geometry_msg_to_world.orientation.x = 0; 236 | markers_[0].geometry_msg_to_world.orientation.y = 0; 237 | markers_[0].geometry_msg_to_world.orientation.z = 0; 238 | markers_[0].geometry_msg_to_world.orientation.w = 1; 239 | 240 | // Relative position and Global position 241 | markers_[0].geometry_msg_to_previous.position.x = 0; 242 | markers_[0].geometry_msg_to_previous.position.y = 0; 243 | markers_[0].geometry_msg_to_previous.position.z = 0; 244 | 245 | markers_[0].geometry_msg_to_previous.orientation.x = 0; 246 | markers_[0].geometry_msg_to_previous.orientation.y = 0; 247 | markers_[0].geometry_msg_to_previous.orientation.z = 0; 248 | markers_[0].geometry_msg_to_previous.orientation.w = 1; 249 | 250 | // Transformation Pose to TF 251 | tf::Vector3 position; 252 | position.setX(0); 253 | position.setY(0); 254 | position.setZ(0); 255 | 256 | tf::Quaternion rotation; 257 | rotation.setX(0); 258 | rotation.setY(0); 259 | rotation.setZ(0); 260 | rotation.setW(1); 261 | 262 | markers_[0].tf_to_previous.setOrigin(position); 263 | markers_[0].tf_to_previous.setRotation(rotation); 264 | 265 | // Relative position of first marker equals Global position 266 | markers_[0].tf_to_world=markers_[0].tf_to_previous; 267 | 268 | // Increase count 269 | marker_counter_++; 270 | 271 | // Set sign of visibility of first marker 272 | markers_[0].visible=true; 273 | 274 | ROS_INFO_STREAM("First marker with ID: " << markers_[0].marker_id << " detected"); 275 | 276 | //First marker does not have any previous marker 277 | markers_[0].previous_marker_id = THIS_IS_FIRST_MARKER; 278 | } 279 | 280 | //------------------------------------------------------ 281 | // FOR EVERY MARKER DO 282 | //------------------------------------------------------ 283 | for(size_t i = 0; i < temp_markers.size();i++) 284 | { 285 | int index; 286 | int current_marker_id = temp_markers[i].id; 287 | 288 | //Draw marker convex, ID, cube and axis 289 | temp_markers[i].draw(output_image, cv::Scalar(0,0,255),2); 290 | aruco::CvDrawingUtils::draw3dCube(output_image,temp_markers[i], aruco_calib_params_); 291 | aruco::CvDrawingUtils::draw3dAxis(output_image,temp_markers[i], aruco_calib_params_); 292 | 293 | // Existing marker ? 294 | bool existing = false; 295 | int temp_counter = 0; 296 | 297 | while((existing == false) && (temp_counter < marker_counter_)) 298 | { 299 | if(markers_[temp_counter].marker_id == current_marker_id) 300 | { 301 | index = temp_counter; 302 | existing = true; 303 | ROS_DEBUG_STREAM("Existing marker with ID: " << current_marker_id << "found"); 304 | } 305 | temp_counter++; 306 | } 307 | 308 | //New marker ? 309 | if(existing == false) 310 | { 311 | index = marker_counter_; 312 | markers_[index].marker_id = current_marker_id; 313 | existing = true; 314 | ROS_DEBUG_STREAM("New marker with ID: " << current_marker_id << " found"); 315 | } 316 | 317 | // Change visibility flag of new marker 318 | for(size_t j = 0;j < marker_counter_; j++) 319 | { 320 | for(size_t k = 0;k < temp_markers.size(); k++) 321 | { 322 | if(markers_[j].marker_id == temp_markers[k].id) 323 | markers_[j].visible = true; 324 | } 325 | } 326 | 327 | //------------------------------------------------------ 328 | // For existing marker do 329 | //------------------------------------------------------ 330 | if((index < marker_counter_) && (first_marker_detected_ == true)) 331 | { 332 | markers_[index].current_camera_tf = arucoMarker2Tf(temp_markers[i]); 333 | markers_[index].current_camera_tf = markers_[index].current_camera_tf.inverse(); 334 | 335 | const tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin(); 336 | markers_[index].current_camera_pose.position.x = marker_origin.getX(); 337 | markers_[index].current_camera_pose.position.y = marker_origin.getY(); 338 | markers_[index].current_camera_pose.position.z = marker_origin.getZ(); 339 | 340 | const tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation(); 341 | markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX(); 342 | markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY(); 343 | markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ(); 344 | markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW(); 345 | } 346 | 347 | //------------------------------------------------------ 348 | // For new marker do 349 | //------------------------------------------------------ 350 | if((index == marker_counter_) && (first_marker_detected_ == true)) 351 | { 352 | markers_[index].current_camera_tf=arucoMarker2Tf(temp_markers[i]); 353 | 354 | tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin(); 355 | markers_[index].current_camera_pose.position.x = marker_origin.getX(); 356 | markers_[index].current_camera_pose.position.y = marker_origin.getY(); 357 | markers_[index].current_camera_pose.position.z = marker_origin.getZ(); 358 | 359 | tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation(); 360 | markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX(); 361 | markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY(); 362 | markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ(); 363 | markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW(); 364 | 365 | // Naming - TFs 366 | std::stringstream camera_tf_id; 367 | std::stringstream camera_tf_id_old; 368 | std::stringstream marker_tf_id_old; 369 | 370 | camera_tf_id << "camera_" << index; 371 | 372 | // Flag to keep info if any_known marker_visible in actual image 373 | bool any_known_marker_visible = false; 374 | 375 | // Array ID of markers, which position of new marker is calculated 376 | int last_marker_id; 377 | 378 | // Testing, if is possible calculate position of a new marker to old known marker 379 | for(int k = 0; k < index; k++) 380 | { 381 | if((markers_[k].visible == true) && (any_known_marker_visible == false)) 382 | { 383 | if(markers_[k].previous_marker_id != -1) 384 | { 385 | any_known_marker_visible = true; 386 | camera_tf_id_old << "camera_" << k; 387 | marker_tf_id_old << "marker_" << k; 388 | markers_[index].previous_marker_id = k; 389 | last_marker_id = k; 390 | } 391 | } 392 | } 393 | 394 | // New position can be calculated 395 | if(any_known_marker_visible == true) 396 | { 397 | // Generating TFs for listener 398 | for(char k = 0; k < 10; k++) 399 | { 400 | // TF from old marker and its camera 401 | broadcaster_.sendTransform(tf::StampedTransform(markers_[last_marker_id].current_camera_tf,ros::Time::now(), 402 | marker_tf_id_old.str(),camera_tf_id_old.str())); 403 | 404 | // TF from old camera to new camera 405 | broadcaster_.sendTransform(tf::StampedTransform(markers_[index].current_camera_tf,ros::Time::now(), 406 | camera_tf_id_old.str(),camera_tf_id.str())); 407 | 408 | ros::Duration(BROADCAST_WAIT_INTERVAL).sleep(); 409 | } 410 | 411 | // Calculate TF between two markers 412 | listener_->waitForTransform(marker_tf_id_old.str(),camera_tf_id.str(),ros::Time(0), 413 | ros::Duration(WAIT_FOR_TRANSFORM_INTERVAL)); 414 | try 415 | { 416 | broadcaster_.sendTransform(tf::StampedTransform(markers_[last_marker_id].current_camera_tf,ros::Time::now(), 417 | marker_tf_id_old.str(),camera_tf_id_old.str())); 418 | 419 | broadcaster_.sendTransform(tf::StampedTransform(markers_[index].current_camera_tf,ros::Time::now(), 420 | camera_tf_id_old.str(),camera_tf_id.str())); 421 | 422 | listener_->lookupTransform(marker_tf_id_old.str(),camera_tf_id.str(),ros::Time(0), 423 | markers_[index].tf_to_previous); 424 | } 425 | catch(tf::TransformException &e) 426 | { 427 | ROS_ERROR("Not able to lookup transform"); 428 | } 429 | 430 | // Save origin and quaternion of calculated TF 431 | marker_origin = markers_[index].tf_to_previous.getOrigin(); 432 | marker_quaternion = markers_[index].tf_to_previous.getRotation(); 433 | 434 | // If plane type selected roll, pitch and Z axis are zero 435 | if(space_type_ == "plane") 436 | { 437 | double roll, pitch, yaw; 438 | tf::Matrix3x3(marker_quaternion).getRPY(roll,pitch,yaw); 439 | roll = 0; 440 | pitch = 0; 441 | marker_origin.setZ(0); 442 | marker_quaternion.setRPY(pitch,roll,yaw); 443 | } 444 | 445 | markers_[index].tf_to_previous.setRotation(marker_quaternion); 446 | markers_[index].tf_to_previous.setOrigin(marker_origin); 447 | 448 | marker_origin = markers_[index].tf_to_previous.getOrigin(); 449 | markers_[index].geometry_msg_to_previous.position.x = marker_origin.getX(); 450 | markers_[index].geometry_msg_to_previous.position.y = marker_origin.getY(); 451 | markers_[index].geometry_msg_to_previous.position.z = marker_origin.getZ(); 452 | 453 | marker_quaternion = markers_[index].tf_to_previous.getRotation(); 454 | markers_[index].geometry_msg_to_previous.orientation.x = marker_quaternion.getX(); 455 | markers_[index].geometry_msg_to_previous.orientation.y = marker_quaternion.getY(); 456 | markers_[index].geometry_msg_to_previous.orientation.z = marker_quaternion.getZ(); 457 | markers_[index].geometry_msg_to_previous.orientation.w = marker_quaternion.getW(); 458 | 459 | // increase marker count 460 | marker_counter_++; 461 | 462 | // Invert and position of new marker to compute camera pose above it 463 | markers_[index].current_camera_tf = markers_[index].current_camera_tf.inverse(); 464 | 465 | marker_origin = markers_[index].current_camera_tf.getOrigin(); 466 | markers_[index].current_camera_pose.position.x = marker_origin.getX(); 467 | markers_[index].current_camera_pose.position.y = marker_origin.getY(); 468 | markers_[index].current_camera_pose.position.z = marker_origin.getZ(); 469 | 470 | marker_quaternion = markers_[index].current_camera_tf.getRotation(); 471 | markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX(); 472 | markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY(); 473 | markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ(); 474 | markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW(); 475 | 476 | // Publish all TFs and markers 477 | publishTfs(false); 478 | } 479 | } 480 | 481 | //------------------------------------------------------ 482 | // Compute global position of new marker 483 | //------------------------------------------------------ 484 | if((marker_counter_previous_ < marker_counter_) && (first_marker_detected_ == true)) 485 | { 486 | // Publish all TF five times for listener 487 | for(char k = 0; k < 5; k++) 488 | publishTfs(false); 489 | 490 | std::stringstream marker_tf_name; 491 | marker_tf_name << "marker_" << index; 492 | 493 | listener_->waitForTransform("world",marker_tf_name.str(),ros::Time(0), 494 | ros::Duration(WAIT_FOR_TRANSFORM_INTERVAL)); 495 | try 496 | { 497 | listener_->lookupTransform("world",marker_tf_name.str(),ros::Time(0), 498 | markers_[index].tf_to_world); 499 | } 500 | catch(tf::TransformException &e) 501 | { 502 | ROS_ERROR("Not able to lookup transform"); 503 | } 504 | 505 | // Saving TF to Pose 506 | const tf::Vector3 marker_origin = markers_[index].tf_to_world.getOrigin(); 507 | markers_[index].geometry_msg_to_world.position.x = marker_origin.getX(); 508 | markers_[index].geometry_msg_to_world.position.y = marker_origin.getY(); 509 | markers_[index].geometry_msg_to_world.position.z = marker_origin.getZ(); 510 | 511 | tf::Quaternion marker_quaternion=markers_[index].tf_to_world.getRotation(); 512 | markers_[index].geometry_msg_to_world.orientation.x = marker_quaternion.getX(); 513 | markers_[index].geometry_msg_to_world.orientation.y = marker_quaternion.getY(); 514 | markers_[index].geometry_msg_to_world.orientation.z = marker_quaternion.getZ(); 515 | markers_[index].geometry_msg_to_world.orientation.w = marker_quaternion.getW(); 516 | } 517 | } 518 | 519 | //------------------------------------------------------ 520 | // Compute which of visible markers is the closest to the camera 521 | //------------------------------------------------------ 522 | bool any_markers_visible=false; 523 | int num_of_visible_markers=0; 524 | 525 | if(first_marker_detected_ == true) 526 | { 527 | double minimal_distance = INIT_MIN_SIZE_VALUE; 528 | for(int k = 0; k < num_of_markers_; k++) 529 | { 530 | double a,b,c,size; 531 | 532 | // If marker is visible, distance is calculated 533 | if(markers_[k].visible==true) 534 | { 535 | a = markers_[k].current_camera_pose.position.x; 536 | b = markers_[k].current_camera_pose.position.y; 537 | c = markers_[k].current_camera_pose.position.z; 538 | size = std::sqrt((a * a) + (b * b) + (c * c)); 539 | if(size < minimal_distance) 540 | { 541 | minimal_distance = size; 542 | closest_camera_index_ = k; 543 | } 544 | 545 | any_markers_visible = true; 546 | num_of_visible_markers++; 547 | } 548 | } 549 | } 550 | 551 | //------------------------------------------------------ 552 | // Publish all known markers 553 | //------------------------------------------------------ 554 | if(first_marker_detected_ == true) 555 | publishTfs(true); 556 | 557 | //------------------------------------------------------ 558 | // Compute global camera pose 559 | //------------------------------------------------------ 560 | if((first_marker_detected_ == true) && (any_markers_visible == true)) 561 | { 562 | std::stringstream closest_camera_tf_name; 563 | closest_camera_tf_name << "camera_" << closest_camera_index_; 564 | 565 | listener_->waitForTransform("world",closest_camera_tf_name.str(),ros::Time(0), 566 | ros::Duration(WAIT_FOR_TRANSFORM_INTERVAL)); 567 | try 568 | { 569 | listener_->lookupTransform("world",closest_camera_tf_name.str(),ros::Time(0), 570 | world_position_transform_); 571 | } 572 | catch(tf::TransformException &ex) 573 | { 574 | ROS_ERROR("Not able to lookup transform"); 575 | } 576 | 577 | // Saving TF to Pose 578 | const tf::Vector3 marker_origin = world_position_transform_.getOrigin(); 579 | world_position_geometry_msg_.position.x = marker_origin.getX(); 580 | world_position_geometry_msg_.position.y = marker_origin.getY(); 581 | world_position_geometry_msg_.position.z = marker_origin.getZ(); 582 | 583 | tf::Quaternion marker_quaternion = world_position_transform_.getRotation(); 584 | world_position_geometry_msg_.orientation.x = marker_quaternion.getX(); 585 | world_position_geometry_msg_.orientation.y = marker_quaternion.getY(); 586 | world_position_geometry_msg_.orientation.z = marker_quaternion.getZ(); 587 | world_position_geometry_msg_.orientation.w = marker_quaternion.getW(); 588 | } 589 | 590 | //------------------------------------------------------ 591 | // Publish all known markers 592 | //------------------------------------------------------ 593 | if(first_marker_detected_ == true) 594 | publishTfs(true); 595 | 596 | //------------------------------------------------------ 597 | // Publish custom marker message 598 | //------------------------------------------------------ 599 | aruco_mapping::ArucoMarker marker_msg; 600 | 601 | if((any_markers_visible == true)) 602 | { 603 | marker_msg.header.stamp = ros::Time::now(); 604 | marker_msg.header.frame_id = "world"; 605 | marker_msg.marker_visibile = true; 606 | marker_msg.num_of_visible_markers = num_of_visible_markers; 607 | marker_msg.global_camera_pose = world_position_geometry_msg_; 608 | marker_msg.marker_ids.clear(); 609 | marker_msg.global_marker_poses.clear(); 610 | for(size_t j = 0; j < marker_counter_; j++) 611 | { 612 | if(markers_[j].visible == true) 613 | { 614 | marker_msg.marker_ids.push_back(markers_[j].marker_id); 615 | marker_msg.global_marker_poses.push_back(markers_[j].geometry_msg_to_world); 616 | } 617 | } 618 | } 619 | else 620 | { 621 | marker_msg.header.stamp = ros::Time::now(); 622 | marker_msg.header.frame_id = "world"; 623 | marker_msg.num_of_visible_markers = num_of_visible_markers; 624 | marker_msg.marker_visibile = false; 625 | marker_msg.marker_ids.clear(); 626 | marker_msg.global_marker_poses.clear(); 627 | } 628 | 629 | // Publish custom marker msg 630 | marker_msg_pub_.publish(marker_msg); 631 | 632 | return true; 633 | } 634 | 635 | //////////////////////////////////////////////////////////////////////////////////////////////// 636 | 637 | void 638 | ArucoMapping::publishTfs(bool world_option) 639 | { 640 | for(int i = 0; i < marker_counter_; i++) 641 | { 642 | // Actual Marker 643 | std::stringstream marker_tf_id; 644 | marker_tf_id << "marker_" << i; 645 | // Older marker - or World 646 | std::stringstream marker_tf_id_old; 647 | if(i == 0) 648 | marker_tf_id_old << "world"; 649 | else 650 | marker_tf_id_old << "marker_" << markers_[i].previous_marker_id; 651 | broadcaster_.sendTransform(tf::StampedTransform(markers_[i].tf_to_previous,ros::Time::now(),marker_tf_id_old.str(),marker_tf_id.str())); 652 | 653 | // Position of camera to its marker 654 | std::stringstream camera_tf_id; 655 | camera_tf_id << "camera_" << i; 656 | broadcaster_.sendTransform(tf::StampedTransform(markers_[i].current_camera_tf,ros::Time::now(),marker_tf_id.str(),camera_tf_id.str())); 657 | 658 | if(world_option == true) 659 | { 660 | // Global position of marker TF 661 | std::stringstream marker_globe; 662 | marker_globe << "marker_globe_" << i; 663 | broadcaster_.sendTransform(tf::StampedTransform(markers_[i].tf_to_world,ros::Time::now(),"world",marker_globe.str())); 664 | } 665 | 666 | // Cubes for RVIZ - markers 667 | publishMarker(markers_[i].geometry_msg_to_previous,markers_[i].marker_id,i); 668 | } 669 | 670 | // Global Position of object 671 | if(world_option == true) 672 | broadcaster_.sendTransform(tf::StampedTransform(world_position_transform_,ros::Time::now(),"world","camera_position")); 673 | } 674 | 675 | //////////////////////////////////////////////////////////////////////////////////////////////// 676 | 677 | void 678 | ArucoMapping::publishMarker(geometry_msgs::Pose marker_pose, int marker_id, int index) 679 | { 680 | visualization_msgs::Marker vis_marker; 681 | 682 | if(index == 0) 683 | vis_marker.header.frame_id = "world"; 684 | else 685 | { 686 | std::stringstream marker_tf_id_old; 687 | marker_tf_id_old << "marker_" << markers_[index].previous_marker_id; 688 | vis_marker.header.frame_id = marker_tf_id_old.str(); 689 | } 690 | 691 | vis_marker.header.stamp = ros::Time::now(); 692 | vis_marker.ns = "basic_shapes"; 693 | vis_marker.id = marker_id; 694 | vis_marker.type = visualization_msgs::Marker::CUBE; 695 | vis_marker.action = visualization_msgs::Marker::ADD; 696 | 697 | vis_marker.pose = marker_pose; 698 | vis_marker.scale.x = marker_size_; 699 | vis_marker.scale.y = marker_size_; 700 | vis_marker.scale.z = RVIZ_MARKER_HEIGHT; 701 | 702 | vis_marker.color.r = RVIZ_MARKER_COLOR_R; 703 | vis_marker.color.g = RVIZ_MARKER_COLOR_G; 704 | vis_marker.color.b = RVIZ_MARKER_COLOR_B; 705 | vis_marker.color.a = RVIZ_MARKER_COLOR_A; 706 | 707 | vis_marker.lifetime = ros::Duration(RVIZ_MARKER_LIFETIME); 708 | 709 | marker_visualization_pub_.publish(vis_marker); 710 | } 711 | 712 | //////////////////////////////////////////////////////////////////////////////////////////////// 713 | 714 | tf::Transform 715 | ArucoMapping::arucoMarker2Tf(const aruco::Marker &marker) 716 | { 717 | cv::Mat marker_rotation(3,3, CV_32FC1); 718 | cv::Rodrigues(marker.Rvec, marker_rotation); 719 | cv::Mat marker_translation = marker.Tvec; 720 | 721 | cv::Mat rotate_to_ros(3,3,CV_32FC1); 722 | rotate_to_ros.at(0,0) = -1.0; 723 | rotate_to_ros.at(0,1) = 0; 724 | rotate_to_ros.at(0,2) = 0; 725 | rotate_to_ros.at(1,0) = 0; 726 | rotate_to_ros.at(1,1) = 0; 727 | rotate_to_ros.at(1,2) = 1.0; 728 | rotate_to_ros.at(2,0) = 0.0; 729 | rotate_to_ros.at(2,1) = 1.0; 730 | rotate_to_ros.at(2,2) = 0.0; 731 | 732 | marker_rotation = marker_rotation * rotate_to_ros.t(); 733 | 734 | // Origin solution 735 | tf::Matrix3x3 marker_tf_rot(marker_rotation.at(0,0),marker_rotation.at(0,1),marker_rotation.at(0,2), 736 | marker_rotation.at(1,0),marker_rotation.at(1,1),marker_rotation.at(1,2), 737 | marker_rotation.at(2,0),marker_rotation.at(2,1),marker_rotation.at(2,2)); 738 | 739 | tf::Vector3 marker_tf_tran(marker_translation.at(0,0), 740 | marker_translation.at(1,0), 741 | marker_translation.at(2,0)); 742 | 743 | return tf::Transform(marker_tf_rot, marker_tf_tran); 744 | } 745 | 746 | 747 | 748 | } //aruco_mapping 749 | 750 | #endif //ARUCO_MAPPING_CPP 751 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************************************//** 2 | * @file main.cpp 3 | * 4 | * Copyright (c) 5 | * Smart Robotic Systems 6 | * March 2015 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | ******************************************************************************/ 31 | 32 | /* Author: Jan Bacik */ 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | int 39 | main(int argc, char **argv) 40 | { 41 | ros::init(argc,argv,"aruco_mapping"); 42 | ros::NodeHandle nh; 43 | 44 | // Aruco mapping object 45 | aruco_mapping::ArucoMapping obj(&nh); 46 | 47 | // Image node and subscriber 48 | image_transport::ImageTransport it(nh); 49 | image_transport::Subscriber img_sub = it.subscribe("/image_raw", 1, &aruco_mapping::ArucoMapping::imageCallback, &obj); 50 | 51 | ros::spin(); 52 | 53 | return(EXIT_SUCCESS); 54 | } 55 | 56 | --------------------------------------------------------------------------------