ImageConverter.cpp
Go to the documentation of this file.
1 
2 #include <depthai_bridge/ImageConverter.hpp>
3 
4 namespace dai {
5 
6 namespace ros {
7 
8 std::unordered_map<dai::RawImgFrame::Type, std::string> ImageConverter::encodingEnumMap = {{dai::RawImgFrame::Type::YUV422i, "yuv422"},
9  {dai::RawImgFrame::Type::RGBA8888, "rgba8"},
10  {dai::RawImgFrame::Type::RGB888i, "rgb8"},
11  {dai::RawImgFrame::Type::BGR888i, "bgr8"},
12  {dai::RawImgFrame::Type::GRAY8, "mono8"},
13  {dai::RawImgFrame::Type::RAW8, "mono8"},
14  {dai::RawImgFrame::Type::RAW16, "16UC1"},
15  {dai::RawImgFrame::Type::YUV420p, "YUV420"}};
16 // TODO(sachin) : Move Planare to encodingEnumMap and use default planar namings. And convertt those that are not supported in ROS using ImageTransport in the
17 // bridge.
18 std::unordered_map<dai::RawImgFrame::Type, std::string> ImageConverter::planarEncodingEnumMap = {
19  {dai::RawImgFrame::Type::BGR888p, "rgb8"}, // 3_1_bgr8 represents 3 planes/channels and 1 byte per pixel in BGR format
20  {dai::RawImgFrame::Type::RGB888p, "rgb8"},
21  {dai::RawImgFrame::Type::NV12, "rgb8"},
22  {dai::RawImgFrame::Type::YUV420p, "rgb8"}};
23 
24 ImageConverter::ImageConverter(bool interleaved) : _daiInterleaved(interleaved) {}
25 
26 ImageConverter::ImageConverter(const std::string frameName, bool interleaved) : _frameName(frameName), _daiInterleaved(interleaved) {}
27 
28 void ImageConverter::toRosMsg(std::shared_ptr<dai::ImgFrame> inData, std::deque<ImageMsgs::Image>& outImageMsgs) {
29  auto tstamp = inData->getTimestamp();
30  ImageMsgs::Image outImageMsg;
31  StdMsgs::Header header;
32  header.frame_id = _frameName;
33 
34 #ifdef IS_ROS2
35  auto rclNow = rclcpp::Clock().now();
36  auto steadyTime = std::chrono::steady_clock::now();
37  auto diffTime = steadyTime - tstamp;
38  auto rclStamp = rclNow - diffTime;
39  header.stamp = rclStamp;
40 #else
41  auto rosNow = ::ros::Time::now();
42  auto steadyTime = std::chrono::steady_clock::now();
43  auto diffTime = steadyTime - tstamp;
44  uint64_t nsec = rosNow.toNSec() - diffTime.count();
45  auto rosStamp = rosNow.fromNSec(nsec);
46  header.stamp = rosStamp;
47  header.seq = inData->getSequenceNum();
48 #endif
49 
50  if(planarEncodingEnumMap.find(inData->getType()) != planarEncodingEnumMap.end()) {
51  // cv::Mat inImg = inData->getCvFrame();
52  cv::Mat mat, output;
53  cv::Size size = {0, 0};
54  int type = 0;
55  switch(inData->getType()) {
56  case dai::RawImgFrame::Type::BGR888p:
57  case dai::RawImgFrame::Type::RGB888p:
58  size = cv::Size(inData->getWidth(), inData->getHeight());
59  type = CV_8UC3;
60  break;
61  case dai::RawImgFrame::Type::YUV420p:
62  case dai::RawImgFrame::Type::NV12:
63  size = cv::Size(inData->getWidth(), inData->getHeight() * 3 / 2);
64  type = CV_8UC1;
65  break;
66 
67  default:
68  std::runtime_error("Invalid dataType inputs..");
69  break;
70  }
71  mat = cv::Mat(size, type, inData->getData().data());
72 
73  switch(inData->getType()) {
74  case dai::RawImgFrame::Type::RGB888p: {
75  cv::Size s(inData->getWidth(), inData->getHeight());
76  std::vector<cv::Mat> channels;
77  // RGB
78  channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 2));
79  channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 1));
80  channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 0));
81  cv::merge(channels, output);
82  } break;
83 
84  case dai::RawImgFrame::Type::BGR888p: {
85  cv::Size s(inData->getWidth(), inData->getHeight());
86  std::vector<cv::Mat> channels;
87  // BGR
88  channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 0));
89  channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 1));
90  channels.push_back(cv::Mat(s, CV_8UC1, inData->getData().data() + s.area() * 2));
91  cv::merge(channels, output);
92  } break;
93 
94  case dai::RawImgFrame::Type::YUV420p:
95  cv::cvtColor(mat, output, cv::ColorConversionCodes::COLOR_YUV2BGR_IYUV);
96  break;
97 
98  case dai::RawImgFrame::Type::NV12:
99  cv::cvtColor(mat, output, cv::ColorConversionCodes::COLOR_YUV2BGR_NV12);
100  break;
101 
102  default:
103  output = mat.clone();
104  break;
105  }
106  cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, output).toImageMsg(outImageMsg);
107 
108  } else if(encodingEnumMap.find(inData->getType()) != encodingEnumMap.end()) {
109  // copying the data to ros msg
110  outImageMsg.header = header;
111  std::string temp_str(encodingEnumMap[inData->getType()]);
112  outImageMsg.encoding = temp_str;
113  outImageMsg.height = inData->getHeight();
114  outImageMsg.width = inData->getWidth();
115  outImageMsg.step = inData->getData().size() / inData->getHeight();
116  if(outImageMsg.encoding == "16UC1")
117  outImageMsg.is_bigendian = false;
118  else
119  outImageMsg.is_bigendian = true;
120 
121  size_t size = inData->getData().size();
122  outImageMsg.data.resize(size);
123  unsigned char* imageMsgDataPtr = reinterpret_cast<unsigned char*>(&outImageMsg.data[0]);
124  unsigned char* daiImgData = reinterpret_cast<unsigned char*>(inData->getData().data());
125 
126  // TODO(Sachin): Try using assign since it is a vector
127  // img->data.assign(packet.data->cbegin(), packet.data->cend());
128  memcpy(imageMsgDataPtr, daiImgData, size);
129  }
130  outImageMsgs.push_back(outImageMsg);
131  return;
132 }
133 
134 // TODO(sachin): Not tested
135 void ImageConverter::toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData) {
136  std::unordered_map<dai::RawImgFrame::Type, std::string>::iterator revEncodingIter;
137  if(_daiInterleaved) {
138  revEncodingIter = std::find_if(encodingEnumMap.begin(), encodingEnumMap.end(), [&](const std::pair<dai::RawImgFrame::Type, std::string>& pair) {
139  return pair.second == inMsg.encoding;
140  });
141  if(revEncodingIter == encodingEnumMap.end())
142  std::runtime_error(
143  "Unable to find DAI encoding for the corresponding "
144  "sensor_msgs::image.encoding stream");
145 
146  outData.setData(inMsg.data);
147  } else {
148  revEncodingIter = std::find_if(encodingEnumMap.begin(), encodingEnumMap.end(), [&](const std::pair<dai::RawImgFrame::Type, std::string>& pair) {
149  return pair.second.find(inMsg.encoding) != std::string::npos;
150  });
151 
152  std::istringstream f(revEncodingIter->second);
153  std::vector<std::string> encoding_info;
154  std::string s;
155 
156  while(getline(f, s, '_')) encoding_info.push_back(s);
157 
158  std::vector<std::uint8_t> opData(inMsg.data.size());
159  interleavedToPlanar(inMsg.data, opData, inMsg.height, inMsg.width, std::stoi(encoding_info[0]), std::stoi(encoding_info[1]));
160  outData.setData(opData);
161  }
162 
168  /* #ifdef IS_ROS2
169  TimePoint ts(std::chrono::seconds((int)inMsg.header.stamp.seconds ()) + std::chrono::nanoseconds(inMsg.header.stamp.nanoseconds()));
170  #else
171  TimePoint ts(std::chrono::seconds((int)inMsg.header.stamp.toSec()) + std::chrono::nanoseconds(inMsg.header.stamp.toNSec()));
172  #endif
173 
174  outData.setTimestamp(ts);
175  outData.setSequenceNum(inMsg.header.seq); */
176  outData.setWidth(inMsg.width);
177  outData.setHeight(inMsg.height);
178  outData.setType(revEncodingIter->first);
179 }
180 
181 ImagePtr ImageConverter::toRosMsgPtr(std::shared_ptr<dai::ImgFrame> inData) {
182  std::deque<ImageMsgs::Image> msgQueue;
183  toRosMsg(inData, msgQueue);
184  auto msg = msgQueue.front();
185 
186 #ifdef IS_ROS2
187  ImagePtr ptr = std::make_shared<ImageMsgs::Image>(msg);
188 #else
189  ImagePtr ptr = boost::make_shared<ImageMsgs::Image>(msg);
190 #endif
191  return ptr;
192 }
193 
194 void ImageConverter::planarToInterleaved(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp) {
195  if(numPlanes == 3) {
196  // optimization (cache)
197  for(int i = 0; i < w * h; i++) {
198  uint8_t b = srcData.data()[i + w * h * 0];
199  destData[i * 3 + 0] = b;
200  }
201  for(int i = 0; i < w * h; i++) {
202  uint8_t g = srcData.data()[i + w * h * 1];
203  destData[i * 3 + 1] = g;
204  }
205  for(int i = 0; i < w * h; i++) {
206  uint8_t r = srcData.data()[i + w * h * 2];
207  destData[i * 3 + 2] = r;
208  }
209  } else {
210  std::runtime_error(
211  "If you encounter the scenario where you need this "
212  "please create an issue on github");
213  }
214  return;
215 }
216 
217 void ImageConverter::interleavedToPlanar(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp) {
218  if(numPlanes == 3) {
219  // optimization (cache)
220  for(int i = 0; i < w * h; i++) {
221  uint8_t b = srcData[i * 3 + 0];
222  uint8_t g = srcData[i * 3 + 1];
223  uint8_t r = srcData[i * 3 + 2];
224 
225  destData[i + w * h * 0] = b;
226  destData[i + w * h * 1] = g;
227  destData[i + w * h * 2] = r;
228  }
229  // for(int i = 0; i < w*h; i++) {
230  // uint8_t g = srcData.data()[i + w*h * 1];
231  // destData[i*3+1] = g;
232  // }
233  // for(int i = 0; i < w*h; i++) {
234  // uint8_t r = srcData.data()[i + w*h * 2];
235  // destData[i*3+2] = r;
236  // }
237  } else {
238  std::runtime_error(
239  "If you encounter the scenario where you need this "
240  "please create an issue on github");
241  }
242  return;
243 }
244 
245 cv::Mat ImageConverter::rosMsgtoCvMat(ImageMsgs::Image& inMsg) {
246  cv::Mat rgb(inMsg.height, inMsg.width, CV_8UC3);
247  if(inMsg.encoding == "nv12") {
248  cv::Mat nv_frame(inMsg.height * 3 / 2, inMsg.width, CV_8UC1, inMsg.data.data());
249  cv::cvtColor(nv_frame, rgb, cv::COLOR_YUV2BGR_NV12);
250  return rgb;
251  } else {
252  std::runtime_error("This frature is still WIP");
253  return rgb;
254  }
255 }
256 
257 ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
258  dai::CalibrationHandler calibHandler, dai::CameraBoardSocket cameraId, int width, int height, Point2f topLeftPixelId, Point2f bottomRightPixelId) {
259  std::vector<std::vector<float>> camIntrinsics, rectifiedRotation;
260  std::vector<float> distCoeffs;
261  std::vector<double> flatIntrinsics, distCoeffsDouble;
262  int defWidth, defHeight;
263  ImageMsgs::CameraInfo cameraData;
264  std::tie(std::ignore, defWidth, defHeight) = calibHandler.getDefaultIntrinsics(cameraId);
265 
266  if(width == -1) {
267  cameraData.width = static_cast<uint32_t>(defWidth);
268  } else {
269  cameraData.width = static_cast<uint32_t>(width);
270  }
271 
272  if(height == -1) {
273  cameraData.height = static_cast<uint32_t>(defHeight);
274  } else {
275  cameraData.height = static_cast<uint32_t>(height);
276  }
277 
278  camIntrinsics = calibHandler.getCameraIntrinsics(cameraId, cameraData.width, cameraData.height, topLeftPixelId, bottomRightPixelId);
279 
280  flatIntrinsics.resize(9);
281  for(int i = 0; i < 3; i++) {
282  std::copy(camIntrinsics[i].begin(), camIntrinsics[i].end(), flatIntrinsics.begin() + 3 * i);
283  }
284 
285 #ifdef IS_ROS2
286  auto& intrinsics = cameraData.k;
287  auto& distortions = cameraData.d;
288  auto& projection = cameraData.p;
289  auto& rotation = cameraData.r;
290 #else
291  auto& intrinsics = cameraData.K;
292  auto& distortions = cameraData.D;
293  auto& projection = cameraData.P;
294  auto& rotation = cameraData.R;
295 #endif
296  // Set rotation to reasonable default even for non-stereo pairs
297  rotation[0] = rotation[4] = rotation[8] = 1;
298  for(size_t i = 0; i < 3; i++) {
299  std::copy(flatIntrinsics.begin() + i * 3, flatIntrinsics.begin() + (i + 1) * 3, projection.begin() + i * 4);
300  }
301  std::copy(flatIntrinsics.begin(), flatIntrinsics.end(), intrinsics.begin());
302 
303  distCoeffs = calibHandler.getDistortionCoefficients(cameraId);
304 
305  for(size_t i = 0; i < 8; i++) {
306  distortions.push_back(static_cast<double>(distCoeffs[i]));
307  }
308 
309  // Setting Projection matrix if the cameras are stereo pair. Right as the first and left as the second.
310  if(calibHandler.getStereoRightCameraId() != dai::CameraBoardSocket::AUTO && calibHandler.getStereoLeftCameraId() != dai::CameraBoardSocket::AUTO) {
311  if(calibHandler.getStereoRightCameraId() == cameraId || calibHandler.getStereoLeftCameraId() == cameraId) {
312  std::vector<std::vector<float>> stereoIntrinsics = calibHandler.getCameraIntrinsics(
313  calibHandler.getStereoRightCameraId(), cameraData.width, cameraData.height, topLeftPixelId, bottomRightPixelId);
314  std::vector<double> stereoFlatIntrinsics(12), flatRectifiedRotation(9);
315  for(int i = 0; i < 3; i++) {
316  std::copy(stereoIntrinsics[i].begin(), stereoIntrinsics[i].end(), stereoFlatIntrinsics.begin() + 4 * i);
317  stereoFlatIntrinsics[(4 * i) + 3] = 0;
318  }
319 
320  if(calibHandler.getStereoLeftCameraId() == cameraId) {
321  // This defines where the first camera is w.r.t second camera coordinate system giving it a translation to place all the points in the first
322  // camera to second camera by multiplying that translation vector using transformation function.
323  stereoFlatIntrinsics[3] = stereoFlatIntrinsics[0]
324  * calibHandler.getCameraExtrinsics(calibHandler.getStereoRightCameraId(), calibHandler.getStereoLeftCameraId())[0][3]
325  / 100.0; // Converting to meters
326  rectifiedRotation = calibHandler.getStereoLeftRectificationRotation();
327  } else {
328  rectifiedRotation = calibHandler.getStereoRightRectificationRotation();
329  }
330 
331  for(int i = 0; i < 3; i++) {
332  std::copy(rectifiedRotation[i].begin(), rectifiedRotation[i].end(), flatRectifiedRotation.begin() + 3 * i);
333  }
334 
335  std::copy(stereoFlatIntrinsics.begin(), stereoFlatIntrinsics.end(), projection.begin());
336  std::copy(flatRectifiedRotation.begin(), flatRectifiedRotation.end(), rotation.begin());
337  }
338  }
339  cameraData.distortion_model = "rational_polynomial";
340 
341  return cameraData;
342 }
343 
344 } // namespace ros
345 } // namespace dai
dai::ros::ImageConverter::planarEncodingEnumMap
static std::unordered_map< dai::RawImgFrame::Type, std::string > planarEncodingEnumMap
Definition: ImageConverter.hpp:70
dai::ros::ImageConverter::_frameName
const std::string _frameName
Definition: ImageConverter.hpp:75
sensor_msgs::image_encodings::BGR8
const std::string BGR8
f
f
dai::ros::ImageConverter::toRosMsgPtr
ImagePtr toRosMsgPtr(std::shared_ptr< dai::ImgFrame > inData)
Definition: ImageConverter.cpp:181
s
XmlRpcServer s
dai::ros::ImageConverter::ImageConverter
ImageConverter(const std::string frameName, bool interleaved)
Definition: ImageConverter.cpp:26
dai::ros::ImageConverter::rosMsgtoCvMat
cv::Mat rosMsgtoCvMat(ImageMsgs::Image &inMsg)
Definition: ImageConverter.cpp:245
dai::ros::ImageConverter::planarToInterleaved
void planarToInterleaved(const std::vector< uint8_t > &srcData, std::vector< uint8_t > &destData, int w, int h, int numPlanes, int bpp)
Definition: ImageConverter.cpp:194
cv_bridge::CvImage::toImageMsg
sensor_msgs::ImagePtr toImageMsg() const
dai::ros::ImageConverter::toDaiMsg
void toDaiMsg(const ImageMsgs::Image &inMsg, dai::ImgFrame &outData)
Definition: ImageConverter.cpp:135
dai::ros::ImageConverter::interleavedToPlanar
void interleavedToPlanar(const std::vector< uint8_t > &srcData, std::vector< uint8_t > &destData, int w, int h, int numPlanes, int bpp)
Definition: ImageConverter.cpp:217
cv_bridge::CvImage
dai::ros::ImagePtr
ImageMsgs::ImagePtr ImagePtr
Definition: BridgePublisher.hpp:45
ros::Time::now
static Time now()
dai::ros::ImageConverter::encodingEnumMap
static std::unordered_map< dai::RawImgFrame::Type, std::string > encodingEnumMap
Definition: ImageConverter.hpp:69
header
const std::string header
dai::ros::ImageConverter::calibrationToCameraInfo
ImageMsgs::CameraInfo calibrationToCameraInfo(dai::CalibrationHandler calibHandler, dai::CameraBoardSocket cameraId, int width=-1, int height=-1, Point2f topLeftPixelId=Point2f(), Point2f bottomRightPixelId=Point2f())
Definition: ImageConverter.cpp:257
dai::ros::ImageConverter::toRosMsg
void toRosMsg(std::shared_ptr< dai::ImgFrame > inData, std::deque< ImageMsgs::Image > &outImageMsgs)
Definition: ImageConverter.cpp:28

depthai_bridge
Author(s): Sachin Guruswamy
autogenerated on Tue May 10 2022 03:01:27