Skip to content

Commit

Permalink
Merge pull request #1392 from fspindle/fix_rs2_pcl
Browse files Browse the repository at this point in the history
Fix realsense2 save and read examples + PCL defines usage
  • Loading branch information
fspindle authored Apr 29, 2024
2 parents 665d9e5 + c9c2553 commit e1b6b55
Show file tree
Hide file tree
Showing 5 changed files with 33 additions and 25 deletions.
7 changes: 4 additions & 3 deletions example/device/framegrabber/readRealSenseData.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -81,15 +81,15 @@ void usage(const char *name, const char *badparam)
<< std::endl
<< "SYNOPSIS " << std::endl
<< " " << name
<< " [--i <directory>]"
<< " [-i <directory>]"
<< " [-c]"
<< " [-b]"
<< " [-o]"
<< " [-d]"
<< " [--help,-h]"
<< std::endl;
std::cout << "\nOPTIONS " << std::endl
<< " --i <directory>" << std::endl
<< " -i <directory>" << std::endl
<< " Input folder that contains the data to read." << std::endl
<< std::endl
<< " -c" << std::endl
Expand Down Expand Up @@ -191,8 +191,9 @@ bool readData(int cpt, const std::string &input_directory, vpImage<vpRGBa> &I_co
}

// Read color
if (vpIoTools::checkFilename(filename_color))
if (vpIoTools::checkFilename(filename_color)) {
vpImageIo::read(I_color, filename_color);
}

// Read raw depth
std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
Expand Down
36 changes: 17 additions & 19 deletions example/device/framegrabber/saveRealSenseData.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,6 @@
#endif

#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpIoException.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/gui/vpDisplayGDI.h>
Expand Down Expand Up @@ -132,6 +131,10 @@ void usage(const char *name, const char *badparam, int fps)
<< " --help, -h" << std::endl
<< " Display this helper message." << std::endl
<< std::endl;
std::cout << "\nEXAMPLE " << std::endl
<< "- Save aligned color + depth + point cloud in data folder" << std::endl
<< " " << name << " -s -a -c -d -p -b -o data" << std::endl
<< std::endl;

if (badparam) {
std::cout << "\nERROR: Bad parameter " << badparam << std::endl;
Expand Down Expand Up @@ -439,10 +442,8 @@ class vpStorageWorker
}
}
else {
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO)
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO) && defined(VISP_HAVE_PCL_COMMON)
pcl::io::savePCDFileBinary(filename_point_cloud, *pointCloud);
#elif defined(VISP_HAVE_PCL)
throw(vpIoException(vpIoException::fatalError, "Cannot save as pcd files without PCL io module"));
#endif
}
}
Expand Down Expand Up @@ -543,7 +544,6 @@ int main(int argc, const char *argv[])
#endif

vpImage<vpRGBa> I_color(height, width);
vpImage<unsigned char> I_gray(height, width);
vpImage<unsigned char> I_depth(height, width);
vpImage<uint16_t> I_depth_raw(height, width);
vpImage<unsigned char> I_infrared(height, width);
Expand All @@ -553,22 +553,21 @@ int main(int argc, const char *argv[])
#else
vpDisplayGDI d1, d2, d3;
#endif
d1.init(I_gray, 0, 0, "RealSense color stream");
d2.init(I_depth, I_gray.getWidth() + 80, 0, "RealSense depth stream");
d3.init(I_infrared, I_gray.getWidth() + 80, I_gray.getHeight() + 70, "RealSense infrared stream");
d1.init(I_color, 0, 0, "RealSense color stream");
d2.init(I_depth, I_color.getWidth() + 80, 0, "RealSense depth stream");
d3.init(I_infrared, I_color.getWidth() + 80, I_color.getHeight() + 70, "RealSense infrared stream");

while (true) {
realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, nullptr);
vpImageConvert::convert(I_color, I_gray);
vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);

vpDisplay::display(I_gray);
vpDisplay::display(I_color);
vpDisplay::display(I_depth);
vpDisplay::displayText(I_gray, 20, 20, "Click when ready.", vpColor::red);
vpDisplay::flush(I_gray);
vpDisplay::displayText(I_color, 20, 20, "Click when ready.", vpColor::red);
vpDisplay::flush(I_color);
vpDisplay::flush(I_depth);

if (vpDisplay::getClick(I_gray, false)) {
if (vpDisplay::getClick(I_color, false)) {
break;
}
}
Expand Down Expand Up @@ -683,23 +682,22 @@ int main(int argc, const char *argv[])
#endif
}

vpImageConvert::convert(I_color, I_gray);
vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);

vpDisplay::display(I_gray);
vpDisplay::display(I_color);
vpDisplay::display(I_depth);
vpDisplay::display(I_infrared);

if (!click_to_save) {
vpDisplay::displayText(I_gray, 20, 20, "Click to quit.", vpColor::red);
vpDisplay::displayText(I_color, 20, 20, "Click to quit.", vpColor::red);
}
else {
std::stringstream ss;
ss << "Images saved: " << nb_saves;
vpDisplay::displayText(I_gray, 20, 20, ss.str(), vpColor::red);
vpDisplay::displayText(I_color, 20, 20, ss.str(), vpColor::red);
}

vpDisplay::flush(I_gray);
vpDisplay::flush(I_color);
vpDisplay::flush(I_depth);
vpDisplay::flush(I_infrared);

Expand All @@ -713,7 +711,7 @@ int main(int argc, const char *argv[])
}

vpMouseButton::vpMouseButtonType button;
if (vpDisplay::getClick(I_gray, button, false)) {
if (vpDisplay::getClick(I_color, button, false)) {
if (!click_to_save) {
save_queue.cancel();
quit = true;
Expand Down
2 changes: 1 addition & 1 deletion modules/gui/src/pointcloud/vpPclViewer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS

#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) && defined(VISP_HAVE_THREADS)
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO) && defined(VISP_HAVE_THREADS)
// PCL
#include<pcl/io/pcd_io.h>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ int main(int argc, const char **argv)
int color_id = -1;
unsigned int thickness = 2;
bool align_frame = false;
bool opt_verbose = false;

#if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
bool display_off = true;
Expand Down Expand Up @@ -66,6 +67,9 @@ int main(int argc, const char **argv)
else if (std::string(argv[i]) == "--z_aligned") {
align_frame = true;
}
else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
opt_verbose = true;
}
else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
std::cout << "Usage: " << argv[0]
<< " [--tag_size <tag_size in m> (default: 0.053)]"
Expand All @@ -81,7 +85,7 @@ int main(int argc, const char **argv)
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
std::cout << " [--display_off] [--color <color id>] [--thickness <line thickness>]";
#endif
std::cout << " [--help]" << std::endl;
std::cout << " [--verbose,-v] [--help,-h]" << std::endl;
return EXIT_SUCCESS;
}
}
Expand Down Expand Up @@ -219,6 +223,11 @@ int main(int argc, const char **argv)
std::stringstream ss;
ss << "Tag id " << tags_id[i] << " confidence: " << confidence_index;
vpDisplay::displayText(I_color2, 35 + i * 15, 20, ss.str(), vpColor::red);

if (opt_verbose) {
std::cout << "cMo[" << i << "]: \n" << cMo_vec[i] << std::endl;
std::cout << "cMo[" << i << "] using depth: \n" << cMo << std::endl;
}
}
}
//! [Pose from depth map]
Expand Down
2 changes: 1 addition & 1 deletion tutorial/gui/pcl-visualizer/ClassUsingPclViewer.cpp
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
//! \example ClassUsingPclViewer.cpp
#include "ClassUsingPclViewer.h"

#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO)
// PCL
#include <pcl/io/pcd_io.h>

Expand Down

0 comments on commit e1b6b55

Please sign in to comment.