conversion.cpp
Go to the documentation of this file.
00001 #include <cstring>
00002 #include <stdexcept>
00003 
00004 #include <boost/format.hpp>
00005 
00006 #include <tf/transform_datatypes.h>
00007 
00008 #include <sensor_msgs/Image.h>
00009 #include <sensor_msgs/image_encodings.h>
00010 #include <sensor_msgs/distortion_models.h>
00011 
00012 #include <visp/vpImage.h>
00013 #include <visp/vpTranslationVector.h>
00014 #include <visp/vpQuaternionVector.h>
00015 
00016 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
00017 # define protected public
00018 #endif
00019 # include <visp/vpMbEdgeTracker.h>
00020 # include <visp/vpMbKltTracker.h>
00021 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
00022 # undef protected
00023 #endif
00024 
00025 #include "conversion.hh"
00026 
00027 void rosImageToVisp(vpImage<unsigned char>& dst,
00028                     const sensor_msgs::Image::ConstPtr& src)
00029 {
00030   using sensor_msgs::image_encodings::RGB8;
00031   using sensor_msgs::image_encodings::RGBA8;
00032   using sensor_msgs::image_encodings::BGR8;
00033   using sensor_msgs::image_encodings::BGRA8;
00034   using sensor_msgs::image_encodings::MONO8;
00035   using sensor_msgs::image_encodings::MONO16;
00036   using sensor_msgs::image_encodings::numChannels;
00037 
00038   // Resize the image if necessary.
00039   if (src->width != dst.getWidth() || src->height != dst.getHeight())
00040   {
00041     ROS_INFO("dst is %dx%d but src size is %dx%d, resizing.",
00042              dst.getWidth (), dst.getHeight (), src->width, src->height);
00043     dst.resize (src->height, src->width);
00044   }
00045 
00046   if(src->encoding == MONO8) {
00047     memcpy(dst.bitmap, &src->data[0], dst.getHeight () * src->step * sizeof(unsigned char));
00048   }
00049   else if(src->encoding == RGB8 || src->encoding == RGBA8
00050           || src->encoding == BGR8 || src->encoding == BGRA8)
00051   {
00052     unsigned nc = numChannels(src->encoding);
00053     unsigned cEnd =
00054         (src->encoding == RGBA8 || src->encoding == BGRA8) ? nc - 1 : nc;
00055 
00056     for(unsigned i = 0; i < dst.getWidth (); ++i)
00057       for(unsigned j = 0; j < dst.getHeight (); ++j)
00058       {
00059         int acc = 0;
00060         for(unsigned c = 0; c < cEnd; ++c)
00061           acc += src->data[j * src->step + i * nc + c];
00062         dst[j][i] =  acc / nc;
00063       }
00064   }
00065   else
00066   {
00067     boost::format fmt("bad encoding '%1'");
00068     fmt % src->encoding;
00069     throw std::runtime_error(fmt.str());
00070   }
00071 }
00072 
00073 void vispImageToRos(sensor_msgs::Image& dst,
00074                     const vpImage<unsigned char>& src)
00075 {
00076   dst.width = src.getWidth();
00077   dst.height = src.getHeight();
00078   dst.encoding = sensor_msgs::image_encodings::MONO8;
00079   dst.step = src.getWidth();
00080   dst.data.resize(dst.height * dst.step);
00081   for(unsigned i = 0; i < src.getWidth (); ++i)
00082     for(unsigned j = 0; j < src.getHeight (); ++j)
00083       dst.data[j * dst.step + i] = src[j][i];
00084 }
00085 
00086 
00087 std::string convertVpMbTrackerToRosMessage(const vpMbTracker* tracker)
00088 {
00089   std::stringstream stream;
00090 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
00091   stream << "Model Based Tracker Common Setttings\n" <<
00092             " Angle for polygons apparition...." << vpMath::deg(tracker->getAngleAppear()) <<" degrees\n" <<
00093             " Angle for polygons disparition..." << vpMath::deg(tracker->getAngleDisappear()) << " degrees\n";
00094 #else
00095   const vpMbEdgeTracker* tracker_edge = dynamic_cast<const vpMbEdgeTracker*>(tracker);
00096   if (tracker_edge != NULL) {
00097     stream << "Model Based Tracker Common Setttings\n" <<
00098               " Angle for polygons apparition...." << vpMath::deg(tracker_edge->getAngleAppear()) <<" degrees\n" <<
00099               " Angle for polygons disparition..." << vpMath::deg(tracker_edge->getAngleDisappear()) << " degrees\n";
00100   }
00101   else {
00102     const vpMbKltTracker* tracker_klt = dynamic_cast<const vpMbKltTracker*>(tracker);
00103     if (tracker_klt != NULL) {
00104       stream << "Model Based Tracker Common Setttings\n" <<
00105                 " Angle for polygons apparition...." << vpMath::deg(tracker_klt->getAngleAppear()) <<" degrees\n" <<
00106                 " Angle for polygons disparition..." << vpMath::deg(tracker_klt->getAngleDisappear()) << " degrees\n";
00107     }
00108   }
00109 #endif
00110   return stream.str();
00111 }
00112 
00113 std::string convertVpMeToRosMessage(const vpMbTracker* tracker, const vpMe& moving_edge)
00114 {
00115   const vpMbEdgeTracker* t = dynamic_cast<const vpMbEdgeTracker*>(tracker);
00116   std::stringstream stream;
00117   stream  << "Moving Edge Setttings\n" <<
00118              " Size of the convolution masks...." << moving_edge.getMaskSize() <<"x"<< moving_edge.getMaskSize() <<" pixels\n" <<
00119              " Query range +/- J................" << moving_edge.getRange() <<" pixels\n" <<
00120              " Likelihood test ratio............" << moving_edge.getThreshold() << "\n" <<
00121              " Contrast tolerance +/-..........." << moving_edge.getMu1() * 100 << "% and " << moving_edge.getMu2() * 100 << "% \n" <<
00122              " Sample step......................" << moving_edge.getSampleStep() <<" pixels\n" <<
00123              " Strip............................" << moving_edge.getStrip() << " pixels\n";
00124 
00125 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
00126   stream  << " Good moving edge threshold......." << t->getGoodMovingEdgesRatioThreshold()*100 << "%\n";
00127 #else
00128   stream  << " Good moving edge threshold......." << t->getFirstThreshold()*100 << "%\n";
00129 #endif
00130 
00131   return stream.str();
00132 }
00133 
00134 std::string convertVpKltOpencvToRosMessage(const vpMbTracker* tracker, const vpKltOpencv& klt)
00135 {
00136   const vpMbKltTracker* t = dynamic_cast<const vpMbKltTracker*>(tracker);
00137   std::stringstream stream;
00138   stream << "KLT Setttings\n" <<
00139             " Window size......................" << klt.getWindowSize() <<"x"<< klt.getWindowSize() <<" pixels\n" <<
00140             " Mask border......................" << t->getMaskBorder() << " pixels\n" <<
00141             " Maximum number of features......." << klt.getMaxFeatures() <<"\n" <<
00142             " Detected points quality.........." << klt.getQuality() << "\n" <<
00143             " Minimum distance between points.." << klt.getMinDistance() << " pixels\n" <<
00144             " Harris free parameter............" << klt.getHarrisFreeParameter() <<"\n" <<
00145             " Block size......................." << klt.getBlockSize() << "x" << klt.getBlockSize() << " pixels\n" <<
00146             " Number of pyramid levels........." << klt.getPyramidLevels() << "\n";
00147 
00148   return stream.str();
00149 }
00150 
00151 void vpHomogeneousMatrixToTransform(geometry_msgs::Transform& dst,
00152                                     const vpHomogeneousMatrix& src)
00153 {
00154   vpQuaternionVector quaternion;
00155   src.extract(quaternion);
00156 
00157   dst.translation.x = src[0][3];
00158   dst.translation.y = src[1][3];
00159   dst.translation.z = src[2][3];
00160 
00161   dst.rotation.x = quaternion.x();
00162   dst.rotation.y = quaternion.y();
00163   dst.rotation.z = quaternion.z();
00164   dst.rotation.w = quaternion.w();
00165 }
00166 
00167 void transformToVpHomogeneousMatrix(vpHomogeneousMatrix& dst,
00168                                     const geometry_msgs::Transform& src)
00169 {
00170   vpTranslationVector translation(src.translation.x,src.translation.y,src.translation.z);
00171   vpQuaternionVector quaternion(src.rotation.x,src.rotation.y,src.rotation.z,src.rotation.w);
00172   dst.buildFrom(translation, quaternion);
00173 }
00174 
00175 void transformToVpHomogeneousMatrix(vpHomogeneousMatrix& dst,
00176                                     const geometry_msgs::Pose& src)
00177 {
00178   vpQuaternionVector quaternion
00179     (src.orientation.x, src.orientation.y, src.orientation.z,
00180      src.orientation.w);
00181   vpRotationMatrix rotation(quaternion);
00182 
00183   // Copy the rotation component.
00184   for(unsigned i = 0; i < 3; ++i)
00185     for(unsigned j = 0; j < 3; ++j)
00186       dst[i][j] = rotation[i][j];
00187 
00188   // Copy the translation component.
00189   dst[0][3] = src.position.x;
00190   dst[1][3] = src.position.y;
00191   dst[2][3] = src.position.z;
00192 }
00193 
00194 void transformToVpHomogeneousMatrix(vpHomogeneousMatrix& dst,
00195                                     const tf::Transform& src)
00196 {
00197   // Copy the rotation component.
00198   for(unsigned i = 0; i < 3; ++i)
00199     for(unsigned j = 0; j < 3; ++j)
00200       dst[i][j] = src.getBasis ()[i][j];
00201 
00202   // Copy the translation component.
00203   for (unsigned i = 0; i < 3; ++i)
00204     dst[i][3] = src.getOrigin ()[i];
00205   dst[3][3] = 1.;
00206 }
00207 
00208 void convertVpMbTrackerToInitRequest(const vpMbTracker* tracker,
00209                                      visp_tracker::Init& srv)
00210 {
00211 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
00212   srv.request.tracker_param.angle_appear = vpMath::deg(tracker->getAngleAppear());
00213   srv.request.tracker_param.angle_disappear = vpMath::deg(tracker->getAngleDisappear());
00214 #else
00215   const vpMbEdgeTracker* tracker_edge = dynamic_cast<const vpMbEdgeTracker*>(tracker);
00216   if (tracker_edge != NULL) {
00217     ROS_INFO("Set service angle from edge");
00218     srv.request.tracker_param.angle_appear = vpMath::deg(tracker_edge->getAngleAppear());
00219     srv.request.tracker_param.angle_disappear = vpMath::deg(tracker_edge->getAngleDisappear());
00220   }
00221   else {
00222     const vpMbKltTracker* tracker_klt = dynamic_cast<const vpMbKltTracker*>(tracker);
00223     if (tracker_klt != NULL) {
00224       ROS_INFO("Set service angle from klt");
00225       srv.request.tracker_param.angle_appear = vpMath::deg(tracker_klt->getAngleAppear());
00226       srv.request.tracker_param.angle_disappear = vpMath::deg(tracker_klt->getAngleDisappear());
00227     }
00228   }
00229 #endif
00230 }
00231 
00232 void convertInitRequestToVpMbTracker(const visp_tracker::Init::Request& req,
00233                                      vpMbTracker* tracker)
00234 {
00235 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
00236   tracker->setAngleAppear(vpMath::rad(req.tracker_param.angle_appear));
00237   tracker->setAngleDisappear(vpMath::rad(req.tracker_param.angle_disappear));
00238 #else
00239   vpMbEdgeTracker* tracker_edge = dynamic_cast<vpMbEdgeTracker*>(tracker);
00240   if (tracker_edge != NULL) { // Also valid for hybrid
00241     tracker_edge->setAngleAppear(vpMath::rad(req.tracker_param.angle_appear));
00242     tracker_edge->setAngleDisappear(vpMath::rad(req.tracker_param.angle_disappear));
00243   }
00244   else {
00245     vpMbKltTracker* tracker_klt = dynamic_cast<vpMbKltTracker*>(tracker);
00246     if (tracker_klt != NULL) {
00247       tracker_klt->setAngleAppear(vpMath::rad(req.tracker_param.angle_appear));
00248       tracker_klt->setAngleDisappear(vpMath::rad(req.tracker_param.angle_disappear));
00249     }
00250   }
00251 #endif
00252 }
00253 
00254 void convertVpMeToInitRequest(const vpMe& moving_edge,
00255                               const vpMbTracker* tracker,
00256                               visp_tracker::Init& srv)
00257 {
00258   const vpMbEdgeTracker* t = dynamic_cast<const vpMbEdgeTracker*>(tracker);
00259   
00260 
00261 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
00262   srv.request.moving_edge.first_threshold = t->getGoodMovingEdgesRatioThreshold();
00263   srv.request.moving_edge.mask_size = moving_edge.getMaskSize();
00264   srv.request.moving_edge.range = moving_edge.getRange();
00265   srv.request.moving_edge.threshold = moving_edge.getThreshold();
00266   srv.request.moving_edge.mu1 = moving_edge.getMu1();
00267   srv.request.moving_edge.mu2 = moving_edge.getMu2();
00268   srv.request.moving_edge.sample_step = moving_edge.getSampleStep();
00269   srv.request.moving_edge.strip = moving_edge.getStrip();
00270 #else
00271   srv.request.moving_edge.first_threshold = t->getFirstThreshold();
00272   srv.request.moving_edge.mask_size = moving_edge.mask_size;
00273   srv.request.moving_edge.range = moving_edge.range;
00274   srv.request.moving_edge.threshold = moving_edge.threshold;
00275   srv.request.moving_edge.mu1 = moving_edge.mu1;
00276   srv.request.moving_edge.mu2 = moving_edge.mu2;
00277   srv.request.moving_edge.sample_step = moving_edge.sample_step;
00278   srv.request.moving_edge.strip = moving_edge.strip;
00279 #endif
00280 }
00281 
00282 void convertInitRequestToVpMe(const visp_tracker::Init::Request& req,
00283                               vpMbTracker* tracker,
00284                               vpMe& moving_edge)
00285 {
00286   vpMbEdgeTracker* t = dynamic_cast<vpMbEdgeTracker*>(tracker);
00287   
00288 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
00289   t->setGoodMovingEdgesRatioThreshold(req.moving_edge.first_threshold);
00290   moving_edge.setMaskSize( req.moving_edge.mask_size );
00291   moving_edge.setRange( req.moving_edge.range );
00292   moving_edge.setThreshold( req.moving_edge.threshold );
00293   moving_edge.setMu1( req.moving_edge.mu1 );
00294   moving_edge.setMu2( req.moving_edge.mu2 );
00295   moving_edge.setSampleStep( req.moving_edge.sample_step );
00296   moving_edge.setStrip( req.moving_edge.strip );
00297 #else
00298   t->setFirstThreshold(req.moving_edge.first_threshold);
00299   moving_edge.mask_size = req.moving_edge.mask_size;
00300   moving_edge.range = req.moving_edge.range;
00301   moving_edge.threshold = req.moving_edge.threshold;
00302   moving_edge.mu1 = req.moving_edge.mu1;
00303   moving_edge.mu2 = req.moving_edge.mu2;
00304   moving_edge.sample_step = req.moving_edge.sample_step;
00305   moving_edge.strip = req.moving_edge.strip;
00306 #endif
00307 
00308   //FIXME: not sure if this is needed.
00309   moving_edge.initMask();
00310   //Reset the tracker and the node state.
00311   t->setMovingEdge(moving_edge);
00312 }
00313 
00314 void convertVpKltOpencvToInitRequest(const vpKltOpencv& klt,
00315             const vpMbTracker* tracker,
00316             visp_tracker::Init& srv)
00317 {
00318   const vpMbKltTracker* t = dynamic_cast<const vpMbKltTracker*>(tracker);
00319   
00320   srv.request.klt_param.max_features = klt.getMaxFeatures();
00321   srv.request.klt_param.window_size = klt.getWindowSize();
00322   srv.request.klt_param.quality = klt.getQuality();
00323   srv.request.klt_param.min_distance = klt.getMinDistance();
00324   srv.request.klt_param.harris = klt.getHarrisFreeParameter();
00325   srv.request.klt_param.size_block = klt.getBlockSize();
00326   srv.request.klt_param.pyramid_lvl = klt.getPyramidLevels();
00327   srv.request.klt_param.mask_border = t->getMaskBorder();
00328 }
00329 
00330 void convertInitRequestToVpKltOpencv(const visp_tracker::Init::Request& req,
00331             vpMbTracker* tracker,
00332             vpKltOpencv& klt)
00333 {
00334   vpMbKltTracker* t = dynamic_cast<vpMbKltTracker*>(tracker);
00335   
00336   klt.setMaxFeatures(req.klt_param.max_features);
00337   klt.setWindowSize(req.klt_param.window_size);
00338   klt.setQuality(req.klt_param.quality);
00339   klt.setMinDistance(req.klt_param.min_distance);
00340   klt.setHarrisFreeParameter(req.klt_param.harris);
00341   klt.setBlockSize(req.klt_param.size_block);
00342   klt.setPyramidLevels(req.klt_param.pyramid_lvl);
00343   t->setMaskBorder((unsigned)req.klt_param.mask_border);
00344 
00345   t->setKltOpencv(klt);
00346 }
00347 
00348 void initializeVpCameraFromCameraInfo(vpCameraParameters& cam,
00349                                       sensor_msgs::CameraInfoConstPtr info)
00350 {
00351   if (!info)
00352     throw std::runtime_error ("missing camera calibration data");
00353 
00354   // Check that the camera is calibrated, as specified in the
00355   // sensor_msgs/CameraInfo message documentation.
00356   if (info->K.size() != 3 * 3 || info->K[0] == 0.)
00357     throw std::runtime_error ("uncalibrated camera");
00358 
00359   // Check matrix size.
00360   if (!info || info->P.size() != 3 * 4)
00361     throw std::runtime_error
00362       ("camera calibration P matrix has an incorrect size");
00363 
00364   if (info->distortion_model.empty ())
00365     {
00366       const double& px = info->K[0 * 3 + 0];
00367       const double& py = info->K[1 * 3 + 1];
00368       const double& u0 = info->K[0 * 3 + 2];
00369       const double& v0 = info->K[1 * 3 + 2];
00370       cam.initPersProjWithoutDistortion(px, py, u0, v0);
00371       return;
00372     }
00373 
00374   if (info->distortion_model == sensor_msgs::distortion_models::PLUMB_BOB)
00375     {
00376       const double& px = info->P[0 * 4 + 0];
00377       const double& py = info->P[1 * 4 + 1];
00378       const double& u0 = info->P[0 * 4 + 2];
00379       const double& v0 = info->P[1 * 4 + 2];
00380       cam.initPersProjWithoutDistortion(px, py, u0, v0);
00381       return;
00382     }
00383 
00384   throw std::runtime_error ("unsupported distortion model");
00385 }


visp_tracker
Author(s): Thomas Moulard
autogenerated on Sun Feb 19 2017 03:28:46