void
showScene (KinfuTracker& kinfu, const PtrStepSz<const pcl::gpu::kinfuLS::PixelRGB>& rgb24, bool registration, Eigen::Affine3f* pose_ptr = 0)
{
if (pose_ptr)
{
raycaster_ptr_->run ( kinfu.volume (), *pose_ptr, kinfu.getCyclicalBufferStructure () ); //says in cmake it does not know it
raycaster_ptr_->generateSceneView(view_device_);
}
else
{
kinfu.getImage (view_device_);
}
if (paint_image_ && registration && !pose_ptr)
{
colors_device_.upload (rgb24.data, rgb24.step, rgb24.rows, rgb24.cols);
paint3DView (colors_device_, view_device_);
}
int cols;
view_device_.download (view_host_, cols);
viewerScene_.showRGBImage (reinterpret_cast<unsigned char*> (&view_host_[0]), view_device_.cols (), view_device_.rows ());
//viewerColor_.showRGBImage ((unsigned char*)&rgb24.data, rgb24.cols, rgb24.rows);
#ifdef HAVE_OPENCV
if (accumulate_views_)
{
views_.push_back (cv::Mat ());
cv::cvtColor (cv::Mat (480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back (), CV_RGB2GRAY);
//cv::copy(cv::Mat(480, 640, CV_8UC3, (void*)&view_host_[0]), views_.back());
}
#endif
}
void
showGeneratedDepth (KinfuTracker& kinfu, const Eigen::Affine3f& pose)
{
raycaster_ptr_->run(kinfu.volume(), pose, kinfu.getCyclicalBufferStructure ());
raycaster_ptr_->generateDepthImage(generated_depth_);
int c;
vector<unsigned short> data;
generated_depth_.download(data, c);
viewerDepth_.showShortImage (&data[0], generated_depth_.cols(), generated_depth_.rows(), 0, 5000, true);
}
void
publishGeneratedDepth (KinfuTracker& kinfu)
{
const Eigen::Affine3f& pose= kinfu.getCameraPose();
raycaster_ptr_->run(kinfu.volume(), pose, kinfu.getCyclicalBufferStructure());
raycaster_ptr_->generateDepthImage(generated_depth_);
int c;
vector<unsigned short> data;
generated_depth_.download(data, c);
sensor_msgs::ImagePtr msg(new sensor_msgs::Image);
sensor_msgs::fillImage(*msg, "bgr8", view_device_.rows(), view_device_.cols(), view_device_.cols() * 3, &view_host_[0]);
pubgen.publish(msg);
}