Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support OpenNI2 devices without RGB #1174

Closed
wants to merge 3 commits into from
Closed

Support OpenNI2 devices without RGB #1174

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7dfc3ca
Patched openni2_viewer to support Structure.io (and other OpenNI2 dev…
metacomgd Mar 11, 2015
b207315
Fixed spacing in previous patch
metacomgd Mar 14, 2015
0235cc1
Added openni2_pcd_recorder
metacomgd Mar 14, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion io/src/openni2/openni2_device.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,10 @@ pcl::io::openni2::OpenNI2Device::OpenNI2Device (const std::string& device_URI) :
// Set default resolution if not reading a file
if (!openni_device_->isFile ())
{
setColorVideoMode (getDefaultColorMode ());
if (openni_device_->hasSensor (openni::SENSOR_COLOR))
{
setColorVideoMode (getDefaultColorMode ());
}
setDepthVideoMode (getDefaultDepthMode ());
setIRVideoMode (getDefaultIRMode ());
}
Expand Down
6 changes: 3 additions & 3 deletions io/src/openni2_grabber.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -219,7 +219,7 @@ pcl::io::OpenNI2Grabber::start ()
try
{
// check if we need to start/stop any stream
if (image_required_ && !device_->isColorStreamStarted () )
if (image_required_ && !device_->isColorStreamStarted () && device_->hasColorSensor () )
{
block_signals ();
device_->startColorStream ();
Expand Down Expand Up @@ -401,8 +401,8 @@ pcl::io::OpenNI2Grabber::startSynchronization ()
{
try
{
if (device_->isSynchronizationSupported () && !device_->isSynchronized () && !device_->isFile () &&
device_->getColorVideoMode ().frame_rate_ == device_->getDepthVideoMode ().frame_rate_)
if (device_->hasColorSensor () && (device_->isSynchronizationSupported () && !device_->isSynchronized () && !device_->isFile () &&
device_->getColorVideoMode ().frame_rate_ == device_->getDepthVideoMode ().frame_rate_))
device_->setSynchronization (true);
}
catch (const IOException& exception)
Expand Down
70 changes: 66 additions & 4 deletions visualization/tools/openni2_viewer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/boost.h>
#include <pcl/visualization/image_viewer.h>
#include <pcl/visualization/common/float_image_utils.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
Expand Down Expand Up @@ -119,8 +120,10 @@ class OpenNI2Viewer
OpenNI2Viewer (pcl::io::OpenNI2Grabber& grabber)
: cloud_viewer_ (new pcl::visualization::PCLVisualizer ("PCL OpenNI2 cloud"))
, image_viewer_ ()
, depth_viewer_ ()
, grabber_ (grabber)
, rgb_data_ (0), rgb_data_size_ (0)
, xyz_data_ (0), xyz_data_size_ (0)
{
}

Expand Down Expand Up @@ -152,6 +155,17 @@ class OpenNI2Viewer
}
}


void
depth_callback (const boost::shared_ptr<pcl::io::openni2::DepthImage>& depth)
{
FPS_CALC ("depth callback");

boost::mutex::scoped_lock lock (depth_mutex_);
depth_ = depth;

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please delete the extra line returns (there are a few across your pull request)

}

void
keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
{
Expand Down Expand Up @@ -189,20 +203,34 @@ class OpenNI2Viewer
boost::signals2::connection image_connection;
if (grabber_.providesCallback<void (const boost::shared_ptr<pcl::io::openni2::Image>&)>())
{
image_viewer_.reset (new pcl::visualization::ImageViewer ("PCL OpenNI image"));
image_viewer_.reset (new pcl::visualization::ImageViewer ("PCL OpenNI2 image"));
image_viewer_->registerMouseCallback (&OpenNI2Viewer::mouse_callback, *this);
image_viewer_->registerKeyboardCallback (&OpenNI2Viewer::keyboard_callback, *this);
boost::function<void (const boost::shared_ptr<pcl::io::openni2::Image>&) > image_cb = boost::bind (&OpenNI2Viewer::image_callback, this, _1);
image_connection = grabber_.registerCallback (image_cb);
}

bool image_init = false, cloud_init = false;
boost::signals2::connection depth_connection;
if (grabber_.providesCallback<void (const boost::shared_ptr<pcl::io::openni2::DepthImage>&)>())
{
depth_viewer_.reset (new pcl::visualization::ImageViewer ("PCL OpenNI2 Depth"));
depth_viewer_->registerMouseCallback (&OpenNI2Viewer::mouse_callback, *this);
depth_viewer_->registerKeyboardCallback (&OpenNI2Viewer::keyboard_callback, *this);
boost::function<void (const boost::shared_ptr<pcl::io::openni2::DepthImage>&) > depth_cb = boost::bind (&OpenNI2Viewer::depth_callback, this, _1);
depth_connection = grabber_.registerCallback (depth_cb);
}




bool image_init = false, cloud_init = false, depth_init = false;

grabber_.start ();

while (!cloud_viewer_->wasStopped () && (image_viewer_ && !image_viewer_->wasStopped ()))
while (!cloud_viewer_->wasStopped () && ((image_viewer_ && !image_viewer_->wasStopped ()) || ( depth_viewer_ && !depth_viewer_->wasStopped ())))
{
boost::shared_ptr<pcl::io::openni2::Image> image;
boost::shared_ptr<pcl::io::openni2::DepthImage> depth;
CloudConstPtr cloud;

cloud_viewer_->spinOnce ();
Expand Down Expand Up @@ -260,32 +288,66 @@ class OpenNI2Viewer
image_viewer_->spinOnce ();

}
}

if (depth_mutex_.try_lock ())
{
depth_.swap (depth);
depth_mutex_.unlock ();
}

if (depth)
{
if (!depth_init && cloud && cloud->width != 0)
{
depth_viewer_->setPosition (cloud->width, 0);
depth_viewer_->setSize (cloud->width, cloud->height);
depth_init = !depth_init;
}

unsigned char* data = pcl::visualization::FloatImageUtils::getVisualImage (
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please fix the indentation here

reinterpret_cast<const unsigned short*> (depth->getData ()),
depth->getWidth (), depth->getHeight (),
std::numeric_limits<unsigned short>::min (),
std::numeric_limits<unsigned short>::max () / 10,
true);

depth_viewer_->addRGBImage ( data, depth->getWidth (), depth->getHeight ());

depth_viewer_->spinOnce ();

}
}
grabber_.stop ();

cloud_connection.disconnect ();
image_connection.disconnect ();
depth_connection.disconnect ();
if (rgb_data_)
delete[] rgb_data_;
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> cloud_viewer_;
boost::shared_ptr<pcl::visualization::ImageViewer> image_viewer_;
boost::shared_ptr<pcl::visualization::ImageViewer> depth_viewer_;

pcl::io::OpenNI2Grabber& grabber_;
boost::mutex cloud_mutex_;
boost::mutex image_mutex_;
boost::mutex depth_mutex_;

CloudConstPtr cloud_;
boost::shared_ptr<pcl::io::openni2::Image> image_;
boost::shared_ptr<pcl::io::openni2::DepthImage> depth_;
unsigned char* rgb_data_;
unsigned char* xyz_data_;
unsigned rgb_data_size_;
unsigned xyz_data_size_;
};

// Create the PCLVisualizer object
boost::shared_ptr<pcl::visualization::PCLVisualizer> cld;
boost::shared_ptr<pcl::visualization::ImageViewer> img;
boost::shared_ptr<pcl::visualization::ImageViewer> dep;

/* ---[ */
int
Expand Down