/***********************************************************************************************************************
* @BRIEF Starts data acquisition and handling
* @AUTHOR Christopher D. McMurrough
**********************************************************************************************************************/
void run()
{
// create a new grabber for OpenNI2 devices
pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();
// bind the callbacks to the appropriate member functions
boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind(&OpenNI2Processor::cloudCallback, this, _1);
// connect callback function for desired signal. In this case its a point cloud with color values
interface->registerCallback(f);
// start receiving point clouds
interface->start();
// start the timer
m_stopWatch.reset();
// wait until user quits program
while (!m_viewer.wasStopped())
{
Sleep(1);
}
// stop the grabber
interface->stop();
}
void run ()
{
pcl::Grabber* interface = new pcl::OpenNIGrabber();
boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);
interface->registerCallback (f);
viewer.registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);
interface->start ();
while (!viewer.wasStopped())
{
boost::this_thread::sleep (boost::posix_time::seconds (10));
if(flag > 0)
{
save_cloud();
// count++;
}
if (count >= 2)
{
return;
}
}
interface->stop ();
}
void PbMapVisualizer::Visualize()
{
cloudViewer.runOnVisualizationThread (boost::bind(&PbMapVisualizer::viz_cb, this, _1), "viz_cb");
cloudViewer.registerKeyboardCallback ( keyboardEventOccurred );
while (!cloudViewer.wasStopped() )
mrpt::system::sleep(10);
}
void cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud){ //fonction <> =>classe template
if(!viewer.wasStopped()){
/*for(int i=0;i<cloud->width;i++){
for (int j=0;j<cloud->height;j++){
if (i>300 && j>300)
std::cout <<cloud->width<< cloud->height <<std::endl;
}}
*/
nuage=*cloud;
//viewer.showCloud(nuage);//on montre le viewer tant qu'on ne l'a pas arreté
}
}
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
if (!viewer.wasStopped())
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr result (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PassThrough<pcl::PointXYZRGBA> pass;
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 3.0);
pass.setInputCloud (cloud);
pass.filter (*result);
s_cloud = *result;
viewer.showCloud (result);
}
}
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
if (!viewer.wasStopped())
{
//// Green region detection
// pcl::PointCloud<pcl::PointXYZRGBA>::Ptr final_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
// final_cloud->width = cloud->width;
// final_cloud->height = cloud->height;
// final_cloud->resize (cloud->width * cloud->height);
//
// size_t i = 0;
viewer.showCloud (cloud);
}
}
void run(){
depth=Mat(480,640,DataType<float>::type);
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr nuage3(&nuage2);// (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGBA point;
it=1000;
pcl::OpenNIGrabber* interface =new pcl::OpenNIGrabber();//creation d'un objet interface qui vient de l'include openni_grabber
//namedWindow( "Display Image", CV_WINDOW_AUTOSIZE );
namedWindow( "Harris Image", CV_WINDOW_AUTOSIZE );
//namedWindow( "Depth Image", CV_WINDOW_AUTOSIZE );
// VideoCapture capture(1);
// Mat frame;
// capture >> frame;
// record=VideoWriter("/home/guerric/Bureau/test.avi", CV_FOURCC('M','J','P','G'), 30, frame.size(), true);
boost::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)>
f = boost::bind (&ImageVIewer::cloud_cb_, this, _1);
boost::function<void(const boost::shared_ptr<openni_wrapper::Image>&)>
g = boost::bind (&ImageVIewer::image_cb_, this, _1);
boost::function<void(const boost::shared_ptr<openni_wrapper::DepthImage>&)>
h = boost::bind (&ImageVIewer::depth_cb_, this, _1);
interface->registerCallback (f);
interface->registerCallback (g);
interface->registerCallback (h);
interface->start();
//on reste dans cet état d'acquisition tant qu'on ne stoppe pas dans le viewer
while(!viewer.wasStopped()){
boost::this_thread::sleep(boost::posix_time::seconds(1)); //met la fonction en attente pendant une seconde <=> sleep(1) mais plus précis pour les multicores
viewer.showCloud(nuage3);
}
interface->stop();
record.release();
destroyAllWindows();
}
void run ()
{
pcl::Grabber* interface = new pcl::OpenNIGrabber();
boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);
interface->registerCallback (f);
viewer.registerPointPickingCallback (pointpickingEventOccurred, (void*)&viewer);
interface->start ();
while (!viewer.wasStopped())
{
boost::this_thread::sleep (boost::posix_time::seconds (1));
}
interface->stop ();
}
void cloud_cb(const boost::shared_ptr<const sensor_msgs::PointCloud2>& rototranslatedpcBoostPtr){
boost::shared_ptr <pcl::PointCloud <pcl::PointXYZ> > pclCloudBoostPtr (new pcl::PointCloud<pcl::PointXYZ> );
pcl::fromROSMsg( *rototranslatedpcBoostPtr , *pclCloudBoostPtr ); // ORIG WORKING
// // Perform voxel filter
// boost::shared_ptr <pcl::PointCloud <pcl::PointXYZ> > filteredCloudBoostPtr (new pcl::PointCloud<pcl::PointXYZ> ); // Uncomment to use filtering
// pcl::VoxelGrid<pcl::PointXYZ> sor;
// sor.setInputCloud (pclCloudBoostPtr);
// sor.setLeafSize (0.01f, 0.01f, 0.01f);
// sor.filter (*filteredCloudBoostPtr);
//
// // Prints filtered pointcloud
// for (size_t i = 0; i < filteredCloudBoostPtr->points.size (); ++i)
// std::cout << filteredCloudBoostPtr->points[i].x
// << " "<< filteredCloudBoostPtr->points[i].y
// << " "<< filteredCloudBoostPtr->points[i].z << std::endl;
//
// std::cout<<filteredCloudBoostPtr->points.size ()<<std::endl;
// if (!viewer.wasStopped()) viewer.showCloud (filteredCloudBoostPtr, "sample cloud");
if (!viewer.wasStopped()) viewer.showCloud ( pclCloudBoostPtr, "sample cloud"); // IGNORE ECLIPSE ERROR HERE. COMPILER WORKS.
}
void cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud){ //fonction <> =>classe template
if(!viewer.wasStopped()){
viewer.showCloud(cloud);//on montre le viewer tant qu'on ne l'a pas arreté
}
}
int main (int argc, char** argv)
{
//----------------------------------------------------------------------------------
//Read pcd file
//----------------------------------------------------------------------------------
if (pcl::io::loadPCDFile<PoinT> ("Cosyslab-0.pcd", *source_cloud) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file source_cloud.pcd \n");
return (-1);
}
cout << "Loaded " << source_cloud->width * source_cloud->height << " data points "<< endl;
if (pcl::io::loadPCDFile<PoinT> ("Cosyslab-1.pcd", *target_cloud) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file target_test_nonoise.pcd \n");
return (-1);
}
cout << "Loaded " << target_cloud->width * target_cloud->height << " data points "<< endl;
//----------------------------------------------------------------------------------
//remove NAN points from the cloud
//----------------------------------------------------------------------------------
std::vector<int> indices_src, indices_tgt;
pcl::removeNaNFromPointCloud(*source_cloud,*source_cloud, indices_src);
pcl::removeNaNFromPointCloud(*target_cloud,*target_cloud, indices_tgt);
//----------------------------------------------------------------------------------
//Reduce number of points
//----------------------------------------------------------------------------------
pcl::VoxelGrid<PoinT> grid, grid1;
grid.setLeafSize (0.01, 0.01, 0.05);
grid.setInputCloud (source_cloud);
grid.filter(*source_cloud);
cout << "source cloud number of point after voxelgrid: " << source_cloud->points.size() << endl;
grid1.setLeafSize (0.01, 0.01, 0.05);
grid1.setInputCloud (target_cloud);
grid1.filter(*target_cloud);
cout << "target cloud number of point after voxelgrid: " << target_cloud->points.size() << endl;
//----------------------------------------------------------------------------------
//Make source cloud blue
//----------------------------------------------------------------------------------
for (int i = 0; i < source_cloud->points.size(); ++i) {
source_cloud->points[i].r = 0;
source_cloud->points[i].g = 0;
source_cloud->points[i].b = 255;
}
//----------------------------------------------------------------------------------
//Make Target cloud Red
//----------------------------------------------------------------------------------
for (int i = 0; i < target_cloud->points.size(); ++i) {
target_cloud->points[i].r = 255;
target_cloud->points[i].g = 0;
target_cloud->points[i].b = 0;
}
//----------------------------------------------------------------------------------
//Apply PCA transformation from target to source
//----------------------------------------------------------------------------------
if(PCAregistration == true){
Eigen::Vector4f centroid_source, centroid_target;
Eigen::Matrix4f transformationm_source, transformationm_target, PCAtransformation;
applyPCAregistration(source_cloud, centroid_source, transformationm_source);
applyPCAregistration(target_cloud, centroid_target, transformationm_target);
PCAtransformation = transformationm_source * transformationm_target.transpose();
//Apply rotation transformation
pcl::transformPointCloud(*source_cloud, *transformed_cloud, PCAtransformation);
//calculate fitnesscore
FitnesscorePCA = calculateFitnesscore(target_cloud,source_cloud, PCAtransformation);
cout << "FitnesscorePCA is : " << FitnesscorePCA << " meter "<< endl;
}
//----------------------------------------------------------------------------------
//Apply SVD transformation from target to source
//----------------------------------------------------------------------------------
if(SVDregistration == true){
Eigen::Matrix4f trans_matrix_svd;
applySVDregistration(source_cloud, target_cloud, trans_matrix_svd);
pcl::transformPointCloud (*source_cloud, *transformed_cloud, trans_matrix_svd);
//calculate fitnesscore
FitnesscoreSVD = calculateFitnesscore(target_cloud,source_cloud, trans_matrix_svd);
cout << "FitnesscoreSVD is : " << FitnesscoreSVD << " meter "<< endl;
}
//----------------------------------------------------------------------------------
//Make transformed cloud White
//----------------------------------------------------------------------------------
for (int i = 0; i < transformed_cloud->points.size(); ++i) {
transformed_cloud->points[i].r = 255;
transformed_cloud->points[i].g = 255;
transformed_cloud->points[i].b = 255;
}
//----------------------------------------------------------------------------------
//Show cloud in viewer
//----------------------------------------------------------------------------------
viewer.showCloud (source_cloud, "source");
viewer.showCloud (transformed_cloud, "transformed");
viewer.showCloud (target_cloud, "target");
while (!viewer.wasStopped ())
{
//while keypress "q" is pressed
}
return (0);
}
void cloud_cb_ (const sensor_msgs::PointCloud2ConstPtr& input)
{
if (!viewer.wasStopped()){
// if(!cloud_received){
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr in_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg (*input, *in_cloud);
//downsample
pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
downSample(in_cloud, downsampled_cloud);
//crop
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::IndicesPtr indices (new std::vector <int>);
cropCloud(downsampled_cloud, cropped_cloud, indices);
//rotate
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rotated_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
rotateCloud(cropped_cloud, rotated_cloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
colorSegment(rotated_cloud, filtered_cloud);
/*
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (msg);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
return;
//return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
cloud.points.resize(inliers->indices.size ());
cloud.width=1;
cloud.height= inliers->indices.size ();
for (size_t i = 0; i < inliers->indices.size (); ++i){
cloud.points[i].x = (*msg).points[inliers->indices[i]].x;
cloud.points[i].y = (*msg).points[inliers->indices[i]].y;
cloud.points[i].z = (*msg).points[inliers->indices[i]].z;
//std::cerr << inliers->indices[i] << " " << (*msg).points[inliers->indices[i]].x << " "
// << (*msg).points[inliers->indices[i]].y << " "
// << (*msg).points[inliers->indices[i]].z << std::endl;
}
//cloud_received = true;
}
*/
//return (0);
cv::waitKey(3);
viewer.showCloud(filtered_cloud);
//viewer.showCloud(msg);
}
}
