41#include <visp3/core/vpConfig.h>
43#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
44#include <condition_variable>
51#include <pcl/common/common.h>
52#include <pcl/io/pcd_io.h>
53#include <pcl/visualization/cloud_viewer.h>
56#if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
60#include <visp3/core/vpImageConvert.h>
61#include <visp3/core/vpIoTools.h>
62#include <visp3/gui/vpDisplayGDI.h>
63#include <visp3/gui/vpDisplayX.h>
64#include <visp3/io/vpImageIo.h>
65#include <visp3/io/vpParseArgv.h>
66#include <visp3/io/vpVideoWriter.h>
68#define GETOPTARGS "ci:bodh"
73void usage(
const char *name,
const char *badparam)
76 Read RealSense data.\n\
87 Pointcloud is in binary format.\n\
90 Save color and depth side by side to image sequence.\n\
93 Display depth in color.\n\
100 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
103bool getOptions(
int argc,
char **argv, std::string &input_directory,
bool &click,
bool &pointcloud_binary_format,
104 bool &save_video,
bool &color_depth)
107 const char **argv1 = (
const char **)argv;
113 input_directory = optarg;
119 pointcloud_binary_format =
true;
129 usage(argv[0], NULL);
134 usage(argv[0], optarg);
140 if ((c == 1) || (c == -1)) {
142 usage(argv[0], NULL);
143 std::cerr <<
"ERROR: " << std::endl;
144 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
152pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
153bool cancelled =
false, update_pointcloud =
false;
158 explicit ViewerWorker(std::mutex &mutex) : m_mutex(mutex) { }
162 pcl::PointCloud<pcl::PointXYZ>::Ptr local_pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
164 bool local_update =
false, local_cancelled =
false;
165 pcl::visualization::PCLVisualizer::Ptr viewer(
new pcl::visualization::PCLVisualizer(
"3D Viewer"));
166 viewer->setBackgroundColor(0, 0, 0);
167 viewer->initCameraParameters();
168 viewer->setPosition(640 + 80, 480 + 80);
169 viewer->setCameraPosition(0, 0, -0.25, 0, -1, 0);
170 viewer->setSize(640, 480);
172 bool first_init =
true;
173 while (!local_cancelled) {
175 std::unique_lock<std::mutex> lock(m_mutex, std::try_to_lock);
177 if (lock.owns_lock()) {
178 local_update = update_pointcloud;
179 update_pointcloud =
false;
180 local_cancelled = cancelled;
181 local_pointcloud = pointcloud->makeShared();
185 if (local_update && !local_cancelled) {
186 local_update =
false;
189 viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud,
"sample cloud");
190 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
"sample cloud");
194 viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud,
"sample cloud");
198 viewer->spinOnce(10);
201 std::cout <<
"End of point cloud display thread" << std::endl;
210 bool pointcloud_binary_format
213 pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud
217 char buffer[FILENAME_MAX];
218 std::stringstream ss;
219 ss << input_directory <<
"/color_image_%04d.jpg";
220 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
221 std::string filename_color = buffer;
224 ss << input_directory <<
"/depth_image_%04d.bin";
225 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
226 std::string filename_depth = buffer;
229 ss << input_directory <<
"/point_cloud_%04d" << (pointcloud_binary_format ?
".bin" :
".pcd");
230 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
231 std::string filename_pointcloud = buffer;
235 std::cerr <<
"End of sequence." << std::endl;
244 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
245 if (file_depth.is_open()) {
246 unsigned int height = 0, width = 0;
249 I_depth_raw.
resize(height, width);
251 uint16_t depth_value = 0;
252 for (
unsigned int i = 0; i < height; i++) {
253 for (
unsigned int j = 0; j < width; j++) {
255 I_depth_raw[i][j] = depth_value;
262 if (pointcloud_binary_format) {
263 std::ifstream file_pointcloud(filename_pointcloud.c_str(), std::ios::in | std::ios::binary);
264 if (!file_pointcloud.is_open()) {
265 std::cerr <<
"Cannot read pointcloud file: " << filename_pointcloud << std::endl;
268 uint32_t height = 0, width = 0;
272 file_pointcloud.read((
char *)(&is_dense),
sizeof(is_dense));
274 point_cloud->width = width;
275 point_cloud->height = height;
276 point_cloud->is_dense = (is_dense != 0);
277 point_cloud->resize((
size_t)width * height);
279 float x = 0.0f, y = 0.0f, z = 0.0f;
280 for (uint32_t i = 0; i < height; i++) {
281 for (uint32_t j = 0; j < width; j++) {
286 point_cloud->points[(size_t)(i * width + j)].x = x;
287 point_cloud->points[(size_t)(i * width + j)].y = y;
288 point_cloud->points[(size_t)(i * width + j)].z = z;
293 if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename_pointcloud, *point_cloud) == -1) {
294 std::cerr <<
"Cannot read PCD: " << filename_pointcloud << std::endl;
303int main(
int argc,
char *argv[])
305 std::string input_directory =
"";
307 bool pointcloud_binary_format =
false;
308 bool save_video =
false;
309 bool color_depth =
false;
312 if (!getOptions(argc, argv, input_directory, click, pointcloud_binary_format, save_video, color_depth)) {
325 bool init_display =
false;
329 ViewerWorker viewer(mutex);
330 std::thread viewer_thread(&ViewerWorker::run, &viewer);
338 writer.
setFileName(output_directory +
"/%04d.png");
348 std::lock_guard<std::mutex> lock(mutex);
349 update_pointcloud =
true;
350 quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format, pointcloud);
353 quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format);
362 d1.
init(I_color, 0, 0,
"Color image");
364 d2.
init(I_depth_color, I_color.
getWidth() + 10, 0,
"Depth image");
366 d2.
init(I_depth, I_color.
getWidth() + 10, 0,
"Depth image");
375 std::stringstream ss;
376 ss <<
"Frame: " << cpt_frame;
425 std::lock_guard<std::mutex> lock(mutex);
428 viewer_thread.join();
436 std::cerr <<
"Enable C++11 or higher (cmake -DUSE_CXX_STANDARD=11) and install X11 or GDI!" << std::endl;
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
unsigned int getSize() const
unsigned int getHeight() const
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
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()