conversion.cpp
Go to the documentation of this file.
1 #include <cstring>
2 #include <stdexcept>
3 
4 #include <boost/format.hpp>
5 
7 
8 #include <sensor_msgs/Image.h>
11 
12 #include <visp/vpImage.h>
13 #include <visp/vpTranslationVector.h>
14 #include <visp/vpQuaternionVector.h>
15 
16 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
17 # define protected public
18 #endif
19 # include <visp/vpMbEdgeTracker.h>
20 # include <visp/vpMbKltTracker.h>
21 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0)
22 # undef protected
23 #endif
24 
25 #include "conversion.hh"
26 
27 void rosImageToVisp(vpImage<unsigned char>& dst,
28  const sensor_msgs::Image::ConstPtr& src)
29 {
37 
38  // Resize the image if necessary.
39  if (src->width != dst.getWidth() || src->height != dst.getHeight())
40  {
41  ROS_INFO("dst is %dx%d but src size is %dx%d, resizing.",
42  dst.getWidth (), dst.getHeight (), src->width, src->height);
43  dst.resize (src->height, src->width);
44  }
45 
46  if(src->encoding == MONO8) {
47  memcpy(dst.bitmap, &src->data[0], dst.getHeight () * src->step * sizeof(unsigned char));
48  }
49  else if(src->encoding == RGB8 || src->encoding == RGBA8
50  || src->encoding == BGR8 || src->encoding == BGRA8)
51  {
52  unsigned nc = numChannels(src->encoding);
53  unsigned cEnd =
54  (src->encoding == RGBA8 || src->encoding == BGRA8) ? nc - 1 : nc;
55 
56  for(unsigned i = 0; i < dst.getWidth (); ++i)
57  for(unsigned j = 0; j < dst.getHeight (); ++j)
58  {
59  int acc = 0;
60  for(unsigned c = 0; c < cEnd; ++c)
61  acc += src->data[j * src->step + i * nc + c];
62  dst[j][i] = acc / nc;
63  }
64  }
65  else
66  {
67  boost::format fmt("bad encoding '%1'");
68  fmt % src->encoding;
69  throw std::runtime_error(fmt.str());
70  }
71 }
72 
73 void vispImageToRos(sensor_msgs::Image& dst,
74  const vpImage<unsigned char>& src)
75 {
76  dst.width = src.getWidth();
77  dst.height = src.getHeight();
79  dst.step = src.getWidth();
80  dst.data.resize(dst.height * dst.step);
81  for(unsigned i = 0; i < src.getWidth (); ++i)
82  for(unsigned j = 0; j < src.getHeight (); ++j)
83  dst.data[j * dst.step + i] = src[j][i];
84 }
85 
86 
87 std::string convertVpMbTrackerToRosMessage(const vpMbTracker* tracker)
88 {
89  std::stringstream stream;
90 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
91  stream << "Model Based Tracker Common Setttings\n" <<
92  " Angle for polygons apparition...." << vpMath::deg(tracker->getAngleAppear()) <<" degrees\n" <<
93  " Angle for polygons disparition..." << vpMath::deg(tracker->getAngleDisappear()) << " degrees\n";
94 #else
95  const vpMbEdgeTracker* tracker_edge = dynamic_cast<const vpMbEdgeTracker*>(tracker);
96  if (tracker_edge != NULL) {
97  stream << "Model Based Tracker Common Setttings\n" <<
98  " Angle for polygons apparition...." << vpMath::deg(tracker_edge->getAngleAppear()) <<" degrees\n" <<
99  " Angle for polygons disparition..." << vpMath::deg(tracker_edge->getAngleDisappear()) << " degrees\n";
100  }
101  else {
102  const vpMbKltTracker* tracker_klt = dynamic_cast<const vpMbKltTracker*>(tracker);
103  if (tracker_klt != NULL) {
104  stream << "Model Based Tracker Common Setttings\n" <<
105  " Angle for polygons apparition...." << vpMath::deg(tracker_klt->getAngleAppear()) <<" degrees\n" <<
106  " Angle for polygons disparition..." << vpMath::deg(tracker_klt->getAngleDisappear()) << " degrees\n";
107  }
108  }
109 #endif
110  return stream.str();
111 }
112 
113 std::string convertVpMeToRosMessage(const vpMbTracker* tracker, const vpMe& moving_edge)
114 {
115  const vpMbEdgeTracker* t = dynamic_cast<const vpMbEdgeTracker*>(tracker);
116  std::stringstream stream;
117  stream << "Moving Edge Setttings\n" <<
118  " Size of the convolution masks...." << moving_edge.getMaskSize() <<"x"<< moving_edge.getMaskSize() <<" pixels\n" <<
119  " Query range +/- J................" << moving_edge.getRange() <<" pixels\n" <<
120  " Likelihood test ratio............" << moving_edge.getThreshold() << "\n" <<
121  " Contrast tolerance +/-..........." << moving_edge.getMu1() * 100 << "% and " << moving_edge.getMu2() * 100 << "% \n" <<
122  " Sample step......................" << moving_edge.getSampleStep() <<" pixels\n" <<
123  " Strip............................" << moving_edge.getStrip() << " pixels\n";
124 
125 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
126  stream << " Good moving edge threshold......." << t->getGoodMovingEdgesRatioThreshold()*100 << "%\n";
127 #else
128  stream << " Good moving edge threshold......." << t->getFirstThreshold()*100 << "%\n";
129 #endif
130 
131  return stream.str();
132 }
133 
134 std::string convertVpKltOpencvToRosMessage(const vpMbTracker* tracker, const vpKltOpencv& klt)
135 {
136  const vpMbKltTracker* t = dynamic_cast<const vpMbKltTracker*>(tracker);
137  std::stringstream stream;
138  stream << "KLT Setttings\n" <<
139  " Window size......................" << klt.getWindowSize() <<"x"<< klt.getWindowSize() <<" pixels\n" <<
140  " Mask border......................" << t->getMaskBorder() << " pixels\n" <<
141  " Maximum number of features......." << klt.getMaxFeatures() <<"\n" <<
142  " Detected points quality.........." << klt.getQuality() << "\n" <<
143  " Minimum distance between points.." << klt.getMinDistance() << " pixels\n" <<
144  " Harris free parameter............" << klt.getHarrisFreeParameter() <<"\n" <<
145  " Block size......................." << klt.getBlockSize() << "x" << klt.getBlockSize() << " pixels\n" <<
146  " Number of pyramid levels........." << klt.getPyramidLevels() << "\n";
147 
148  return stream.str();
149 }
150 
151 void vpHomogeneousMatrixToTransform(geometry_msgs::Transform& dst,
152  const vpHomogeneousMatrix& src)
153 {
154  vpQuaternionVector quaternion;
155  src.extract(quaternion);
156 
157  dst.translation.x = src[0][3];
158  dst.translation.y = src[1][3];
159  dst.translation.z = src[2][3];
160 
161  dst.rotation.x = quaternion.x();
162  dst.rotation.y = quaternion.y();
163  dst.rotation.z = quaternion.z();
164  dst.rotation.w = quaternion.w();
165 }
166 
167 void transformToVpHomogeneousMatrix(vpHomogeneousMatrix& dst,
168  const geometry_msgs::Transform& src)
169 {
170  vpTranslationVector translation(src.translation.x,src.translation.y,src.translation.z);
171  vpQuaternionVector quaternion(src.rotation.x,src.rotation.y,src.rotation.z,src.rotation.w);
172  dst.buildFrom(translation, quaternion);
173 }
174 
175 void transformToVpHomogeneousMatrix(vpHomogeneousMatrix& dst,
176  const geometry_msgs::Pose& src)
177 {
178  vpQuaternionVector quaternion
179  (src.orientation.x, src.orientation.y, src.orientation.z,
180  src.orientation.w);
181  vpRotationMatrix rotation(quaternion);
182 
183  // Copy the rotation component.
184  for(unsigned i = 0; i < 3; ++i)
185  for(unsigned j = 0; j < 3; ++j)
186  dst[i][j] = rotation[i][j];
187 
188  // Copy the translation component.
189  dst[0][3] = src.position.x;
190  dst[1][3] = src.position.y;
191  dst[2][3] = src.position.z;
192 }
193 
194 void transformToVpHomogeneousMatrix(vpHomogeneousMatrix& dst,
195  const tf::Transform& src)
196 {
197  // Copy the rotation component.
198  for(unsigned i = 0; i < 3; ++i)
199  for(unsigned j = 0; j < 3; ++j)
200  dst[i][j] = src.getBasis ()[i][j];
201 
202  // Copy the translation component.
203  for (unsigned i = 0; i < 3; ++i)
204  dst[i][3] = src.getOrigin ()[i];
205  dst[3][3] = 1.;
206 }
207 
208 void convertVpMbTrackerToInitRequest(const vpMbTracker* tracker,
209  visp_tracker::Init& srv)
210 {
211 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
212  srv.request.tracker_param.angle_appear = vpMath::deg(tracker->getAngleAppear());
213  srv.request.tracker_param.angle_disappear = vpMath::deg(tracker->getAngleDisappear());
214 #else
215  const vpMbEdgeTracker* tracker_edge = dynamic_cast<const vpMbEdgeTracker*>(tracker);
216  if (tracker_edge != NULL) {
217  ROS_INFO("Set service angle from edge");
218  srv.request.tracker_param.angle_appear = vpMath::deg(tracker_edge->getAngleAppear());
219  srv.request.tracker_param.angle_disappear = vpMath::deg(tracker_edge->getAngleDisappear());
220  }
221  else {
222  const vpMbKltTracker* tracker_klt = dynamic_cast<const vpMbKltTracker*>(tracker);
223  if (tracker_klt != NULL) {
224  ROS_INFO("Set service angle from klt");
225  srv.request.tracker_param.angle_appear = vpMath::deg(tracker_klt->getAngleAppear());
226  srv.request.tracker_param.angle_disappear = vpMath::deg(tracker_klt->getAngleDisappear());
227  }
228  }
229 #endif
230 }
231 
232 void convertInitRequestToVpMbTracker(const visp_tracker::Init::Request& req,
233  vpMbTracker* tracker)
234 {
235 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
236  tracker->setAngleAppear(vpMath::rad(req.tracker_param.angle_appear));
237  tracker->setAngleDisappear(vpMath::rad(req.tracker_param.angle_disappear));
238 #else
239  vpMbEdgeTracker* tracker_edge = dynamic_cast<vpMbEdgeTracker*>(tracker);
240  if (tracker_edge != NULL) { // Also valid for hybrid
241  tracker_edge->setAngleAppear(vpMath::rad(req.tracker_param.angle_appear));
242  tracker_edge->setAngleDisappear(vpMath::rad(req.tracker_param.angle_disappear));
243  }
244  else {
245  vpMbKltTracker* tracker_klt = dynamic_cast<vpMbKltTracker*>(tracker);
246  if (tracker_klt != NULL) {
247  tracker_klt->setAngleAppear(vpMath::rad(req.tracker_param.angle_appear));
248  tracker_klt->setAngleDisappear(vpMath::rad(req.tracker_param.angle_disappear));
249  }
250  }
251 #endif
252 }
253 
254 void convertVpMeToInitRequest(const vpMe& moving_edge,
255  const vpMbTracker* tracker,
256  visp_tracker::Init& srv)
257 {
258  const vpMbEdgeTracker* t = dynamic_cast<const vpMbEdgeTracker*>(tracker);
259 
260 
261 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
262  srv.request.moving_edge.first_threshold = t->getGoodMovingEdgesRatioThreshold();
263  srv.request.moving_edge.mask_size = moving_edge.getMaskSize();
264  srv.request.moving_edge.range = moving_edge.getRange();
265  srv.request.moving_edge.threshold = moving_edge.getThreshold();
266  srv.request.moving_edge.mu1 = moving_edge.getMu1();
267  srv.request.moving_edge.mu2 = moving_edge.getMu2();
268  srv.request.moving_edge.sample_step = moving_edge.getSampleStep();
269  srv.request.moving_edge.strip = moving_edge.getStrip();
270 #else
271  srv.request.moving_edge.first_threshold = t->getFirstThreshold();
272  srv.request.moving_edge.mask_size = moving_edge.mask_size;
273  srv.request.moving_edge.range = moving_edge.range;
274  srv.request.moving_edge.threshold = moving_edge.threshold;
275  srv.request.moving_edge.mu1 = moving_edge.mu1;
276  srv.request.moving_edge.mu2 = moving_edge.mu2;
277  srv.request.moving_edge.sample_step = moving_edge.sample_step;
278  srv.request.moving_edge.strip = moving_edge.strip;
279 #endif
280 }
281 
282 void convertInitRequestToVpMe(const visp_tracker::Init::Request& req,
283  vpMbTracker* tracker,
284  vpMe& moving_edge)
285 {
286  vpMbEdgeTracker* t = dynamic_cast<vpMbEdgeTracker*>(tracker);
287 
288 #if VISP_VERSION_INT >= VP_VERSION_INT(2,10,0)
289  t->setGoodMovingEdgesRatioThreshold(req.moving_edge.first_threshold);
290  moving_edge.setMaskSize( req.moving_edge.mask_size );
291  moving_edge.setRange( req.moving_edge.range );
292  moving_edge.setThreshold( req.moving_edge.threshold );
293  moving_edge.setMu1( req.moving_edge.mu1 );
294  moving_edge.setMu2( req.moving_edge.mu2 );
295  moving_edge.setSampleStep( req.moving_edge.sample_step );
296  moving_edge.setStrip( req.moving_edge.strip );
297 #else
298  t->setFirstThreshold(req.moving_edge.first_threshold);
299  moving_edge.mask_size = req.moving_edge.mask_size;
300  moving_edge.range = req.moving_edge.range;
301  moving_edge.threshold = req.moving_edge.threshold;
302  moving_edge.mu1 = req.moving_edge.mu1;
303  moving_edge.mu2 = req.moving_edge.mu2;
304  moving_edge.sample_step = req.moving_edge.sample_step;
305  moving_edge.strip = req.moving_edge.strip;
306 #endif
307 
308  //FIXME: not sure if this is needed.
309  moving_edge.initMask();
310  //Reset the tracker and the node state.
311  t->setMovingEdge(moving_edge);
312 }
313 
314 void convertVpKltOpencvToInitRequest(const vpKltOpencv& klt,
315  const vpMbTracker* tracker,
316  visp_tracker::Init& srv)
317 {
318  const vpMbKltTracker* t = dynamic_cast<const vpMbKltTracker*>(tracker);
319 
320  srv.request.klt_param.max_features = klt.getMaxFeatures();
321  srv.request.klt_param.window_size = klt.getWindowSize();
322  srv.request.klt_param.quality = klt.getQuality();
323  srv.request.klt_param.min_distance = klt.getMinDistance();
324  srv.request.klt_param.harris = klt.getHarrisFreeParameter();
325  srv.request.klt_param.size_block = klt.getBlockSize();
326  srv.request.klt_param.pyramid_lvl = klt.getPyramidLevels();
327  srv.request.klt_param.mask_border = t->getMaskBorder();
328 }
329 
330 void convertInitRequestToVpKltOpencv(const visp_tracker::Init::Request& req,
331  vpMbTracker* tracker,
332  vpKltOpencv& klt)
333 {
334  vpMbKltTracker* t = dynamic_cast<vpMbKltTracker*>(tracker);
335 
336  klt.setMaxFeatures(req.klt_param.max_features);
337  klt.setWindowSize(req.klt_param.window_size);
338  klt.setQuality(req.klt_param.quality);
339  klt.setMinDistance(req.klt_param.min_distance);
340  klt.setHarrisFreeParameter(req.klt_param.harris);
341  klt.setBlockSize(req.klt_param.size_block);
342  klt.setPyramidLevels(req.klt_param.pyramid_lvl);
343  t->setMaskBorder((unsigned)req.klt_param.mask_border);
344 
345  t->setKltOpencv(klt);
346 }
347 
348 void initializeVpCameraFromCameraInfo(vpCameraParameters& cam,
349  sensor_msgs::CameraInfoConstPtr info)
350 {
351  if (!info)
352  throw std::runtime_error ("missing camera calibration data");
353 
354  // Check that the camera is calibrated, as specified in the
355  // sensor_msgs/CameraInfo message documentation.
356  if (info->K.size() != 3 * 3 || info->K[0] == 0.)
357  throw std::runtime_error ("uncalibrated camera");
358 
359  // Check matrix size.
360  if (!info || info->P.size() != 3 * 4)
361  throw std::runtime_error
362  ("camera calibration P matrix has an incorrect size");
363 
364  if (info->distortion_model.empty ())
365  {
366  const double& px = info->K[0 * 3 + 0];
367  const double& py = info->K[1 * 3 + 1];
368  const double& u0 = info->K[0 * 3 + 2];
369  const double& v0 = info->K[1 * 3 + 2];
370  cam.initPersProjWithoutDistortion(px, py, u0, v0);
371  return;
372  }
373 
374  if (info->distortion_model == sensor_msgs::distortion_models::PLUMB_BOB)
375  {
376  const double& px = info->P[0 * 4 + 0];
377  const double& py = info->P[1 * 4 + 1];
378  const double& u0 = info->P[0 * 4 + 2];
379  const double& v0 = info->P[1 * 4 + 2];
380  cam.initPersProjWithoutDistortion(px, py, u0, v0);
381  return;
382  }
383 
384  throw std::runtime_error ("unsupported distortion model");
385 }
std::string convertVpMeToRosMessage(const vpMbTracker *tracker, const vpMe &moving_edge)
Definition: conversion.cpp:113
void transformToVpHomogeneousMatrix(vpHomogeneousMatrix &dst, const geometry_msgs::Transform &src)
Definition: conversion.cpp:167
void convertVpMeToInitRequest(const vpMe &moving_edge, const vpMbTracker *tracker, visp_tracker::Init &srv)
Definition: conversion.cpp:254
void vispImageToRos(sensor_msgs::Image &dst, const vpImage< unsigned char > &src)
Convert a ViSP image into a ROS one.
Definition: conversion.cpp:73
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
std::string convertVpKltOpencvToRosMessage(const vpMbTracker *tracker, const vpKltOpencv &klt)
Definition: conversion.cpp:134
void convertInitRequestToVpMbTracker(const visp_tracker::Init::Request &req, vpMbTracker *tracker)
Definition: conversion.cpp:232
void convertVpMbTrackerToInitRequest(const vpMbTracker *tracker, visp_tracker::Init &srv)
Definition: conversion.cpp:208
void convertInitRequestToVpKltOpencv(const visp_tracker::Init::Request &req, vpMbTracker *tracker, vpKltOpencv &klt)
Definition: conversion.cpp:330
void convertInitRequestToVpMe(const visp_tracker::Init::Request &req, vpMbTracker *tracker, vpMe &moving_edge)
Definition: conversion.cpp:282
#define ROS_INFO(...)
const std::string MONO16
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
static int numChannels(const std::string &encoding)
void convertVpKltOpencvToInitRequest(const vpKltOpencv &klt, const vpMbTracker *tracker, visp_tracker::Init &srv)
Definition: conversion.cpp:314
void initializeVpCameraFromCameraInfo(vpCameraParameters &cam, sensor_msgs::CameraInfoConstPtr info)
Definition: conversion.cpp:348
void vpHomogeneousMatrixToTransform(geometry_msgs::Transform &dst, const vpHomogeneousMatrix &src)
Definition: conversion.cpp:151
std::string convertVpMbTrackerToRosMessage(const vpMbTracker *tracker)
Definition: conversion.cpp:87
void rosImageToVisp(vpImage< unsigned char > &dst, const sensor_msgs::Image::ConstPtr &src)
Convert a ROS image into a ViSP one.
Definition: conversion.cpp:27


visp_tracker
Author(s): Thomas Moulard
autogenerated on Wed Jul 3 2019 19:48:07