void SceneCloudView::generateCloud(KinfuTracker& kinfu, bool integrate_colors)
{
viewer_pose_ = kinfu.getCameraPose();
ScopeTimeT time ("PointCloud Extraction");
cout << "\nGetting cloud... " << flush;
valid_combined_ = false;
bool valid_extracted_ = false;
if (extraction_mode_ != GPU_Connected6) // So use CPU
{
kinfu.volume().fetchCloudHost (*cloud_ptr_, extraction_mode_ == CPU_Connected26);
}
else
{
DeviceArray<PointXYZ> extracted = kinfu.volume().fetchCloud (cloud_buffer_device_);
if(extracted.size() > 0){
valid_extracted_ = true;
extracted.download (cloud_ptr_->points);
cloud_ptr_->width = (int)cloud_ptr_->points.size ();
cloud_ptr_->height = 1;
if (integrate_colors)
{
kinfu.colorVolume().fetchColors(extracted, point_colors_device_);
point_colors_device_.download(point_colors_ptr_->points);
point_colors_ptr_->width = (int)point_colors_ptr_->points.size ();
point_colors_ptr_->height = 1;
//pcl::gpu::mergePointRGB(extracted, point_colors_device_, combined_color_device_);
//combined_color_device_.download (combined_color_ptr_->points);
}
else
point_colors_ptr_->points.clear();
combined_color_ptr_->clear();
generateXYZRGB(cloud_ptr_, point_colors_ptr_, combined_color_ptr_);
}else{
valid_extracted_ = false;
cout << "Failed to Extract Cloud " << endl;
}
}
cout << "Done. Cloud size: " << cloud_ptr_->points.size () / 1000 << "K" << endl;
}
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());
}
}
void ImageView::generateDepth (KinfuTracker& kinfu, const Eigen::Affine3f& pose, KinfuTracker::DepthMap &generated_depth_)
{
raycaster_ptr_->run(kinfu.volume(), pose);
raycaster_ptr_->generateDepthImage(generated_depth_);
//if (viz_)
// viewerDepth_->showShortImage (&data[0], generated_depth_.cols(), generated_depth_.rows(), 0, 1000, true);
}
void
showGeneratedDepth (KinfuTracker& kinfu, const Eigen::Affine3f& pose)
{
raycaster_ptr_->run(kinfu.volume(), pose);
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);
}
void
showMesh(KinfuTracker& kinfu, bool /*integrate_colors*/)
{
ScopeTimeT time ("Mesh Extraction");
cout << "\nGetting mesh... " << flush;
if (!marching_cubes_)
marching_cubes_ = MarchingCubes::Ptr( new MarchingCubes() );
DeviceArray<PointXYZ> triangles_device = marching_cubes_->run(kinfu.volume(), triangles_buffer_device_);
mesh_ptr_ = convertToMesh(triangles_device);
cloud_viewer_.removeAllPointClouds ();
if (mesh_ptr_)
cloud_viewer_.addPolygonMesh(*mesh_ptr_);
cout << "Done. Triangles number: " << triangles_device.size() / MarchingCubes::POINTS_PER_TRIANGLE / 1000 << "K" << endl;
}
void
show (KinfuTracker& kinfu, bool integrate_colors)
{
viewer_pose_ = kinfu.getCameraPose();
ScopeTimeT time ("PointCloud Extraction");
cout << "\nGetting cloud... " << flush;
valid_combined_ = false;
if (extraction_mode_ != GPU_Connected6) // So use CPU
{
kinfu.volume().fetchCloudHost (*cloud_ptr_, extraction_mode_ == CPU_Connected26);
}
else
{
DeviceArray<PointXYZ> extracted = kinfu.volume().fetchCloud (cloud_buffer_device_);
if (compute_normals_)
{
kinfu.volume().fetchNormals (extracted, normals_device_);
pcl::gpu::mergePointNormal (extracted, normals_device_, combined_device_);
combined_device_.download (combined_ptr_->points);
combined_ptr_->width = (int)combined_ptr_->points.size ();
combined_ptr_->height = 1;
valid_combined_ = true;
}
else
{
extracted.download (cloud_ptr_->points);
cloud_ptr_->width = (int)cloud_ptr_->points.size ();
cloud_ptr_->height = 1;
}
if (integrate_colors)
{
kinfu.colorVolume().fetchColors(extracted, point_colors_device_);
point_colors_device_.download(point_colors_ptr_->points);
point_colors_ptr_->width = (int)point_colors_ptr_->points.size ();
point_colors_ptr_->height = 1;
}
else
point_colors_ptr_->points.clear();
}
size_t points_size = valid_combined_ ? combined_ptr_->points.size () : cloud_ptr_->points.size ();
cout << "Done. Cloud size: " << points_size / 1000 << "K" << endl;
cloud_viewer_.removeAllPointClouds ();
if (valid_combined_)
{
visualization::PointCloudColorHandlerRGBHack<PointNormal> rgb(combined_ptr_, point_colors_ptr_);
cloud_viewer_.addPointCloud<PointNormal> (combined_ptr_, rgb, "Cloud");
cloud_viewer_.addPointCloudNormals<PointNormal>(combined_ptr_, 50);
}
else
{
visualization::PointCloudColorHandlerRGBHack<PointXYZ> rgb(cloud_ptr_, point_colors_ptr_);
cloud_viewer_.addPointCloud<PointXYZ> (cloud_ptr_, rgb);
}
}