image_utilities.cpp
Go to the documentation of this file.
1 // Do not change the order of this block. Otherwise getBinaryData with CV::mat overload will not be recognized
2 #include <cv_bridge/cv_bridge.h>
4 
5 #include <string>
6 #include <utility>
7 #include <vector>
8 
9 #include <ros/ros.h>
12 
13 double const NXLIB_TIMESTAMP_OFFSET = 11644473600;
14 
15 std::string imageEncoding(bool isFloat, int channels, int bytesPerElement)
16 {
17  if (isFloat)
18  {
19  switch (channels)
20  {
21  case 1:
22  return (bytesPerElement == 4) ? sensor_msgs::image_encodings::TYPE_32FC1 :
24  case 2:
25  return (bytesPerElement == 4) ? sensor_msgs::image_encodings::TYPE_32FC2 :
27  case 3:
28  return (bytesPerElement == 4) ? sensor_msgs::image_encodings::TYPE_32FC3 :
30  case 4:
31  return (bytesPerElement == 4) ? sensor_msgs::image_encodings::TYPE_32FC4 :
33  }
34  }
35  else
36  {
37  switch (channels)
38  {
39  case 1:
40  switch (bytesPerElement)
41  {
42  case 1:
44  case 2:
46  case 4:
48  }
49  break;
50  case 2:
51  switch (bytesPerElement)
52  {
53  case 1:
55  case 2:
57  case 4:
59  }
60  break;
61  case 3:
62  switch (bytesPerElement)
63  {
64  case 1:
66  case 2:
68  case 4:
70  }
71  break;
72  case 4:
73  switch (bytesPerElement)
74  {
75  case 1:
77  case 2:
79  case 4:
81  }
82  break;
83  }
84  }
85 
86  ROS_ERROR("Invalid image encoding in binary NxLib node.");
87  return "";
88 }
89 
90 sensor_msgs::ImagePtr imageFromNxLibNode(NxLibItem const& node, std::string const& frame)
91 {
92  auto image = boost::make_shared<sensor_msgs::Image>();
93 
94  bool isFloat;
95  int width, height, channels, bytesPerElement;
96  double timestamp;
97  node.getBinaryDataInfo(&width, &height, &channels, &bytesPerElement, &isFloat, &timestamp);
98 
99  image->header.stamp.fromSec(timestamp - NXLIB_TIMESTAMP_OFFSET);
100  image->header.frame_id = frame;
101  image->width = width;
102  image->height = height;
103  image->step = width * channels * bytesPerElement;
104  image->encoding = imageEncoding(isFloat, channels, bytesPerElement);
105 
106  node.getBinaryData(image->data, 0);
107 
108  return image;
109 }
110 
111 std::pair<sensor_msgs::ImagePtr, sensor_msgs::ImagePtr> imagePairFromNxLibNode(const NxLibItem& node,
112  std::string const& frame)
113 {
114  auto leftImage = imageFromNxLibNode(node[itmLeft], frame);
115  auto rightImage = imageFromNxLibNode(node[itmRight], frame);
116 
117  return { leftImage, rightImage };
118 }
119 
120 std::vector<std::pair<sensor_msgs::ImagePtr, sensor_msgs::ImagePtr>> imagePairsFromNxLibNode(const NxLibItem& node,
121  std::string const& frame)
122 {
123  std::vector<std::pair<sensor_msgs::ImagePtr, sensor_msgs::ImagePtr>> result;
124 
125  if (node.isArray())
126  {
127  for (int i = 0; i < node.count(); i++)
128  {
129  result.push_back(imagePairFromNxLibNode(node[i], frame));
130  }
131  }
132  else
133  {
134  result.push_back(imagePairFromNxLibNode(node, frame));
135  }
136 
137  return result;
138 }
139 
140 std::vector<sensor_msgs::ImagePtr> imagesFromNxLibNode(NxLibItem const& node, std::string const& frame)
141 {
142  std::vector<sensor_msgs::ImagePtr> result;
143 
144  if (node.isArray())
145  {
146  for (int i = 0; i < node.count(); i++)
147  {
148  result.push_back(imageFromNxLibNode(node[i], frame));
149  }
150  }
151  else
152  {
153  result.push_back(imageFromNxLibNode(node, frame));
154  }
155 
156  return result;
157 }
158 
159 ros::Time timestampFromNxLibNode(const NxLibItem& node)
160 {
161  double timestamp;
162  node.getBinaryDataInfo(0, 0, 0, 0, 0, &timestamp);
163 
164  return ros::Time(timestamp - NXLIB_TIMESTAMP_OFFSET);
165 }
166 
167 sensor_msgs::ImagePtr depthImageFromNxLibNode(NxLibItem const& node, std::string const& frame)
168 {
169  double timestamp;
170  cv::Mat pointMap;
171  node.getBinaryData(pointMap, &timestamp);
172 
173  cv::Mat depthImage;
174  cv::extractChannel(pointMap, depthImage, 2);
175 
176  // convert cv mat to ros image
177  cv_bridge::CvImage out_msg;
178  out_msg.header.stamp.fromSec(timestamp - NXLIB_TIMESTAMP_OFFSET);
179  out_msg.header.frame_id = frame;
181  out_msg.image = depthImage;
182 
183  return out_msg.toImageMsg();
184 }
185 
186 void fillDistortionParamsFromNxLib(NxLibItem const& distortionItem, sensor_msgs::CameraInfoPtr const& info)
187 {
188  std::vector<double> distParams(5, 0.);
189  bool isPlumbModel = info->distortion_model == sensor_msgs::distortion_models::PLUMB_BOB;
190  if (!isPlumbModel)
191  {
192  info->D = distParams;
193  return;
194  }
195 
196  auto getNxLibValue = [](NxLibItem const& itemToCheck) -> double {
197  double value = 0.;
198  try
199  {
200  value = itemToCheck.asDouble();
201  }
202  catch (...)
203  {
204  ROS_WARN("The distortion parameter %s does not exist. Using value 0.0 instead.", itemToCheck.path.c_str());
205  }
206  return value;
207  };
208 
209  if (distortionItem.isObject())
210  {
211  distParams[0] = getNxLibValue(distortionItem[itmK1]);
212  distParams[1] = getNxLibValue(distortionItem[itmK2]);
213  distParams[2] = getNxLibValue(distortionItem[itmT1]);
214  distParams[3] = getNxLibValue(distortionItem[itmT2]);
215  distParams[4] = getNxLibValue(distortionItem[itmK3]);
216  }
217  else if (distortionItem.isArray())
218  {
219  for (int i = 0; i < 5; i++)
220  {
221  info->D.push_back(getNxLibValue(distortionItem[i]));
222  }
223  }
224  info->D = distParams;
225 }
const std::string TYPE_16UC4
sensor_msgs::ImagePtr imageFromNxLibNode(NxLibItem const &node, std::string const &frame)
const std::string TYPE_64FC3
const std::string TYPE_8UC4
ros::Time timestampFromNxLibNode(const NxLibItem &node)
std::vector< sensor_msgs::ImagePtr > imagesFromNxLibNode(NxLibItem const &node, std::string const &frame)
double const NXLIB_TIMESTAMP_OFFSET
const std::string TYPE_32FC4
std::string encoding
sensor_msgs::ImagePtr depthImageFromNxLibNode(NxLibItem const &node, std::string const &frame)
#define ROS_WARN(...)
std::string imageEncoding(bool isFloat, int channels, int bytesPerElement)
std::pair< sensor_msgs::ImagePtr, sensor_msgs::ImagePtr > imagePairFromNxLibNode(const NxLibItem &node, std::string const &frame)
void fillDistortionParamsFromNxLib(NxLibItem const &distortionItem, sensor_msgs::CameraInfoPtr const &info)
const std::string TYPE_32FC1
const std::string TYPE_8UC2
const std::string MONO16
const std::string TYPE_64FC4
const std::string TYPE_16UC3
const std::string TYPE_16UC2
const std::string TYPE_32SC2
const std::string TYPE_64FC1
const std::string TYPE_64FC2
const std::string TYPE_32SC3
const std::string TYPE_8UC3
const std::string TYPE_32SC4
const std::string TYPE_32FC3
const std::string TYPE_32FC2
#define ROS_ERROR(...)
std::vector< std::pair< sensor_msgs::ImagePtr, sensor_msgs::ImagePtr > > imagePairsFromNxLibNode(const NxLibItem &node, std::string const &frame)
const std::string TYPE_32SC1
sensor_msgs::ImagePtr toImageMsg() const
std_msgs::Header header


ensenso_camera
Author(s): Ensenso
autogenerated on Sat Jul 27 2019 03:51:24