rgbd
multitool.cpp
Go to the documentation of this file.
1 #include <ros/console.h>
2 #include <ros/duration.h>
3 #include <ros/init.h>
4 #include <ros/master.h>
5 #include <ros/names.h>
6 #include <ros/node_handle.h>
7 #include <ros/rate.h>
8 #include <ros/time.h>
9 
10 #include "rgbd/client.h"
11 #include "rgbd/view.h"
12 
13 #include <opencv2/highgui/highgui.hpp>
14 
15 #include <memory>
16 
17 bool PAUSE = false;
18 std::string MODE;
19 
20 int IMAGE_WIDTH, IMAGE_HEIGHT;
21 
22 std::vector<cv::Vec2i> mouse_points;
23 cv::Vec2i mouse_pos;
24 
25 // ----------------------------------------------------------------------------------------------------
26 
27 void CallBackFunc(int event, int x, int y, int /*flags*/, void* /*userdata*/)
28 {
29  x = x % IMAGE_WIDTH;
30  mouse_pos = cv::Vec2i(x, y);
31 
32  if ( event == cv::EVENT_LBUTTONDOWN )
33  {
34  mouse_points.push_back(mouse_pos);
35  }
36  else if ( event == cv::EVENT_RBUTTONDOWN )
37  {
38  // std::cout << "Right button of the mouse is clicked - position (" << x << ", " << y << ")" << std::endl;
39  }
40  else if ( event == cv::EVENT_MBUTTONDOWN )
41  {
42  // std::cout << "Middle button of the mouse is clicked - position (" << x << ", " << y << ")" << std::endl;
43  }
44  else if ( event == cv::EVENT_MOUSEMOVE )
45  {
46 // mouse_pos = cv::Vec2i(x, y);
47  }
48 }
49 
50 // ----------------------------------------------------------------------------------------------------
51 
52 int main(int argc, char **argv)
53 {
54  ros::init(argc, argv, "rgbd_multitool", ros::init_options::AnonymousName);
55  ros::start();
56 
57  std::unique_ptr<rgbd::Client> client(nullptr);
58 
59  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
60 
61  // Parse command line arguments
62 
63  if (argc < 3)
64  {
65  std::cout << "Usage:" << std::endl
66  << std::endl
67  << " multitool --rgbd RGBD_TOPIC" << std::endl
68  << std::endl;
69  return 1;
70  }
71 
72  for(int i = 1; i < argc; i += 2)
73  {
74  std::string opt = argv[i];
75  std::string arg = argv[i + 1];
76 
77  if (opt == "--rgbd")
78  {
79  client = std::unique_ptr<rgbd::Client>(new rgbd::Client);
80  client->initialize(ros::names::resolve(arg));
81  }
82  else
83  {
84  std::cout << "Unknown option: '" << opt << "'." << std::endl;
85  return 1;
86  }
87  }
88 
89  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
90 
91  std::cout << "Keys:" << std::endl
92  << std::endl
93  << " spacebar - Pause" << std::endl
94  << " m - Measure" << std::endl
95  << " q - Quit" << std::endl
96  << std::endl;
97 
98  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
99 
100  const std::string window_name = "RGBD";
101  //Create a window
102  cv::namedWindow(window_name, 1);
103 
104  //set the callback function for any mouse event
105  cv::setMouseCallback(window_name, CallBackFunc, nullptr);
106 
107  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
108 
109  float max_view_distance = 10;
110 
111  rgbd::ImagePtr image;
112  ros::NodeHandle nh_private("~");
113 
114  float rate = 30;
115  nh_private.getParam("rate", rate);
116 
117  ros::WallTime last_master_check = ros::WallTime::now();
118 
119  ros::Rate r(rate);
120  while (ros::ok())
121  {
122  if (ros::WallTime::now() >= last_master_check + ros::WallDuration(1))
123  {
124  last_master_check = ros::WallTime::now();
125  if (!ros::master::check())
126  {
127  ROS_FATAL("Lost connection to master");
128  return 1;
129  }
130  }
131  if (!PAUSE && client)
132  {
133  rgbd::ImagePtr image_tmp = client->nextImage();
134  if (image_tmp)
135  image = image_tmp;
136  }
137 
138  cv::Mat canvas;
139  IMAGE_WIDTH = 0;
140  IMAGE_HEIGHT = 0;
141 
142  if (image)
143  {
144  const cv::Mat& rgb = image->getRGBImage();
145  const cv::Mat& depth = image->getDepthImage();
146 
147  if (depth.data)
148  {
149  cv::Mat depth_canvas(depth.rows, depth.cols, CV_8UC3, cv::Scalar(50, 0, 0));
150  for(int y = 0; y < depth.rows; ++y)
151  {
152  for(int x = 0; x < depth.cols; ++x)
153  {
154  float d = depth.at<float>(y, x);
155  if (d > 0 && d == d)
156  {
157  unsigned char v = static_cast<unsigned char>(std::min<float>(max_view_distance, d / max_view_distance) * 255);
158  depth_canvas.at<cv::Vec3b>(y, x) = cv::Vec3b(v, v, v);
159  }
160  }
161  }
162 
163  if (rgb.data)
164  {
165  IMAGE_WIDTH = std::min(rgb.cols, depth.cols);
166 
167  int rgb_height = IMAGE_WIDTH * rgb.rows / rgb.cols;
168  int depth_height = IMAGE_WIDTH * depth.rows / depth.cols;
169 
170  IMAGE_HEIGHT = std::max(rgb_height, depth_height);
171 
172  canvas = cv::Mat(IMAGE_HEIGHT, IMAGE_WIDTH * 2, CV_8UC3, cv::Scalar(50, 50, 50));
173 
174  cv::Mat rgb_roi = canvas(cv::Rect(cv::Point(0, 0), cv::Size(IMAGE_WIDTH, rgb_height)));
175  cv::Mat depth_roi = canvas(cv::Rect(cv::Point(IMAGE_WIDTH, 0), cv::Size(IMAGE_WIDTH, depth_height)));
176 
177  cv::resize(rgb, rgb_roi, cv::Size(IMAGE_WIDTH, rgb_height));
178  cv::resize(depth_canvas, depth_roi, cv::Size(IMAGE_WIDTH, depth_height));
179  }
180  else
181  {
182  canvas = depth_canvas;
183  }
184  }
185  else if (rgb.data)
186  {
187  canvas = rgb;
188  }
189  }
190 
191  if (!canvas.data)
192  {
193  canvas = cv::Mat(480, 640, CV_8UC3, cv::Scalar(50, 50, 50));
194  cv::line(canvas, cv::Point(0, 0), cv::Point(640, 480), cv::Scalar(255, 255, 255), 5);
195  cv::line(canvas, cv::Point(0, 480), cv::Point(640, 0), cv::Scalar(255, 255, 255), 5);
196  }
197 
198  if (IMAGE_WIDTH == 0 || IMAGE_HEIGHT == 0)
199  {
200  IMAGE_WIDTH = canvas.cols;
201  IMAGE_HEIGHT = canvas.rows;
202  }
203 
204  // Show mouse cursor(s)
205  cv::circle(canvas, mouse_pos, 5, cv::Scalar(255, 0, 0), 1);
206  if (canvas.cols > IMAGE_WIDTH)
207  cv::circle(canvas, mouse_pos + cv::Vec2i(IMAGE_WIDTH, 0), 5, cv::Scalar(255, 0, 0), 1);
208 
209  cv::putText(canvas, MODE, cv::Point(10, 20), cv::FONT_HERSHEY_COMPLEX_SMALL, 1, cv::Scalar(255, 255, 255), 1);
210 
211  if (PAUSE)
212  cv::putText(canvas, "PAUSED", cv::Point(10, canvas.rows - 25), cv::FONT_HERSHEY_COMPLEX_SMALL, 1, cv::Scalar(255, 255, 255), 1);
213 
214  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
215 
216  for(unsigned int i = 0; i < mouse_points.size(); ++i)
217  {
218  cv::circle(canvas, mouse_points[i], 5, cv::Scalar(0, 0, 255), 2);
219  if (canvas.cols > IMAGE_WIDTH)
220  cv::circle(canvas, mouse_points[i] + cv::Vec2i(IMAGE_WIDTH, 0), 5, cv::Scalar(0, 0, 255), 2);
221  }
222 
223  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
224  if (MODE == "DONE")
225  {
226  break;
227  }
228  else if (MODE == "MEASURE")
229  {
230  if (mouse_points.size() == 2)
231  {
232  rgbd::View view(*image, 640);
233 
234  geo::Vector3 p1, p2;
235 
236  if (view.getPoint3D(mouse_points[0][0], mouse_points[0][1], p1) && view.getPoint3D(mouse_points[1][0], mouse_points[1][1], p2))
237  {
238  std::cout << (p1 - p2).length() << " m" << std::endl;
239  }
240 
241  mouse_points.clear();
242  }
243  }
244  else
245  {
246  mouse_points.clear();
247  }
248 
249  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
250 
251  cv::imshow(window_name, canvas);
252  int i_key = cv::waitKey(3);
253  if (i_key >= 0)
254  {
255  char key = static_cast<char>(i_key);
256 
257  switch (key)
258  {
259  case ' ': PAUSE = !PAUSE;
260  break;
261  case 'm': MODE = MODE == "MEASURE" ? "" : "MEASURE";
262  break;
263  case 'q': MODE="DONE";
264  break;
265  default: MODE = "";
266  break;
267  }
268  }
269 
270  r.sleep();
271  }
272 
273  return 0;
274 }
std::string
std::shared_ptr
PAUSE
bool PAUSE
Definition: multitool.cpp:17
std::vector
MODE
std::string MODE
Definition: multitool.cpp:18
main
int main(int argc, char **argv)
Definition: multitool.cpp:52
IMAGE_WIDTH
int IMAGE_WIDTH
Definition: multitool.cpp:20
std::cout
mouse_pos
cv::Vec2i mouse_pos
Definition: multitool.cpp:23
IMAGE_HEIGHT
int IMAGE_HEIGHT
Definition: multitool.cpp:20
rgbd::View::getPoint3D
bool getPoint3D(int x, int y, geo::Vector3 &p) const
Definition: view.h:37
mouse_points
std::vector< cv::Vec2i > mouse_points
Definition: multitool.cpp:22
geo::Vector3
CallBackFunc
void CallBackFunc(int event, int x, int y, int, void *)
Definition: multitool.cpp:27
memory
std::min
T min(T... args)
std::endl
T endl(T... args)
view.h
rgbd::Client
Client which uses the interfaces of ClientRGBD and ClientSHM.
Definition: client.h:29
std::max
T max(T... args)
std::unique_ptr
rgbd::View
Definition: view.h:11
client.h
Generated on Mon Apr 28 2025 04:41:00 for rgbd by   doxygen 1.8.17