aruco_mapping.cpp
Go to the documentation of this file.
00001 /*********************************************************************************************/
00032 /* Author: Jan Bacik */
00033 
00034 #ifndef ARUCO_MAPPING_CPP
00035 #define ARUCO_MAPPING_CPP
00036 
00037 #include <aruco_mapping.h>
00038 
00039 namespace aruco_mapping
00040 {
00041 
00042 ArucoMapping::ArucoMapping(ros::NodeHandle *nh) :
00043   listener_ (new tf::TransformListener),  // Initialize TF Listener  
00044   num_of_markers_ (10),                   // Number of used markers
00045   marker_size_(0.1),                      // Marker size in m
00046   calib_filename_("empty"),               // Calibration filepath
00047   space_type_ ("plane"),                  // Space type - 2D plane 
00048   roi_allowed_ (false),                   // ROI not allowed by default
00049   first_marker_detected_(false),          // First marker not detected by defualt
00050   lowest_marker_id_(-1),                  // Lowest marker ID
00051   marker_counter_(0),                     // Reset marker counter
00052   closest_camera_index_(0)                // Reset closest camera index 
00053   
00054 {
00055   double temp_marker_size;  
00056   
00057   //Parse params from launch file 
00058   nh->getParam("/aruco_mapping/calibration_file", calib_filename_);
00059   nh->getParam("/aruco_mapping/marker_size", temp_marker_size); 
00060   nh->getParam("/aruco_mapping/num_of_markers", num_of_markers_);
00061   nh->getParam("/aruco_maping/space_type",space_type_);
00062   nh->getParam("/aruco_mapping/roi_allowed",roi_allowed_);
00063   nh->getParam("/aruco_mapping/roi_x",roi_x_);
00064   nh->getParam("/aruco_mapping/roi_y",roi_y_);
00065   nh->getParam("/aruco_mapping/roi_w",roi_w_);
00066   nh->getParam("/aruco_mapping/roi_h",roi_h_);
00067      
00068   // Double to float conversion
00069   marker_size_ = float(temp_marker_size);
00070   
00071   if(calib_filename_ == "empty")
00072     ROS_WARN("Calibration filename empty! Check the launch file paths");
00073   else
00074   {
00075     ROS_INFO_STREAM("Calibration file path: " << calib_filename_ );
00076     ROS_INFO_STREAM("Number of markers: " << num_of_markers_);
00077     ROS_INFO_STREAM("Marker Size: " << marker_size_);
00078     ROS_INFO_STREAM("Type of space: " << space_type_);
00079     ROS_INFO_STREAM("ROI allowed: " << roi_allowed_);
00080     ROS_INFO_STREAM("ROI x-coor: " << roi_x_);
00081     ROS_INFO_STREAM("ROI y-coor: " << roi_x_);
00082     ROS_INFO_STREAM("ROI width: "  << roi_w_);
00083     ROS_INFO_STREAM("ROI height: " << roi_h_);      
00084   }
00085     
00086   //ROS publishers
00087   marker_msg_pub_           = nh->advertise<aruco_mapping::ArucoMarker>("aruco_poses",1);
00088   marker_visualization_pub_ = nh->advertise<visualization_msgs::Marker>("aruco_markers",1);
00089           
00090   //Parse data from calibration file
00091   parseCalibrationFile(calib_filename_);
00092 
00093   //Initialize OpenCV window
00094   cv::namedWindow("Mono8", CV_WINDOW_AUTOSIZE);       
00095       
00096   //Resize marker container
00097   markers_.resize(num_of_markers_);
00098   
00099   // Default markers_ initialization
00100   for(size_t i = 0; i < num_of_markers_;i++)
00101   {
00102     markers_[i].previous_marker_id = -1;
00103     markers_[i].visible = false;
00104     markers_[i].marker_id = -1;
00105   }
00106 }
00107 
00108 ArucoMapping::~ArucoMapping()
00109 {
00110  delete listener_;
00111 }
00112 
00113 bool
00114 ArucoMapping::parseCalibrationFile(std::string calib_filename)
00115 {
00116   sensor_msgs::CameraInfo camera_calibration_data;
00117   std::string camera_name = "camera";
00118 
00119   camera_calibration_parsers::readCalibrationIni(calib_filename, camera_name, camera_calibration_data);
00120 
00121   // Alocation of memory for calibration data
00122   cv::Mat  *intrinsics       = new(cv::Mat)(3, 3, CV_64F);
00123   cv::Mat  *distortion_coeff = new(cv::Mat)(5, 1, CV_64F);
00124   cv::Size *image_size       = new(cv::Size);
00125 
00126   image_size->width = camera_calibration_data.width;
00127   image_size->height = camera_calibration_data.height;
00128 
00129   for(size_t i = 0; i < 3; i++)
00130     for(size_t j = 0; j < 3; j++)
00131     intrinsics->at<double>(i,j) = camera_calibration_data.K.at(3*i+j);
00132 
00133   for(size_t i = 0; i < 5; i++)
00134     distortion_coeff->at<double>(i,0) = camera_calibration_data.D.at(i);
00135 
00136   ROS_DEBUG_STREAM("Image width: " << image_size->width);
00137   ROS_DEBUG_STREAM("Image height: " << image_size->height);
00138   ROS_DEBUG_STREAM("Intrinsics:" << std::endl << *intrinsics);
00139   ROS_DEBUG_STREAM("Distortion: " << *distortion_coeff);
00140 
00141 
00142   //Load parameters to aruco_calib_param_ for aruco detection
00143   aruco_calib_params_.setParams(*intrinsics, *distortion_coeff, *image_size);
00144 
00145   //Simple check if calibration data meets expected values
00146   if ((intrinsics->at<double>(2,2) == 1) && (distortion_coeff->at<double>(0,4) == 0))
00147   {
00148     ROS_INFO_STREAM("Calibration data loaded successfully");
00149     return true;
00150   }
00151   else
00152   {
00153     ROS_WARN("Wrong calibration data, check calibration file and filepath");
00154     return false;
00155   }
00156 }
00157 
00158 void
00159 ArucoMapping::imageCallback(const sensor_msgs::ImageConstPtr &original_image)
00160 {
00161   //Create cv_brigde instance
00162   cv_bridge::CvImagePtr cv_ptr;
00163   try
00164   {
00165     cv_ptr=cv_bridge::toCvCopy(original_image, sensor_msgs::image_encodings::MONO8);
00166   }
00167   catch (cv_bridge::Exception& e)
00168   {
00169     ROS_ERROR("Not able to convert sensor_msgs::Image to OpenCV::Mat format %s", e.what());
00170     return;
00171   }
00172   
00173   // sensor_msgs::Image to OpenCV Mat structure
00174   cv::Mat I = cv_ptr->image;
00175   
00176   // region of interest
00177   if(roi_allowed_==true)
00178     I = cv_ptr->image(cv::Rect(roi_x_,roi_y_,roi_w_,roi_h_));
00179 
00180   //Marker detection
00181   processImage(I,I);
00182   
00183   // Show image
00184   cv::imshow("Mono8", I);
00185   cv::waitKey(10);  
00186 }
00187 
00188 
00189 bool
00190 ArucoMapping::processImage(cv::Mat input_image,cv::Mat output_image)
00191 {
00192   aruco::MarkerDetector Detector;
00193   std::vector<aruco::Marker> temp_markers;
00194 
00195   //Set visibility flag to false for all markers
00196   for(size_t i = 0; i < num_of_markers_; i++)
00197       markers_[i].visible = false;
00198 
00199   // Save previous marker count
00200   marker_counter_previous_ = marker_counter_;
00201 
00202   // Detect markers
00203   Detector.detect(input_image,temp_markers,aruco_calib_params_,marker_size_);
00204     
00205   // If no marker found, print statement
00206   if(temp_markers.size() == 0)
00207     ROS_DEBUG("No marker found!");
00208 
00209   //------------------------------------------------------
00210   // FIRST MARKER DETECTED
00211   //------------------------------------------------------
00212   if((temp_markers.size() > 0) && (first_marker_detected_ == false))
00213   {
00214     //Set flag
00215     first_marker_detected_=true;
00216 
00217     // Detect lowest marker ID
00218     lowest_marker_id_ = temp_markers[0].id;
00219     for(size_t i = 0; i < temp_markers.size();i++)
00220     {
00221       if(temp_markers[i].id < lowest_marker_id_)
00222         lowest_marker_id_ = temp_markers[i].id;
00223     }
00224 
00225 
00226     ROS_DEBUG_STREAM("The lowest Id marker " << lowest_marker_id_ );
00227 
00228     // Identify lowest marker ID with world's origin
00229     markers_[0].marker_id = lowest_marker_id_;
00230 
00231     markers_[0].geometry_msg_to_world.position.x = 0;
00232     markers_[0].geometry_msg_to_world.position.y = 0;
00233     markers_[0].geometry_msg_to_world.position.z = 0;
00234 
00235     markers_[0].geometry_msg_to_world.orientation.x = 0;
00236     markers_[0].geometry_msg_to_world.orientation.y = 0;
00237     markers_[0].geometry_msg_to_world.orientation.z = 0;
00238     markers_[0].geometry_msg_to_world.orientation.w = 1;
00239 
00240     // Relative position and Global position
00241     markers_[0].geometry_msg_to_previous.position.x = 0;
00242     markers_[0].geometry_msg_to_previous.position.y = 0;
00243     markers_[0].geometry_msg_to_previous.position.z = 0;
00244 
00245     markers_[0].geometry_msg_to_previous.orientation.x = 0;
00246     markers_[0].geometry_msg_to_previous.orientation.y = 0;
00247     markers_[0].geometry_msg_to_previous.orientation.z = 0;
00248     markers_[0].geometry_msg_to_previous.orientation.w = 1;
00249 
00250     // Transformation Pose to TF
00251     tf::Vector3 position;
00252     position.setX(0);
00253     position.setY(0);
00254     position.setZ(0);
00255 
00256     tf::Quaternion rotation;
00257     rotation.setX(0);
00258     rotation.setY(0);
00259     rotation.setZ(0);
00260     rotation.setW(1);
00261 
00262     markers_[0].tf_to_previous.setOrigin(position);
00263     markers_[0].tf_to_previous.setRotation(rotation);
00264 
00265     // Relative position of first marker equals Global position
00266     markers_[0].tf_to_world=markers_[0].tf_to_previous;
00267 
00268     // Increase count
00269     marker_counter_++;
00270 
00271     // Set sign of visibility of first marker
00272     markers_[0].visible=true;
00273 
00274     ROS_INFO_STREAM("First marker with ID: " << markers_[0].marker_id << " detected");
00275 
00276     //First marker does not have any previous marker
00277     markers_[0].previous_marker_id = THIS_IS_FIRST_MARKER;
00278   }
00279 
00280   //------------------------------------------------------
00281   // FOR EVERY MARKER DO
00282   //------------------------------------------------------
00283   for(size_t i = 0; i < temp_markers.size();i++)
00284   {
00285     int index;
00286     int current_marker_id = temp_markers[i].id;
00287 
00288     //Draw marker convex, ID, cube and axis
00289     temp_markers[i].draw(output_image, cv::Scalar(0,0,255),2);
00290     aruco::CvDrawingUtils::draw3dCube(output_image,temp_markers[i], aruco_calib_params_);
00291     aruco::CvDrawingUtils::draw3dAxis(output_image,temp_markers[i], aruco_calib_params_);
00292 
00293     // Existing marker ?
00294     bool existing = false;
00295     int temp_counter = 0;
00296 
00297     while((existing == false) && (temp_counter < marker_counter_))
00298     {
00299       if(markers_[temp_counter].marker_id == current_marker_id)
00300       {
00301         index = temp_counter;
00302         existing = true;
00303         ROS_DEBUG_STREAM("Existing marker with ID: " << current_marker_id << "found");
00304       }
00305         temp_counter++;
00306     }
00307 
00308     //New marker ?
00309     if(existing == false)
00310     {
00311       index = marker_counter_;
00312       markers_[index].marker_id = current_marker_id;
00313       existing = true;
00314       ROS_DEBUG_STREAM("New marker with ID: " << current_marker_id << " found");
00315     }
00316 
00317     // Change visibility flag of new marker
00318     for(size_t j = 0;j < marker_counter_; j++)
00319     {
00320       for(size_t k = 0;k < temp_markers.size(); k++)
00321       {
00322         if(markers_[j].marker_id == temp_markers[k].id)
00323           markers_[j].visible = true;
00324       }
00325     }
00326 
00327     //------------------------------------------------------
00328     // For existing marker do
00329     //------------------------------------------------------
00330     if((index < marker_counter_) && (first_marker_detected_ == true))
00331     {
00332       markers_[index].current_camera_tf = arucoMarker2Tf(temp_markers[i]);
00333       markers_[index].current_camera_tf = markers_[index].current_camera_tf.inverse();
00334 
00335       const tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin();
00336       markers_[index].current_camera_pose.position.x = marker_origin.getX();
00337       markers_[index].current_camera_pose.position.y = marker_origin.getY();
00338       markers_[index].current_camera_pose.position.z = marker_origin.getZ();
00339 
00340       const tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation();
00341       markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
00342       markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
00343       markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
00344       markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();
00345     }
00346 
00347     //------------------------------------------------------
00348     // For new marker do
00349     //------------------------------------------------------
00350     if((index == marker_counter_) && (first_marker_detected_ == true))
00351     {
00352       markers_[index].current_camera_tf=arucoMarker2Tf(temp_markers[i]);
00353 
00354       tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin();
00355       markers_[index].current_camera_pose.position.x = marker_origin.getX();
00356       markers_[index].current_camera_pose.position.y = marker_origin.getY();
00357       markers_[index].current_camera_pose.position.z = marker_origin.getZ();
00358 
00359       tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation();
00360       markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
00361       markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
00362       markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
00363       markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();
00364 
00365       // Naming - TFs
00366       std::stringstream camera_tf_id;
00367       std::stringstream camera_tf_id_old;
00368       std::stringstream marker_tf_id_old;
00369 
00370       camera_tf_id << "camera_" << index;
00371 
00372       // Flag to keep info if any_known marker_visible in actual image
00373       bool any_known_marker_visible = false;
00374 
00375       // Array ID of markers, which position of new marker is calculated
00376       int last_marker_id;
00377 
00378       // Testing, if is possible calculate position of a new marker to old known marker
00379       for(int k = 0; k < index; k++)
00380       {
00381         if((markers_[k].visible == true) && (any_known_marker_visible == false))
00382         {
00383           if(markers_[k].previous_marker_id != -1)
00384           {
00385             any_known_marker_visible = true;
00386             camera_tf_id_old << "camera_" << k;
00387             marker_tf_id_old << "marker_" << k;
00388             markers_[index].previous_marker_id = k;
00389             last_marker_id = k;
00390            }
00391          }
00392        }
00393 
00394      // New position can be calculated
00395      if(any_known_marker_visible == true)
00396      {
00397        // Generating TFs for listener
00398        for(char k = 0; k < 10; k++)
00399        {
00400          // TF from old marker and its camera
00401          broadcaster_.sendTransform(tf::StampedTransform(markers_[last_marker_id].current_camera_tf,ros::Time::now(),
00402                                                          marker_tf_id_old.str(),camera_tf_id_old.str()));
00403 
00404          // TF from old camera to new camera
00405          broadcaster_.sendTransform(tf::StampedTransform(markers_[index].current_camera_tf,ros::Time::now(),
00406                                                          camera_tf_id_old.str(),camera_tf_id.str()));
00407 
00408          ros::Duration(BROADCAST_WAIT_INTERVAL).sleep();
00409        }
00410 
00411         // Calculate TF between two markers
00412         listener_->waitForTransform(marker_tf_id_old.str(),camera_tf_id.str(),ros::Time(0),
00413                                     ros::Duration(WAIT_FOR_TRANSFORM_INTERVAL));
00414         try
00415         {
00416           broadcaster_.sendTransform(tf::StampedTransform(markers_[last_marker_id].current_camera_tf,ros::Time::now(),
00417                                                           marker_tf_id_old.str(),camera_tf_id_old.str()));
00418 
00419           broadcaster_.sendTransform(tf::StampedTransform(markers_[index].current_camera_tf,ros::Time::now(),
00420                                                           camera_tf_id_old.str(),camera_tf_id.str()));
00421 
00422           listener_->lookupTransform(marker_tf_id_old.str(),camera_tf_id.str(),ros::Time(0),
00423                                      markers_[index].tf_to_previous);
00424         }
00425         catch(tf::TransformException &e)
00426         {
00427           ROS_ERROR("Not able to lookup transform");
00428         }
00429 
00430         // Save origin and quaternion of calculated TF
00431         marker_origin = markers_[index].tf_to_previous.getOrigin();
00432         marker_quaternion = markers_[index].tf_to_previous.getRotation();
00433 
00434         // If plane type selected roll, pitch and Z axis are zero
00435         if(space_type_ == "plane")
00436         {
00437           double roll, pitch, yaw;
00438           tf::Matrix3x3(marker_quaternion).getRPY(roll,pitch,yaw);
00439           roll = 0;
00440           pitch = 0;
00441           marker_origin.setZ(0);
00442           marker_quaternion.setRPY(pitch,roll,yaw);
00443         }
00444 
00445         markers_[index].tf_to_previous.setRotation(marker_quaternion);
00446         markers_[index].tf_to_previous.setOrigin(marker_origin);
00447 
00448         marker_origin = markers_[index].tf_to_previous.getOrigin();
00449         markers_[index].geometry_msg_to_previous.position.x = marker_origin.getX();
00450         markers_[index].geometry_msg_to_previous.position.y = marker_origin.getY();
00451         markers_[index].geometry_msg_to_previous.position.z = marker_origin.getZ();
00452 
00453         marker_quaternion = markers_[index].tf_to_previous.getRotation();
00454         markers_[index].geometry_msg_to_previous.orientation.x = marker_quaternion.getX();
00455         markers_[index].geometry_msg_to_previous.orientation.y = marker_quaternion.getY();
00456         markers_[index].geometry_msg_to_previous.orientation.z = marker_quaternion.getZ();
00457         markers_[index].geometry_msg_to_previous.orientation.w = marker_quaternion.getW();
00458 
00459         // increase marker count
00460         marker_counter_++;
00461 
00462         // Invert and position of new marker to compute camera pose above it
00463         markers_[index].current_camera_tf = markers_[index].current_camera_tf.inverse();
00464 
00465         marker_origin = markers_[index].current_camera_tf.getOrigin();
00466         markers_[index].current_camera_pose.position.x = marker_origin.getX();
00467         markers_[index].current_camera_pose.position.y = marker_origin.getY();
00468         markers_[index].current_camera_pose.position.z = marker_origin.getZ();
00469 
00470         marker_quaternion = markers_[index].current_camera_tf.getRotation();
00471         markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
00472         markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
00473         markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
00474         markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();
00475 
00476         // Publish all TFs and markers
00477         publishTfs(false);
00478       }
00479     }
00480 
00481     //------------------------------------------------------
00482     // Compute global position of new marker
00483     //------------------------------------------------------
00484     if((marker_counter_previous_ < marker_counter_) && (first_marker_detected_ == true))
00485     {
00486       // Publish all TF five times for listener
00487       for(char k = 0; k < 5; k++)
00488         publishTfs(false);
00489 
00490       std::stringstream marker_tf_name;
00491       marker_tf_name << "marker_" << index;
00492 
00493       listener_->waitForTransform("world",marker_tf_name.str(),ros::Time(0),
00494                                   ros::Duration(WAIT_FOR_TRANSFORM_INTERVAL));
00495       try
00496       {
00497         listener_->lookupTransform("world",marker_tf_name.str(),ros::Time(0),
00498                                    markers_[index].tf_to_world);
00499       }
00500       catch(tf::TransformException &e)
00501       {
00502         ROS_ERROR("Not able to lookup transform");
00503       }
00504 
00505       // Saving TF to Pose
00506       const tf::Vector3 marker_origin = markers_[index].tf_to_world.getOrigin();
00507       markers_[index].geometry_msg_to_world.position.x = marker_origin.getX();
00508       markers_[index].geometry_msg_to_world.position.y = marker_origin.getY();
00509       markers_[index].geometry_msg_to_world.position.z = marker_origin.getZ();
00510 
00511       tf::Quaternion marker_quaternion=markers_[index].tf_to_world.getRotation();
00512       markers_[index].geometry_msg_to_world.orientation.x = marker_quaternion.getX();
00513       markers_[index].geometry_msg_to_world.orientation.y = marker_quaternion.getY();
00514       markers_[index].geometry_msg_to_world.orientation.z = marker_quaternion.getZ();
00515       markers_[index].geometry_msg_to_world.orientation.w = marker_quaternion.getW();
00516     }
00517   }
00518 
00519   //------------------------------------------------------
00520   // Compute which of visible markers is the closest to the camera
00521   //------------------------------------------------------
00522   bool any_markers_visible=false;
00523   int num_of_visible_markers=0;
00524 
00525   if(first_marker_detected_ == true)
00526   {
00527     double minimal_distance = INIT_MIN_SIZE_VALUE;
00528     for(int k = 0; k < num_of_markers_; k++)
00529     {
00530       double a,b,c,size;
00531 
00532       // If marker is visible, distance is calculated
00533       if(markers_[k].visible==true)
00534       {
00535         a = markers_[k].current_camera_pose.position.x;
00536         b = markers_[k].current_camera_pose.position.y;
00537         c = markers_[k].current_camera_pose.position.z;
00538         size = std::sqrt((a * a) + (b * b) + (c * c));
00539         if(size < minimal_distance)
00540         {
00541           minimal_distance = size;
00542           closest_camera_index_ = k;
00543         }
00544 
00545         any_markers_visible = true;
00546         num_of_visible_markers++;
00547       }
00548     }
00549   }
00550 
00551   //------------------------------------------------------
00552   // Publish all known markers
00553   //------------------------------------------------------
00554   if(first_marker_detected_ == true)
00555     publishTfs(true);
00556 
00557   //------------------------------------------------------
00558   // Compute global camera pose
00559   //------------------------------------------------------
00560   if((first_marker_detected_ == true) && (any_markers_visible == true))
00561   {
00562     std::stringstream closest_camera_tf_name;
00563     closest_camera_tf_name << "camera_" << closest_camera_index_;
00564 
00565     listener_->waitForTransform("world",closest_camera_tf_name.str(),ros::Time(0),
00566                                 ros::Duration(WAIT_FOR_TRANSFORM_INTERVAL));
00567     try
00568     {
00569       listener_->lookupTransform("world",closest_camera_tf_name.str(),ros::Time(0),
00570                                  world_position_transform_);
00571     }
00572     catch(tf::TransformException &ex)
00573     {
00574       ROS_ERROR("Not able to lookup transform");
00575     }
00576 
00577     // Saving TF to Pose
00578     const tf::Vector3 marker_origin = world_position_transform_.getOrigin();
00579     world_position_geometry_msg_.position.x = marker_origin.getX();
00580     world_position_geometry_msg_.position.y = marker_origin.getY();
00581     world_position_geometry_msg_.position.z = marker_origin.getZ();
00582 
00583     tf::Quaternion marker_quaternion = world_position_transform_.getRotation();
00584     world_position_geometry_msg_.orientation.x = marker_quaternion.getX();
00585     world_position_geometry_msg_.orientation.y = marker_quaternion.getY();
00586     world_position_geometry_msg_.orientation.z = marker_quaternion.getZ();
00587     world_position_geometry_msg_.orientation.w = marker_quaternion.getW();
00588   }
00589 
00590   //------------------------------------------------------
00591   // Publish all known markers
00592   //------------------------------------------------------
00593   if(first_marker_detected_ == true)
00594     publishTfs(true);
00595 
00596   //------------------------------------------------------
00597   // Publish custom marker message
00598   //------------------------------------------------------
00599   aruco_mapping::ArucoMarker marker_msg;
00600 
00601   if((any_markers_visible == true))
00602   {
00603     marker_msg.header.stamp = ros::Time::now();
00604     marker_msg.header.frame_id = "world";
00605     marker_msg.marker_visibile = true;
00606     marker_msg.num_of_visible_markers = num_of_visible_markers;
00607     marker_msg.global_camera_pose = world_position_geometry_msg_;
00608     marker_msg.marker_ids.clear();
00609     marker_msg.global_marker_poses.clear();
00610     for(size_t j = 0; j < marker_counter_; j++)
00611     {
00612       if(markers_[j].visible == true)
00613       {
00614         marker_msg.marker_ids.push_back(markers_[j].marker_id);
00615         marker_msg.global_marker_poses.push_back(markers_[j].geometry_msg_to_world);       
00616       }
00617     }
00618   }
00619   else
00620   {
00621     marker_msg.header.stamp = ros::Time::now();
00622     marker_msg.header.frame_id = "world";
00623     marker_msg.num_of_visible_markers = num_of_visible_markers;
00624     marker_msg.marker_visibile = false;
00625     marker_msg.marker_ids.clear();
00626     marker_msg.global_marker_poses.clear();
00627   }
00628 
00629   // Publish custom marker msg
00630   marker_msg_pub_.publish(marker_msg);
00631 
00632   return true;
00633 }
00634 
00636 
00637 void
00638 ArucoMapping::publishTfs(bool world_option)
00639 {
00640   for(int i = 0; i < marker_counter_; i++)
00641   {
00642     // Actual Marker
00643     std::stringstream marker_tf_id;
00644     marker_tf_id << "marker_" << i;
00645     // Older marker - or World
00646     std::stringstream marker_tf_id_old;
00647     if(i == 0)
00648       marker_tf_id_old << "world";
00649     else
00650       marker_tf_id_old << "marker_" << markers_[i].previous_marker_id;
00651     broadcaster_.sendTransform(tf::StampedTransform(markers_[i].tf_to_previous,ros::Time::now(),marker_tf_id_old.str(),marker_tf_id.str()));
00652 
00653     // Position of camera to its marker
00654     std::stringstream camera_tf_id;
00655     camera_tf_id << "camera_" << i;
00656     broadcaster_.sendTransform(tf::StampedTransform(markers_[i].current_camera_tf,ros::Time::now(),marker_tf_id.str(),camera_tf_id.str()));
00657 
00658     if(world_option == true)
00659     {
00660       // Global position of marker TF
00661       std::stringstream marker_globe;
00662       marker_globe << "marker_globe_" << i;
00663       broadcaster_.sendTransform(tf::StampedTransform(markers_[i].tf_to_world,ros::Time::now(),"world",marker_globe.str()));
00664     }
00665 
00666     // Cubes for RVIZ - markers
00667     publishMarker(markers_[i].geometry_msg_to_previous,markers_[i].marker_id,i);
00668   }
00669 
00670   // Global Position of object
00671   if(world_option == true)
00672     broadcaster_.sendTransform(tf::StampedTransform(world_position_transform_,ros::Time::now(),"world","camera_position"));
00673 }
00674 
00676 
00677 void
00678 ArucoMapping::publishMarker(geometry_msgs::Pose marker_pose, int marker_id, int index)
00679 {
00680   visualization_msgs::Marker vis_marker;
00681 
00682   if(index == 0)
00683     vis_marker.header.frame_id = "world";
00684   else
00685   {
00686     std::stringstream marker_tf_id_old;
00687     marker_tf_id_old << "marker_" << markers_[index].previous_marker_id;
00688     vis_marker.header.frame_id = marker_tf_id_old.str();
00689   }
00690 
00691   vis_marker.header.stamp = ros::Time::now();
00692   vis_marker.ns = "basic_shapes";
00693   vis_marker.id = marker_id;
00694   vis_marker.type = visualization_msgs::Marker::CUBE;
00695   vis_marker.action = visualization_msgs::Marker::ADD;
00696 
00697   vis_marker.pose = marker_pose;
00698   vis_marker.scale.x = marker_size_;
00699   vis_marker.scale.y = marker_size_;
00700   vis_marker.scale.z = RVIZ_MARKER_HEIGHT;
00701 
00702   vis_marker.color.r = RVIZ_MARKER_COLOR_R;
00703   vis_marker.color.g = RVIZ_MARKER_COLOR_G;
00704   vis_marker.color.b = RVIZ_MARKER_COLOR_B;
00705   vis_marker.color.a = RVIZ_MARKER_COLOR_A;
00706 
00707   vis_marker.lifetime = ros::Duration(RVIZ_MARKER_LIFETIME);
00708 
00709   marker_visualization_pub_.publish(vis_marker);
00710 }
00711 
00713 
00714 tf::Transform
00715 ArucoMapping::arucoMarker2Tf(const aruco::Marker &marker)
00716 {
00717   cv::Mat marker_rotation(3,3, CV_32FC1);
00718   cv::Rodrigues(marker.Rvec, marker_rotation);
00719   cv::Mat marker_translation = marker.Tvec;
00720 
00721   cv::Mat rotate_to_ros(3,3,CV_32FC1);
00722   rotate_to_ros.at<float>(0,0) = -1.0;
00723   rotate_to_ros.at<float>(0,1) = 0;
00724   rotate_to_ros.at<float>(0,2) = 0;
00725   rotate_to_ros.at<float>(1,0) = 0;
00726   rotate_to_ros.at<float>(1,1) = 0;
00727   rotate_to_ros.at<float>(1,2) = 1.0;
00728   rotate_to_ros.at<float>(2,0) = 0.0;
00729   rotate_to_ros.at<float>(2,1) = 1.0;
00730   rotate_to_ros.at<float>(2,2) = 0.0;
00731 
00732   marker_rotation = marker_rotation * rotate_to_ros.t();
00733 
00734   // Origin solution
00735   tf::Matrix3x3 marker_tf_rot(marker_rotation.at<float>(0,0),marker_rotation.at<float>(0,1),marker_rotation.at<float>(0,2),
00736                               marker_rotation.at<float>(1,0),marker_rotation.at<float>(1,1),marker_rotation.at<float>(1,2),
00737                               marker_rotation.at<float>(2,0),marker_rotation.at<float>(2,1),marker_rotation.at<float>(2,2));
00738 
00739   tf::Vector3 marker_tf_tran(marker_translation.at<float>(0,0),
00740                              marker_translation.at<float>(1,0),
00741                              marker_translation.at<float>(2,0));
00742 
00743   return tf::Transform(marker_tf_rot, marker_tf_tran);
00744 }
00745 
00746 
00747 
00748 }  //aruco_mapping
00749 
00750 #endif  //ARUCO_MAPPING_CPP


aruco_mapping
Author(s): Jan Bacik
autogenerated on Thu Jun 6 2019 21:48:04