conversion.hh
Go to the documentation of this file.
00001 #ifndef VISP_TRACKER_CONVERSION_HH
00002 # define VISP_TRACKER_CONVERSION_HH
00003 # include <boost/optional.hpp>
00004 
00005 # include <ros/ros.h>
00006 
00007 # include <geometry_msgs/Transform.h>
00008 # include <sensor_msgs/Image.h>
00009 # include <sensor_msgs/CameraInfo.h>
00010 # include <tf/transform_datatypes.h>
00011 
00012 # include <visp_tracker/Init.h>
00013 
00014 #include <visp/vpConfig.h>
00015 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
00016 # define protected public
00017 #endif
00018 # include <visp/vpMbEdgeTracker.h>
00019 # include <visp/vpMbKltTracker.h>
00020 # include <visp/vpMbTracker.h>
00021 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
00022 # undef protected
00023 #endif
00024 
00025 # include <visp/vpHomogeneousMatrix.h>
00026 # include <visp/vpCameraParameters.h>
00027 # include <visp/vpMe.h>
00028 # include <visp/vpKltOpencv.h>
00029 
00040 void rosImageToVisp(vpImage<unsigned char>& dst,
00041                     const sensor_msgs::Image::ConstPtr& src);
00042 
00053 void vispImageToRos(sensor_msgs::Image& dst,
00054                     const vpImage<unsigned char>& src);
00055 
00056 std::string convertVpMbTrackerToRosMessage(const vpMbTracker* tracker);
00057 
00058 std::string convertVpMeToRosMessage(const vpMbTracker* tracker, const vpMe& moving_edge);
00059 
00060 std::string convertVpKltOpencvToRosMessage(const vpMbTracker* tracker, const vpKltOpencv& klt);
00061 
00062 void vpHomogeneousMatrixToTransform(geometry_msgs::Transform& dst,
00063                                     const vpHomogeneousMatrix& src);
00064 
00065 void transformToVpHomogeneousMatrix(vpHomogeneousMatrix& dst,
00066                                     const geometry_msgs::Transform& src);
00067 
00068 void transformToVpHomogeneousMatrix(vpHomogeneousMatrix& dst,
00069                                     const tf::Transform& src);
00070 
00071 void transformToVpHomogeneousMatrix(vpHomogeneousMatrix& dst,
00072                                     const geometry_msgs::Pose& src);
00073 
00074 void convertVpMbTrackerToInitRequest(const vpMbTracker* tracker,
00075             visp_tracker::Init& srv);
00076 
00077 void convertInitRequestToVpMbTracker(const visp_tracker::Init::Request& req,
00078             vpMbTracker* tracker);
00079 
00080 void convertVpMeToInitRequest(const vpMe& moving_edge,
00081                               const vpMbTracker* tracker,
00082                               visp_tracker::Init& srv);
00083 
00084 void convertInitRequestToVpMe(const visp_tracker::Init::Request& req,
00085                               vpMbTracker* tracker,
00086                               vpMe& moving_edge);
00087 
00088 void convertVpKltOpencvToInitRequest(const vpKltOpencv& klt,
00089             const vpMbTracker* tracker,
00090             visp_tracker::Init& srv);
00091 
00092 void convertInitRequestToVpKltOpencv(const visp_tracker::Init::Request& req,
00093             vpMbTracker* tracker,
00094             vpKltOpencv& klt);
00095 
00096 void initializeVpCameraFromCameraInfo(vpCameraParameters& cam,
00097               sensor_msgs::CameraInfoConstPtr info);
00098 
00099 // Dynamic reconfigure template functions
00100 template<class ConfigType>
00101 void convertModelBasedSettingsConfigToVpMbTracker(const ConfigType& config,
00102                                                   vpMbTracker* tracker)
00103 {
00104 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
00105   tracker->setAngleAppear(vpMath::rad(config.angle_appear));
00106   tracker->setAngleDisappear(vpMath::rad(config.angle_disappear));
00107 #else
00108   vpMbEdgeTracker* tracker_edge = dynamic_cast<vpMbEdgeTracker*>(tracker);
00109   if (tracker_edge != NULL) { // Also valid when hybrid
00110     ROS_INFO("Set param angle from edge");
00111     tracker_edge->setAngleAppear(vpMath::rad(config.angle_appear));
00112     tracker_edge->setAngleDisappear(vpMath::rad(config.angle_disappear));
00113   }
00114   else {
00115     vpMbKltTracker* tracker_klt = dynamic_cast<vpMbKltTracker*>(tracker);
00116     if (tracker_klt != NULL) {
00117       ROS_INFO("Set param angle from klt");
00118       tracker_klt->setAngleAppear(vpMath::rad(config.angle_appear));
00119       tracker_klt->setAngleDisappear(vpMath::rad(config.angle_disappear));
00120     }
00121   }
00122 #endif
00123 }
00124 
00125 template<class ConfigType>
00126 void convertVpMbTrackerToModelBasedSettingsConfig(const vpMbTracker* tracker,
00127                                                   ConfigType& config)
00128 {
00129 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
00130   config.angle_appear = vpMath::deg(tracker->getAngleAppear());
00131   config.angle_disappear = vpMath::deg(tracker->getAngleDisappear());
00132 #else
00133   const vpMbEdgeTracker* tracker_edge = dynamic_cast<const vpMbEdgeTracker*>(tracker);
00134   if (tracker_edge != NULL) {
00135     ROS_INFO("Modif config param angle from edge");
00136     config.angle_appear = vpMath::deg(tracker_edge->getAngleAppear());
00137     config.angle_disappear = vpMath::deg(tracker_edge->getAngleDisappear());
00138   }
00139   else {
00140     const vpMbKltTracker* tracker_klt = dynamic_cast<const vpMbKltTracker*>(tracker);
00141     if (tracker_klt != NULL) {
00142       ROS_INFO("Modif config param angle from klt");
00143       config.angle_appear = vpMath::deg(tracker_klt->getAngleAppear());
00144       config.angle_disappear = vpMath::deg(tracker_klt->getAngleDisappear());
00145     }
00146   }
00147 #endif
00148 }
00149 
00150 template<class ConfigType>
00151 void convertModelBasedSettingsConfigToVpMe(const ConfigType& config,
00152            vpMe& moving_edge,
00153            vpMbTracker* tracker)
00154 {
00155   vpMbEdgeTracker* t = dynamic_cast<vpMbEdgeTracker*>(tracker);
00156 
00157 
00158 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
00159   t->setGoodMovingEdgesRatioThreshold(config.first_threshold);
00160   moving_edge.setThreshold( config.threshold );
00161   moving_edge.setMaskSize( config.mask_size );
00162   moving_edge.setRange( config.range );
00163   moving_edge.setMu1( config.mu1 );
00164   moving_edge.setMu2( config.mu2 );
00165   moving_edge.setSampleStep( config.sample_step );
00166   moving_edge.setStrip( config.strip );
00167 #else
00168   t->setFirstThreshold(config.first_threshold);
00169   moving_edge.threshold = config.threshold;
00170   moving_edge.mask_size = config.mask_size;
00171   moving_edge.range = config.range;
00172   moving_edge.mu1 = config.mu1;
00173   moving_edge.mu2 = config.mu2;
00174   moving_edge.sample_step = config.sample_step;
00175   moving_edge.strip = config.strip;
00176 #endif
00177 
00178   //FIXME: not sure if this is needed.
00179   moving_edge.initMask();
00180   //Reset the tracker and the node state.
00181   t->setMovingEdge(moving_edge);
00182 }
00183 
00184 template<class ConfigType>
00185 void convertVpMeToModelBasedSettingsConfig(const vpMe& moving_edge,
00186            const vpMbTracker* tracker,
00187            ConfigType& config)
00188 {
00189   const vpMbEdgeTracker* t = dynamic_cast<const vpMbEdgeTracker*>(tracker);
00190 
00191 
00192 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
00193   config.first_threshold = t->getGoodMovingEdgesRatioThreshold();
00194   config.threshold = moving_edge.getThreshold();
00195   config.mask_size = moving_edge.getMaskSize();
00196   config.range = moving_edge.getRange();
00197   config.mu1 = moving_edge.getMu1();
00198   config.mu2 = moving_edge.getMu2();
00199   config.sample_step = moving_edge.getSampleStep();
00200   config.strip = moving_edge.getStrip();
00201 #else
00202   config.first_threshold = t->getFirstThreshold();
00203   config.threshold = moving_edge.threshold;
00204   config.mask_size = moving_edge.mask_size;
00205   config.range = moving_edge.range;
00206   config.mu1 = moving_edge.mu1;
00207   config.mu2 = moving_edge.mu2;
00208   config.sample_step = moving_edge.sample_step;
00209   config.strip = moving_edge.strip;
00210 #endif
00211 }
00212 
00213 template<class ConfigType>
00214 void convertModelBasedSettingsConfigToVpKltOpencv(const ConfigType& config,
00215            vpKltOpencv& klt,
00216            vpMbTracker* tracker)
00217 {
00218   vpMbKltTracker* t = dynamic_cast<vpMbKltTracker*>(tracker);
00219 
00220   klt.setMaxFeatures(config.max_features);
00221   klt.setWindowSize(config.window_size);
00222   klt.setQuality(config.quality);
00223   klt.setMinDistance(config.min_distance);
00224   klt.setHarrisFreeParameter(config.harris);
00225   klt.setBlockSize(config.size_block);
00226   klt.setPyramidLevels(config.pyramid_lvl);
00227   t->setMaskBorder((unsigned)config.mask_border);
00228 
00229   t->setKltOpencv(klt);
00230 }
00231 
00232 template<class ConfigType>
00233 void convertVpKltOpencvToModelBasedSettingsConfig(const vpKltOpencv& klt,
00234            const vpMbTracker* tracker,
00235            ConfigType& config)
00236 {
00237   const vpMbKltTracker* t = dynamic_cast<const vpMbKltTracker*>(tracker);
00238 
00239   config.max_features = klt.getMaxFeatures();
00240   config.window_size = klt.getWindowSize();
00241   config.quality = klt.getQuality();
00242   config.min_distance = klt.getMinDistance();
00243   config.harris = klt.getHarrisFreeParameter();
00244   config.size_block = klt.getBlockSize();
00245   config.pyramid_lvl = klt.getPyramidLevels();
00246   config.mask_border = t->getMaskBorder();
00247 }
00248 
00249 #endif //! VISP_TRACKER_CONVERSION_HH


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