void
publishScene (KinfuTracker& kinfu, std_msgs::Header header, 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);
//convert image to sensor message
sensor_msgs::ImagePtr msg(new sensor_msgs::Image);
sensor_msgs::fillImage((*msg), "rgb8", view_device_.rows(), view_device_.cols(),
view_device_.cols() * 3, &view_host_[0]);
msg->header.frame_id = header.frame_id;
pubKinfu.publish(msg);
}
void
showScene (KinfuTracker& kinfu, const PtrStepSz<const KinfuTracker::PixelRGB>& rgb24, bool registration, Eigen::Affine3f* pose_ptr = 0)
{
if (pose_ptr)
{
raycaster_ptr_->run(kinfu.volume(), *pose_ptr);
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 ImageView::showScene (KinfuTracker& kinfu, bool registration, Eigen::Affine3f* pose_ptr)
{
if (pose_ptr)
{
raycaster_ptr_->run(kinfu.volume(), *pose_ptr);
raycaster_ptr_->generateSceneView(view_device_);
}
else
kinfu.getImage (view_device_);
int cols;
view_device_.download (view_host_, cols);
if (viz_)
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);
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());
}
}