Visual Servoing Platform version 3.5.0
readRealSenseData.cpp
1#include <iostream>
2
3#include <visp3/core/vpConfig.h>
4
5#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
6#include <fstream>
7#include <queue>
8#include <mutex>
9#include <thread>
10#include <condition_variable>
11
12#ifdef VISP_HAVE_PCL
13#include <pcl/common/common.h>
14#include <pcl/io/pcd_io.h>
15#include <pcl/visualization/cloud_viewer.h>
16#endif
17
18#if defined (VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
19#define USE_PCL_VIEWER
20#endif
21
22#include <visp3/core/vpImageConvert.h>
23#include <visp3/core/vpIoTools.h>
24#include <visp3/gui/vpDisplayX.h>
25#include <visp3/gui/vpDisplayGDI.h>
26#include <visp3/io/vpParseArgv.h>
27#include <visp3/io/vpImageIo.h>
28#include <visp3/io/vpVideoWriter.h>
29
30#define GETOPTARGS "ci:bodh"
31
32namespace {
33
34 void usage(const char *name, const char *badparam) {
35 fprintf(stdout, "\n\
36 Read RealSense data.\n\
37 \n\
38 %s\
39 OPTIONS: \n\
40 -i <directory> \n\
41 Input directory.\n\
42 \n\
43 -c \n\
44 Click enable.\n\
45 \n\
46 -b \n\
47 Pointcloud is in binary format.\n\
48 \n\
49 -o \n\
50 Save color and depth side by side to image sequence.\n\
51 \n\
52 -d \n\
53 Display depth in color.\n\
54 \n\
55 -h \n\
56 Print the help.\n\n",
57 name);
58
59 if (badparam)
60 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
61 }
62
63 bool getOptions(int argc, char **argv,
64 std::string &input_directory,
65 bool &click,
66 bool &pointcloud_binary_format,
67 bool &save_video,
68 bool &color_depth
69 ) {
70 const char *optarg;
71 const char **argv1=(const char**)argv;
72 int c;
73 while ((c = vpParseArgv::parse(argc, argv1, GETOPTARGS, &optarg)) > 1) {
74
75 switch (c) {
76 case 'i': input_directory = optarg; break;
77 case 'c': click = true; break;
78 case 'b': pointcloud_binary_format = true; break;
79 case 'o': save_video = true; break;
80 case 'd': color_depth = true; break;
81
82 case 'h': usage(argv[0], NULL); return false; break;
83
84 default:
85 usage(argv[0], optarg);
86 return false; break;
87 }
88 }
89
90 if ((c == 1) || (c == -1)) {
91 // standalone param or error
92 usage(argv[0], NULL);
93 std::cerr << "ERROR: " << std::endl;
94 std::cerr << " Bad argument " << optarg << std::endl << std::endl;
95 return false;
96 }
97
98 return true;
99 }
100
101#ifdef USE_PCL_VIEWER
102pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
103bool cancelled = false, update_pointcloud = false;
104
105class ViewerWorker {
106public:
107 explicit ViewerWorker(std::mutex &mutex) :
108 m_mutex(mutex) { }
109
110 void run() {
111 pcl::PointCloud<pcl::PointXYZ>::Ptr local_pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
112
113 bool local_update = false, local_cancelled = false;
114 pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
115 viewer->setBackgroundColor (0, 0, 0);
116 viewer->initCameraParameters ();
117 viewer->setPosition(640+80, 480+80);
118 viewer->setCameraPosition(0, 0, -0.25, 0, -1, 0);
119 viewer->setSize(640, 480);
120
121 bool first_init = true;
122 while (!local_cancelled) {
123 {
124 std::unique_lock<std::mutex> lock(m_mutex, std::try_to_lock);
125
126 if (lock.owns_lock()) {
127 local_update = update_pointcloud;
128 update_pointcloud = false;
129 local_cancelled = cancelled;
130 local_pointcloud = pointcloud->makeShared();
131 }
132 }
133
134 if (local_update && !local_cancelled) {
135 local_update = false;
136
137 if (first_init) {
138 viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
139 viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
140 first_init = false;
141 } else {
142 viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
143 }
144 }
145
146 viewer->spinOnce(10);
147 }
148
149 std::cout << "End of point cloud display thread" << std::endl;
150 }
151
152private:
153 std::mutex &m_mutex;
154};
155#endif
156
157bool readData(int cpt, const std::string &input_directory, vpImage<vpRGBa> &I_color, vpImage<uint16_t> &I_depth_raw,
158 bool pointcloud_binary_format
159#ifdef USE_PCL_VIEWER
160 , pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud
161#endif
162 ) {
163 char buffer[256];
164 std::stringstream ss;
165 ss << input_directory << "/color_image_%04d.jpg";
166 sprintf(buffer, ss.str().c_str(), cpt);
167 std::string filename_color = buffer;
168
169 ss.str("");
170 ss << input_directory << "/depth_image_%04d.bin";
171 sprintf(buffer, ss.str().c_str(), cpt);
172 std::string filename_depth = buffer;
173
174 ss.str("");
175 ss << input_directory << "/point_cloud_%04d" << (pointcloud_binary_format ? ".bin" : ".pcd");
176 sprintf(buffer, ss.str().c_str(), cpt);
177 std::string filename_pointcloud = buffer;
178
179 if (!vpIoTools::checkFilename(filename_color) && !vpIoTools::checkFilename(filename_depth)
180 && !vpIoTools::checkFilename(filename_pointcloud)) {
181 std::cerr << "End of sequence." << std::endl;
182 return false;
183 }
184
185 //Read color
186 if (vpIoTools::checkFilename(filename_color))
187 vpImageIo::read(I_color, filename_color);
188
189 //Read raw depth
190 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
191 if (file_depth.is_open()) {
192 unsigned int height = 0, width = 0;
193 vpIoTools::readBinaryValueLE(file_depth, height);
194 vpIoTools::readBinaryValueLE(file_depth, width);
195 I_depth_raw.resize(height, width);
196
197 uint16_t depth_value = 0;
198 for (unsigned int i = 0; i < height; i++) {
199 for (unsigned int j = 0; j < width; j++) {
200 vpIoTools::readBinaryValueLE(file_depth, depth_value);
201 I_depth_raw[i][j] = depth_value;
202 }
203 }
204 }
205
206 //Read pointcloud
207#ifdef USE_PCL_VIEWER
208 if (pointcloud_binary_format) {
209 std::ifstream file_pointcloud(filename_pointcloud.c_str(), std::ios::in | std::ios::binary);
210 if (!file_pointcloud.is_open()) {
211 std::cerr << "Cannot read pointcloud file: " << filename_pointcloud << std::endl;
212 }
213
214 uint32_t height = 0, width = 0;
215 char is_dense = 1;
216 vpIoTools::readBinaryValueLE(file_pointcloud, height);
217 vpIoTools::readBinaryValueLE(file_pointcloud, width);
218 file_pointcloud.read( (char *)(&is_dense), sizeof(is_dense) );
219
220 point_cloud->width = width;
221 point_cloud->height = height;
222 point_cloud->is_dense = (is_dense != 0);
223 point_cloud->resize((size_t) width*height);
224
225 float x = 0.0f, y = 0.0f, z = 0.0f;
226 for (uint32_t i = 0; i < height; i++) {
227 for (uint32_t j = 0; j < width; j++) {
228 vpIoTools::readBinaryValueLE(file_pointcloud, x);
229 vpIoTools::readBinaryValueLE(file_pointcloud, y);
230 vpIoTools::readBinaryValueLE(file_pointcloud, z);
231
232 point_cloud->points[(size_t) (i*width + j)].x = x;
233 point_cloud->points[(size_t) (i*width + j)].y = y;
234 point_cloud->points[(size_t) (i*width + j)].z = z;
235 }
236 }
237 } else {
238 if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pointcloud, *point_cloud) == -1) {
239 std::cerr << "Cannot read PCD: " << filename_pointcloud << std::endl;
240 }
241 }
242#endif
243
244 return true;
245}
246} //Namespace
247
248int main(int argc, char *argv[]) {
249 std::string input_directory = "";
250 bool click = false;
251 bool pointcloud_binary_format = false;
252 bool save_video = false;
253 bool color_depth = false;
254
255 // Read the command line options
256 if (!getOptions(argc, argv, input_directory, click, pointcloud_binary_format,
257 save_video, color_depth)) {
258 return EXIT_FAILURE;
259 }
260
261 vpImage<vpRGBa> I_color(480, 640), I_depth_color(480, 640);
262 vpImage<uint16_t> I_depth_raw(480,640);
263 vpImage<unsigned char> I_depth(480, 640);
264
265#ifdef VISP_HAVE_X11
266 vpDisplayX d1, d2;
267#else
268 vpDisplayGDI d1, d2;
269#endif
270 bool init_display = false;
271
272#ifdef USE_PCL_VIEWER
273 std::mutex mutex;
274 ViewerWorker viewer(mutex);
275 std::thread viewer_thread(&ViewerWorker::run, &viewer);
276#endif
277
278 vpVideoWriter writer;
280 if (save_video) {
281 std::string output_directory = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S");
282 vpIoTools::makeDirectory(output_directory);
283 writer.setFileName(output_directory + "/%04d.png");
284 }
285
286 int cpt_frame = 0;
287 bool quit = false;
288 while (!quit) {
289 double t = vpTime::measureTimeMs();
290
291#ifdef USE_PCL_VIEWER
292 {
293 std::lock_guard<std::mutex> lock(mutex);
294 update_pointcloud = true;
295 quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format, pointcloud);
296 }
297#else
298 quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format);
299#endif
300
301 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
302 if (color_depth)
303 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth_color);
304
305 if (!init_display) {
306 init_display = true;
307 d1.init(I_color, 0, 0, "Color image");
308 if (color_depth)
309 d2.init(I_depth_color, I_color.getWidth()+10, 0, "Depth image");
310 else
311 d2.init(I_depth, I_color.getWidth()+10, 0, "Depth image");
312 }
313
314 vpDisplay::display(I_color);
315 if (color_depth)
316 vpDisplay::display(I_depth_color);
317 else
318 vpDisplay::display(I_depth);
319
320 std::stringstream ss;
321 ss << "Frame: " << cpt_frame;
322 vpDisplay::displayText(I_color, 20, 20, ss.str(), vpColor::red);
323 if (color_depth)
324 vpDisplay::displayText(I_depth_color, 20, 20, ss.str(), vpColor::red);
325 else
326 vpDisplay::displayText(I_depth, 20, 20, ss.str(), vpColor::red);
327
328 vpDisplay::flush(I_color);
329 if (color_depth)
330 vpDisplay::flush(I_depth_color);
331 else
332 vpDisplay::flush(I_depth);
333
334 if (save_video) {
335 if (O.getSize() == 0) {
336 O.resize(I_color.getHeight(), I_color.getWidth()+I_depth_color.getWidth());
337 writer.open(O);
338 }
339
340 O.insert(I_color, vpImagePoint());
341 if (!color_depth)
342 vpImageConvert::convert(I_depth, I_depth_color);
343 O.insert(I_depth_color, vpImagePoint(0, I_color.getWidth()));
344 writer.saveFrame(O);
345 }
346
348 if (vpDisplay::getClick(I_color, button, click)) {
349 switch (button) {
351 if (!quit)
352 quit = !click;
353 break;
354
356 click = !click;
357 break;
358
359 default:
360 break;
361 }
362 }
363
364 vpTime::wait(t, 30);
365 cpt_frame++;
366 }
367
368#ifdef USE_PCL_VIEWER
369 {
370 std::lock_guard<std::mutex> lock(mutex);
371 cancelled = true;
372 }
373 viewer_thread.join();
374#endif
375
376 return EXIT_SUCCESS;
377}
378#else
379int main() {
380 std::cerr << "Enable C++11 or higher (cmake -DUSE_CXX_STANDARD=11) and install X11 or GDI!" << std::endl;
381 return 0;
382}
383#endif
static const vpColor red
Definition: vpColor.h:217
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:149
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
unsigned int getWidth() const
Definition: vpImage.h:246
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
Definition: vpImage.h:1115
unsigned int getSize() const
Definition: vpImage.h:227
unsigned int getHeight() const
Definition: vpImage.h:188
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:802
static void readBinaryValueLE(std::ifstream &file, int16_t &short_value)
Definition: vpIoTools.cpp:1993
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:570
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
Class that enables to write easily a video file or a sequence of images.
void saveFrame(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
void open(vpImage< vpRGBa > &I)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
VISP_EXPORT double measureTimeMs()