template <template <typename> class Storage> void
cloud_cb (const boost::shared_ptr<openni_wrapper::Image>& image,
const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image,
float constant)
{
static unsigned count = 0;
static double last = pcl::getTime ();
double now = pcl::getTime ();
if (++count == 30 || (now - last) > 5)
{
std::cerr << "Average framerate: " << double(count)/double(now - last) << " Hz" << std::endl;
count = 0;
last = now;
}
//static unsigned int nr_planes_last_time = 0;
std::cerr << "received callback" << std::endl;
typename PointCloudAOS<Storage>::Ptr data;
typename SampleConsensusModel1PointPlane<Storage>::Ptr sac_model;
{
pcl::ScopeTime timer ("All: ");
// Compute the PointCloud on the device
d2c.compute<Storage> (depth_image, image, constant, data, true, 2);
{
pcl::ScopeTime t ("creating sac_model");
sac_model.reset (new SampleConsensusModel1PointPlane<Storage> (data));
}
MultiRandomSampleConsensus<Storage> sac (sac_model);
sac.setMinimumCoverage (0.90); // at least 95% points should be explained by planes
sac.setMaximumBatches (5);
sac.setIerationsPerBatch (1000);
sac.setDistanceThreshold (0.05);
{
pcl::ScopeTime timer ("computeModel: ");
if (!sac.computeModel (0))
{
std::cerr << "Failed to compute model" << std::endl;
}
else
{
if (use_viewer)
{
std::cerr << "getting inliers.. ";
std::vector<typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr> planes;
// thrust::host_vector<int> regions_host = region_mask;
// std::copy (regions_host.begin (), regions_host.end(), std::ostream_iterator<int>(std::cerr, " "));
{
ScopeTime t ("retrieving inliers");
planes = sac.getAllInliers ();
}
typename Storage<int>::type region_mask;
markInliers<Storage> (data, region_mask, planes);
std::cerr << "image_size: " << data->width << " x " << data->height << " = " << region_mask.size () << std::endl;
typename StoragePointer<Storage,uchar>::type ptr = StorageAllocator<Storage,uchar>::alloc (data->width * data->height);
createIndicesImage<Storage> (ptr, region_mask);
typename ImageType<Storage>::type dst (data->height, data->width, CV_8UC1, thrust::raw_pointer_cast<char4>(ptr), data->width);
cv::imshow ("Result", cv::Mat(dst));
cv::waitKey (2);
std::vector<int> planes_inlier_counts = sac.getAllInlierCounts ();
std::vector<float4> coeffs = sac.getAllModelCoefficients ();
std::vector<float3> centroids = sac.getAllModelCentroids ();
std::cerr << "Found " << planes_inlier_counts.size () << " planes" << std::endl;
int best_plane = 0;
int best_plane_inliers_count = -1;
for (unsigned int i = 0; i < planes.size (); i++)
{
if (planes_inlier_counts[i] > best_plane_inliers_count)
{
best_plane = i;
best_plane_inliers_count = planes_inlier_counts[i];
}
typename SampleConsensusModel1PointPlane<Storage>::IndicesPtr inliers_stencil;
inliers_stencil = planes[i];//sac.getInliersStencil ();
OpenNIRGB color;
double trand = 255 / (RAND_MAX + 1.0);
//int idx = (int)(rand () * trand);
color.r = (int)(rand () * trand);
color.g = (int)(rand () * trand);
color.b = (int)(rand () * trand);
{
ScopeTime t ("coloring planes");
colorIndices<Storage> (data, inliers_stencil, color);
}
}
}
//{
// ScopeTime t ("copying & transforming logo");
//for (unsigned int i = 0; i < planes.size (); i++)
//{
// // if (i == best_plane) // assume first plane is biggest plane
// {
// if (logo_cloud_)
// {
// boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > transformed_logo (new pcl::PointCloud<pcl::PointXYZRGB>);
// Eigen::Affine3f transformation;
// Eigen::Vector3f plane_normal (coeffs[i].x, coeffs[i].y, coeffs[i].z);
// plane_normal.normalize ();
// if (plane_normal.dot (Eigen::Vector3f::Zero()) - coeffs[i].w > 0)
// plane_normal = -plane_normal;
// Eigen::Vector3f logo_normal (0,0,-1);
// Eigen::Vector3f trans (Eigen::Vector3f(centroids[i].x, centroids[i].y, centroids[i].z) * 0.97);
// Eigen::AngleAxisf rot (acos (logo_normal.dot (plane_normal)), logo_normal.cross (plane_normal).normalized ());
// transformation = Eigen::Affine3f::Identity();// = ....;
// transformation.translate (trans);// = ....;
// transformation.rotate (rot);// = ....;
// transformation.scale (0.001 * sqrt (planes_inlier_counts[i]));// = ....;
// std::cerr << "Plane centroid " << centroids[i].x << " " << centroids[i].y << " " << centroids[i].z << std::endl;
// pcl::getTransformedPointCloud<pcl::PointCloud<pcl::PointXYZRGB> > (*logo_cloud_, transformation, *transformed_logo);
// std::stringstream ss;
// ss << "logo" << i;
// //viewer.showCloud (transformed_logo, ss.str ());
// }
// }
//}
//}
//boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > dummy_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
//for (unsigned int i = planes.size (); i < nr_planes_last_time; i++)
//{
// std::stringstream ss;
// ss << "logo" << i;
// viewer.showCloud (dummy_cloud, ss.str ());
//}
//nr_planes_last_time = planes.size ();
}
}
}
if (use_viewer)
{
typename Storage<float4>::type normals = sac_model->getNormals ();
boost::mutex::scoped_lock l(m_mutex);
normal_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl_cuda::toPCL (*data, normals, *normal_cloud);
new_cloud = true;
}
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl_cuda::toPCL (*data, *output);
//viewer.showCloud (output);
}
