artoolkitplus.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2013 by Markus Bader                                    *
00003  *   markus.bader@tuwien.ac.at                                             *
00004  *                                                                         *
00005  *   This program is free software; you can redistribute it and/or modify  *
00006  *   it under the terms of the GNU General Public License as published by  *
00007  *   the Free Software Foundation; either version 2 of the License, or     *
00008  *   (at your option) any later version.                                   *
00009  *                                                                         *
00010  *   This program is distributed in the hope that it will be useful,       *
00011  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00012  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00013  *   GNU General Public License for more details.                          *
00014  *                                                                         *
00015  *   You should have received a copy of the GNU General Public License     *
00016  *   along with this program; if not, write to the                         *
00017  *   Free Software Foundation, Inc.,                                       *
00018  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
00019  ***************************************************************************/
00020 
00021 #include <tuw_artoolkitplus/artoolkitplus.h>
00022 #include <tuw_artoolkitplus/artoolkitplus_defaults.h>
00023 
00024 #include <tuw_artoolkitplus/TransformArrayStamped.h>
00025 
00026 #include <opencv/highgui.h>
00027 #include <cv_bridge/cv_bridge.h>
00028 #include <sensor_msgs/image_encodings.h>
00029 
00030 #include "ARToolKitPlus/TrackerSingleMarkerImpl.h"
00031 #include "ARToolKitPlus/TrackerMultiMarkerImpl.h"
00032 #include <ARToolKitPlus/CameraAdvImpl.h>
00033 
00034 
00035 
00036 class MyLogger: public ARToolKitPlus::Logger {
00037     void artLog(const char* nStr) {
00038         printf("%s", nStr);
00039     }
00040 };
00041 
00042 ARToolKitPlusNode::ARToolKitPlusNode(ros::NodeHandle & n) :
00043     n_(n), n_param_("~"), callback_counter_(0), imageTransport_(n_),  logger_(NULL), param_() {
00044 
00045     init();
00046     cameraSubscriber_ = imageTransport_.subscribeCamera( ARTOOLKITPLUS_IMAGE_SRC, 1, &ARToolKitPlusNode::imageCallback, this);
00047 }
00048 
00049 
00050 ARToolKitPlusNode::~ARToolKitPlusNode() {
00051     if (logger_ != NULL)
00052         delete logger_;
00053 }
00054 
00055 class ARCamera: public ARToolKitPlus::CameraAdvImpl {
00056 public:
00057     virtual ~ARCamera() {
00058     };
00059 
00060     bool isInputDistorted() {
00061         if (undist_iterations == 1) {
00062             return false;
00063         } else {
00064             return true;
00065         }
00066     }
00067 
00068     ARCamera(const sensor_msgs::CameraInfoConstPtr& _camer_info, int _undist_iterations, bool _input_distorted) {
00069 
00070 
00071         // ARToolKitPlus::CameraAdvImpl Parameter
00072         if (_input_distorted) {
00073             // using the ros Intrinsic camera matrix for the raw (distorted) images.
00074             this->fc[0] = (ARFloat) _camer_info->K[0];
00075             this->fc[1] = (ARFloat) _camer_info->K[4];
00076             this->cc[0] = (ARFloat) _camer_info->K[2];
00077             this->cc[1] = (ARFloat) _camer_info->K[5];
00078 
00079             undist_iterations = _undist_iterations;
00080 
00081             this->kc[0] = (ARFloat) _camer_info->D[0];
00082             this->kc[1] = (ARFloat) _camer_info->D[1];
00083             this->kc[2] = (ARFloat) _camer_info->D[2];
00084             this->kc[3] = (ARFloat) _camer_info->D[3];
00085             this->kc[4] = (ARFloat) _camer_info->D[4];
00086             this->kc[5] = (ARFloat) 0.;
00087 
00088         } else {
00089             // using the ros Projection/camera matrix
00090             this->fc[0] = (ARFloat) _camer_info->P[0];
00091             this->fc[1] = (ARFloat) _camer_info->P[5];
00092             this->cc[0] = (ARFloat) _camer_info->P[2];
00093             this->cc[1] = (ARFloat) _camer_info->P[6];
00094 
00095             undist_iterations = 1;
00096 
00097             for (int i = 0; i < 6; i++)
00098                 this->kc[i] = (ARFloat) 0.;
00099 
00100         }
00101 
00102         // ARToolKitPlus::Camera Parameter
00103         // fileName
00104 
00105         // ARToolKit::ARParam Parameter
00106         xsize = _camer_info->width;
00107         ysize = _camer_info->height;
00108 
00109         for (int i = 0; i < 3; i++)
00110             for (int j = 0; j < 4; j++)
00111                 this->mat[i][j] = (ARFloat) 0.;
00112 
00113         mat[0][0] = fc[0]; // fc_x
00114         mat[1][1] = fc[1]; // fc_y
00115         mat[0][2] = cc[0]; // cc_x
00116         mat[1][2] = cc[1]; // cc_y
00117         mat[2][2] = 1.0;
00118 
00119         if (_input_distorted == false) {
00120             // using the ros Projection/camera matrix
00121             mat[0][3] = (ARFloat) _camer_info->P[3];
00122             mat[1][3] = (ARFloat) _camer_info->P[7];
00123         }
00124 
00125         for (int i = 0; i < 4; i++)
00126             this->dist_factor[i] = this->kc[i];
00127     }
00128 };
00129 
00130 
00131 void ARToolKitPlusNode::initTrackerMultiMarker(const sensor_msgs::CameraInfoConstPtr& camer_info) {
00132     trackerMultiMarker_ = boost::shared_ptr<ARToolKitPlus::TrackerMultiMarker>(new ARToolKitPlus::TrackerMultiMarkerImpl<AR_TRACKER_PARAM>(camer_info->width, camer_info->height));
00133     const char* description = trackerMultiMarker_->getDescription();
00134     ROS_INFO("%s: compile-time information:\n%s", param_.node_name.c_str(), description);
00135 
00136 // set a logger so we can output error messages
00137     if(logger_ == NULL) logger_ = new MyLogger();
00138     trackerMultiMarker_->setLogger(logger_);
00139     trackerMultiMarker_->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_LUM);
00140 
00141     ARCamera *camera = new ARCamera(camer_info, param_.undist_iterations, param_.distorted_input);
00142     if (!trackerMultiMarker_->init(camera, param_.pattern_file.c_str(), 1.0f, 1000.0f)) {
00143         ROS_ERROR("ERROR: init() failed");
00144     }
00145 
00146 }
00147 
00148 void ARToolKitPlusNode::initTrackerSingleMarker(const sensor_msgs::CameraInfoConstPtr& camer_info) {
00149     trackerSingleMarker_ = boost::shared_ptr<ARToolKitPlus::TrackerSingleMarker>(new ARToolKitPlus::TrackerSingleMarkerImpl<AR_TRACKER_PARAM>(camer_info->width, camer_info->height));
00150     const char* description = trackerSingleMarker_->getDescription();
00151     ROS_INFO("%s: compile-time information:\n%s", param_.node_name.c_str(), description);
00152 
00153 // set a logger so we can output error messages
00154     trackerSingleMarker_->setLogger(logger_);
00155     trackerSingleMarker_->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_LUM);
00156 
00157     ARCamera *camera = new ARCamera(camer_info, param_.undist_iterations, param_.distorted_input);
00158     if (!trackerSingleMarker_->init(camera, 1.0f, 1000.0f)) {
00159         ROS_ERROR("ERROR: init() failed");
00160     }
00161 
00162 }
00163 
00164 void ARToolKitPlusNode::updateParameterTrackerSingleMarker(const sensor_msgs::CameraInfoConstPtr& camer_info) {
00165 
00166     ARCamera * camera = (ARCamera *) trackerSingleMarker_->getCamera();
00167     if (camera->isInputDistorted() != param_.distorted_input) {
00168         delete camera;
00169         camera = new ARCamera(camer_info, param_.undist_iterations, param_.distorted_input);
00170         trackerSingleMarker_->setCamera(camera);
00171     }
00172 
00173 // define size of the marker
00174     trackerSingleMarker_->setPatternWidth(param_.patternWidth);
00175 
00176 // the marker in the BCH test image has a thin border...
00177     if (param_.borderWidth > 0) {
00178         trackerSingleMarker_->setBorderWidth(param_.borderWidth);
00179     } else {
00180         trackerSingleMarker_->setBorderWidth(param_.useBCH ? 0.125f : 0.250f);
00181     }
00182 
00183 // set a threshold. alternatively we could also activate automatic thresholding
00184     if (param_.edge_threshold > 0) {
00185         trackerSingleMarker_->activateAutoThreshold(false);
00186         trackerSingleMarker_->setThreshold(param_.edge_threshold);
00187     } else {
00188         trackerSingleMarker_->activateAutoThreshold(true);
00189     }
00190 // let's use lookup-table undistortion for high-speed
00191 // note: LUT only works with images up to 1024x1024
00192     trackerSingleMarker_->setUndistortionMode((ARToolKitPlus::UNDIST_MODE) param_.undist_mode);
00193 
00194 // RPP is more robust than ARToolKit's standard pose estimator
00195     trackerSingleMarker_->setPoseEstimator((ARToolKitPlus::POSE_ESTIMATOR) param_.pose_estimation_mode);
00196 
00197 // switch to simple ID based markers
00198 // use the tool in tools/IdPatGen to generate markers
00199     trackerSingleMarker_->setMarkerMode(param_.useBCH ? ARToolKitPlus::MARKER_ID_BCH : ARToolKitPlus::MARKER_ID_SIMPLE);
00200 
00201 // do the OpenGL camera setup
00202 //glMatrixMode(GL_PROJECTION)
00203 //glLoadMatrixf(tracker->getProjectionMatrix());
00204 }
00205 
00206 void ARToolKitPlusNode::updateParameterTrackerMultiMarker(const sensor_msgs::CameraInfoConstPtr& camer_info) {
00207 
00208     ARCamera * camera = (ARCamera *) trackerMultiMarker_->getCamera();
00209     if (camera->isInputDistorted() != param_.distorted_input) {
00210         delete camera;
00211         camera = new ARCamera(camer_info, param_.undist_iterations, param_.distorted_input);
00212         trackerMultiMarker_->setCamera(camera);
00213     }
00214 
00215     trackerMultiMarker_->setUseDetectLite(param_.use_multi_marker_lite_detection);
00216 
00217 // the marker in the BCH test image has a thin border...
00218     if (param_.borderWidth > 0) {
00219         trackerMultiMarker_->setBorderWidth(param_.borderWidth);
00220     } else {
00221         trackerMultiMarker_->setBorderWidth(param_.useBCH ? 0.125f : 0.250f);
00222     }
00223 
00224 // set a threshold. alternatively we could also activate automatic thresholding
00225     if (param_.edge_threshold > 0) {
00226         trackerMultiMarker_->activateAutoThreshold(false);
00227         trackerMultiMarker_->setThreshold(param_.edge_threshold);
00228     } else {
00229         trackerMultiMarker_->activateAutoThreshold(true);
00230     }
00231 // let's use lookup-table undistortion for high-speed
00232 // note: LUT only works with images up to 1024x1024
00233     trackerMultiMarker_->setUndistortionMode((ARToolKitPlus::UNDIST_MODE) param_.undist_mode);
00234 
00235 // RPP is more robust than ARToolKit's standard pose estimator
00236     trackerMultiMarker_->setPoseEstimator((ARToolKitPlus::POSE_ESTIMATOR) param_.pose_estimation_mode);
00237 
00238 // switch to simple ID based markers
00239 // use the tool in tools/IdPatGen to generate markers
00240     trackerMultiMarker_->setMarkerMode(param_.useBCH ? ARToolKitPlus::MARKER_ID_BCH : ARToolKitPlus::MARKER_ID_SIMPLE);
00241 
00242 // do the OpenGL camera setup
00243 //glMatrixMode(GL_PROJECTION)
00244 //glLoadMatrixf(tracker->getProjectionMatrix());
00245 
00246 }
00247 
00248 
00249 void ARToolKitPlusNode::imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camer_info_) {
00250     callback_counter_++;
00251     if((callback_counter_ % (param_.skip_frames+1) ) != 0) {
00252         return;
00253     }
00254     cv_bridge::CvImagePtr img;
00255     try {
00256         img = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8);
00257     } catch (cv_bridge::Exception& e) {
00258         ROS_ERROR("cv_bridge exception: %s", e.what());
00259         return;
00260     }
00261     if (param_.tracker_single_marker) {
00262         if(trackerSingleMarker_ == NULL) initTrackerSingleMarker(camer_info_);
00263 
00264         updateParameterTrackerSingleMarker(camer_info_);
00265         ARToolKitPlus::ARMarkerInfo* arMarkerInfo;
00266         int nNumMarkers;
00267         int markerId = trackerSingleMarker_->calc(img->image.data, param_.nPattern, param_.nUpdateMatrix, &arMarkerInfo, &nNumMarkers);
00268         float conf = (float) trackerSingleMarker_->getConfidence();
00269         arTags2D_.resize(nNumMarkers);
00270         for(int i = 0; i < arTags2D_.size(); i++) {
00271             arTags2D_[i] = arMarkerInfo[i];
00272         }
00273     }
00274     if (param_.tracker_multi_marker) {
00275         if(trackerMultiMarker_ == NULL) initTrackerMultiMarker(camer_info_);
00276         updateParameterTrackerMultiMarker(camer_info_);
00277         int nNumMarkers = trackerMultiMarker_->calc(img->image.data);
00278 
00279         arMultiMarkerInfo_ = trackerMultiMarker_->getMultiMarkerConfig();
00280         arTags2D_.clear();
00281         arTags2D_.reserve(trackerMultiMarker_->getNumDetectedMarkers());
00283         for(int i = 0; i < trackerMultiMarker_->getNumDetectedMarkers(); i++) {
00284             const ARToolKitPlus::ARMarkerInfo &singleMarker = trackerMultiMarker_->getDetectedMarker(i);
00285             arTags2D_.push_back(singleMarker);
00286             arTags2D_.back().belongsToPattern = ARToolKitPlus::ARTag2D::NO_PATTERN;
00287             for(int j = 0; (j < arMultiMarkerInfo_->marker_num); j++) {
00288                 const ARToolKitPlus::ARMultiEachMarkerInfoT &multiMarker = arMultiMarkerInfo_->marker[j];
00289                 if(singleMarker.id == multiMarker.patt_id) {
00290                     arTags2D_.back().belongsToPattern = ARToolKitPlus::ARTag2D::PATTERN;
00291                     break;
00292                 }
00293             }
00294         }
00295     }
00296 
00297     estimatePoses(image_msg->header);
00298 
00299     publishTf();
00300 
00301     if (param_.show_camera_image_) {
00302         cv::Mat img_debug;
00303         cvtColor(img->image, img_debug, CV_GRAY2BGR);
00304         generateDebugImage(img_debug);
00305         cv::imshow( param_.node_name + std::string(" - debug"), img_debug);
00306         cv::waitKey(5);
00307     }
00308 }
00309 
00310 void ARToolKitPlusNode::estimatePoses(const std_msgs::Header &header) {
00311 
00312     ARFloat center[2];
00313     center[0] = 0;
00314     center[1] = 0;
00315     ARFloat pose[3][4];
00316     tf::Transform trans;
00317     tf::StampedTransform st;
00318     char frame[0xFF];
00319     markerTransforms_.clear();
00320 
00321     if(trackerMultiMarker_) {
00322 
00323         if( arMultiMarkerInfo_->marker_num > 0)
00324         {
00325             const ARFloat *p = trackerMultiMarker_->getModelViewMatrix();
00326             for(int r = 0; r < 3; r++) {
00327                 pose[r][0] = p[r+0];
00328                 pose[r][1] = p[r+4];
00329                 pose[r][2] = p[r+8];
00330                 pose[r][3] = p[r+12];
00331             }
00332             matrix2Tf(pose, trans);
00333             std::string child_frame = tf::resolve(param_.tf_prefix, param_.pattern_frame);
00334             st = tf::StampedTransform(trans, header.stamp, header.frame_id, child_frame);
00335             markerTransforms_.push_back(st);
00336         }
00337     }
00338     for(std::vector<ARToolKitPlus::ARTag2D>::iterator arTag =  arTags2D_.begin(); arTag != arTags2D_.end(); arTag++) {
00339         if (arTag->id < 0)
00340             continue;
00341         if (arTag->belongsToPattern != ARToolKitPlus::ARTag2D::NO_PATTERN)
00342             continue;
00343         sprintf(frame, "t%i", arTag->id);
00344         if(trackerMultiMarker_) trackerMultiMarker_->executeSingleMarkerPoseEstimator(&(*arTag), center, param_.patternWidth, pose);
00345         if(trackerSingleMarker_) trackerSingleMarker_->executeSingleMarkerPoseEstimator(&(*arTag), center, param_.patternWidth, pose);
00346         matrix2Tf(pose, trans);
00347         std::string child_frame = tf::resolve(param_.tf_prefix, frame);
00348         st = tf::StampedTransform(trans, header.stamp, header.frame_id, child_frame);
00349         markerTransforms_.push_back(st);
00350     }
00351 }
00352 
00353 void ARToolKitPlusNode::init() {
00354     if (param_.show_camera_image_) {
00355         cv::namedWindow( param_.node_name + std::string(" - debug"), 1);
00356     }
00357 }
00358 
00359 


tuw_artoolkitplus
Author(s): Markus Bader
autogenerated on Sun May 29 2016 02:50:12