MRPT  2.0.1
stereo_image.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /*---------------------------------------------------------------
11  APPLICATION: mrpt_ros bridge
12  FILE: stereo_image.cpp
13  AUTHOR: Raghavender Sahdev <raghavendersahdev@gmail.com>
14  ---------------------------------------------------------------*/
15 
16 #include <cv_bridge/cv_bridge.h>
17 #include <mrpt/ros1bridge/image.h>
19 #include <sensor_msgs/Image.h>
20 #include <sensor_msgs/image_encodings.h>
21 
22 using namespace ros;
23 using namespace sensor_msgs;
24 using namespace cv;
25 using namespace cv_bridge;
26 
27 namespace mrpt::ros1bridge
28 {
29 bool toROS(
31  const std_msgs::Header& msg_header, sensor_msgs::Image& left,
32  sensor_msgs::Image& right, stereo_msgs::DisparityImage& disparity)
33 {
34  // left image
35  const Mat& cvImgL = obj.imageLeft.asCvMatRef();
36 
37  cv_bridge::CvImage img_bridge;
38  img_bridge =
39  CvImage(left.header, sensor_msgs::image_encodings::BGR8, cvImgL);
40  img_bridge.toImageMsg(left);
41  left.encoding = "bgr8";
42  left.header = msg_header;
43  left.height = obj.imageLeft.getHeight();
44  left.width = obj.imageLeft.getWidth();
45 
46  // right image
47  const Mat& cvImgR = obj.imageLeft.asCvMatRef();
48 
49  cv_bridge::CvImage img_bridge2;
50  img_bridge2 =
51  CvImage(right.header, sensor_msgs::image_encodings::BGR8, cvImgR);
52  img_bridge2.toImageMsg(right);
53  right.encoding = "bgr8";
54  right.header = msg_header;
55  right.height = obj.imageRight.getHeight();
56  right.width = obj.imageRight.getWidth();
57 
58  if (obj.hasImageDisparity)
59  {
60  const Mat& cvImgD = obj.imageDisparity.asCvMatRef();
61 
62  cv_bridge::CvImage img_bridge3;
63  img_bridge3 = CvImage(
64  disparity.header, sensor_msgs::image_encodings::BGR8, cvImgD);
65  img_bridge3.toImageMsg(disparity.image);
66  disparity.image.encoding = "bgr8";
67  disparity.image.header = msg_header;
68  disparity.image.height = obj.imageDisparity.getHeight();
69  disparity.image.width = obj.imageDisparity.getWidth();
70  }
71  return true;
72 }
73 } // namespace mrpt::ros1bridge
74 
75 //
76 /*
77 std_msgs/Header header
78 uint32 height
79 uint32 width
80 string encoding
81 uint8 is_bigendian
82 uint32 step
83 uint8[] data
84  */
mrpt::img::CImage imageDisparity
Disparity image, only contains a valid image if hasImageDisparity == true.
mrpt::img::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
cv::Mat & asCvMatRef()
Get a reference to the internal cv::Mat, which can be resized, etc.
Definition: CImage.cpp:227
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:849
bool hasImageDisparity
Whether imageDisparity actually contains data (Default upon construction: false)
Definition: img/CImage.h:23
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:818
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
bool toROS(const mrpt::obs::CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Convert mrpt::obs::CObservationGPS -> sensor_msgs/NavSatFix The user must supply the "msg_header" fie...
Definition: gps.cpp:48
ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html MRPT message: https://github...
Definition: gps.h:28
mrpt::img::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020