void rawCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloud)
{
//double timeScanCur = laserCloud->header.stamp.toSec();
ros::Time timestamp = laserCloud->header.stamp;
//bool returnBool;
pcl::fromROSMsg(*laserCloud, *laserCloudIn);
if (visualizationFlag)
{
viewer->removeAllPointClouds(0);
viewer->removeAllShapes(0);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudLabeled(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudRaw(new pcl::PointCloud<pcl::PointXYZI>);
// std::vector<std::string> labelsC1;
// std::vector<std::string> labelsC2;
// pcl::PointCloud<SAFEFeatures>::Ptr featureCloudC1Acc(new pcl::PointCloud<SAFEFeatures>);
// pcl::PointCloud<SAFEFeatures>::Ptr featureCloudC2Acc(new pcl::PointCloud<SAFEFeatures>);
std::vector<std::string> labelsC1;
std::vector<std::string> labelsC2;
std::vector<std::string> labelsC3;
std::vector<std::string> labels;
pcl::PointCloud<AllFeatures>::Ptr featureCloudAcc(new pcl::PointCloud<AllFeatures>);
pcl::PointCloud<AllFeatures>::Ptr featureCloudC1Acc(new pcl::PointCloud<AllFeatures>);
pcl::PointCloud<AllFeatures>::Ptr featureCloudC2Acc(new pcl::PointCloud<AllFeatures>);
pcl::PointCloud<AllFeatures>::Ptr featureCloudC3Acc(new pcl::PointCloud<AllFeatures>);
std::vector<Eigen::Matrix3f> confMats;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr classificationCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<DatasetContainer> dataset;
std::string folder = "/home/mikkel/catkin_ws/src/trainingSet/";
pcl::copyPointCloud(*laserCloudIn, *cloudRGB);
pcl::copyPointCloud(*laserCloudIn, *cloudRaw);
// Single colored
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color1(cloudRGB, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZRGB>(cloudRGB,color1,"cloudRGB",viewP(RawCloud));
}
//viewer->addText ("RawCloud", 10, 10, "vPP text", viewP(RawCloud));
// Ground modeling
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudStat(new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::PointCloud<pcl::PointXYZ>::Ptr planeDistCloud(new pcl::PointCloud<pcl::PointXYZ>);
// pcl::copyPointCloud(*cloudRGB,*cloudStat);
// pcl::copyPointCloud(*cloudRGB,*planeDistCloud);
// Remove everything outside the two lines
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);
//pcl::copyPointCloud(*cloudRaw, *cloud_filtered);
// Rotation
float thetaZ = theta*PI/180; // The angle of rotation in radians
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.rotate (Eigen::AngleAxisf (thetaZ, Eigen::Vector3f::UnitZ()));
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::transformPointCloud (*cloudRaw, *cloud_filtered, transform_2);
// Passthrough
pcl::PassThrough<pcl::PointXYZI> passX;
passX.setInputCloud (cloud_filtered);
passX.setFilterFieldName ("x");
passX.setFilterLimits (-0.5, 100.0);
//pass.setFilterLimitsNegative (true);
passX.filter (*cloud_filtered);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filteredRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
if (receivedHough)
{
pcl::PassThrough<pcl::PointXYZI> pass;
pass.setInputCloud (cloud_filtered);
pass.setFilterFieldName ("y");
pass.setFilterLimits (r1+WALL_CLEARANCE, r2-WALL_CLEARANCE);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered);
}
pcl::copyPointCloud(*cloud_filtered, *cloud_filteredRGB);
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color9(cloudRGB, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZRGB>(cloud_filteredRGB,color9,"LinesFiltered",viewP(LinesFiltered));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "LinesFiltered");
viewer->addText ("LinesFiltered", 10, 10, fontsize, 1, 1, 1, "LinesFiltered text", viewP(LinesFiltered));
}
// Grid Minimum
// pcl::PointCloud<pcl::PointXYZI>::Ptr gridCloud(new pcl::PointCloud<pcl::PointXYZI>);
// pcl::GridMinimum<pcl::PointXYZI> gridm(1.0); // Set grid resolution
// gridm.setInputCloud(cloud_filtered);
// gridm.filter(*gridCloud);
//*** Transform point cloud to adjust for a Ground Plane ***//
pcl::ModelCoefficients ground_coefficients;
pcl::PointIndices ground_indices;
pcl::SACSegmentation<pcl::PointXYZI> ground_finder;
ground_finder.setOptimizeCoefficients(true);
ground_finder.setModelType(pcl::SACMODEL_PLANE);
ground_finder.setMethodType(pcl::SAC_RANSAC);
ground_finder.setDistanceThreshold(0.15);
ground_finder.setInputCloud(cloud_filtered);
ground_finder.segment(ground_indices, ground_coefficients);
// Convert plane normal vector from type ModelCoefficients to Vector4f
Eigen::Vector3f np = Eigen::Vector3f(ground_coefficients.values[0],ground_coefficients.values[1],ground_coefficients.values[2]);
// Find rotation vector, u, and angle, theta
Eigen::Vector3f u = np.cross(Eigen::Vector3f(0,0,1));
u.normalize();
float theta = acos(np.dot(Eigen::Vector3f(0,0,1)));
// Construct transformation matrix (rotation matrix from axis and angle)
Eigen::Affine3f tf = Eigen::Affine3f::Identity();
float d = ground_coefficients.values[3];
tf.translation() << d*np[0], d*np[1], d*np[2];
tf.rotate (Eigen::AngleAxisf (theta, u));
// Execute the transformation
pcl::PointCloud<pcl::PointXYZI>::Ptr transformedCloud (new pcl::PointCloud<pcl::PointXYZI> ());
pcl::transformPointCloud(*cloud_filtered, *transformedCloud, tf);
// Compute statistical moments for least significant direction in neighborhood
#if (OLD_METHOD)
pcl::NeighborhoodFeatures<pcl::PointXYZI, AllFeatures> features;
pcl::PointCloud<AllFeatures>::Ptr featureCloud(new pcl::PointCloud<AllFeatures>);
features.setInputCloud(transformedCloud);
pcl::search::KdTree<pcl::PointXYZI>::Ptr search_tree5( new pcl::search::KdTree<pcl::PointXYZI>());
features.setSearchMethod(search_tree5);
//principalComponentsAnalysis.setRadiusSearch(0.6);
//features.setKSearch(30);
features.setRadiusSearch(0.01); // This value does not do anything! Look inside "NeighborgoodFeatures.h" to see adaptive radius calculation.
features.compute(*featureCloud);
// ros::Time tFeatureCalculation2 = ros::Time::now();
// ros::Duration tFeatureCalculation = tFeatureCalculation2-tPreprocessing2;
// if (executionTimes==true)
// ROS_INFO("Feature calculation time = %i",tFeatureCalculation.nsec);
visualizeFeature(*cloudRGB, *featureCloud, 0, viewP(GPDistMean), "GPDistMean");
visualizeFeature(*cloudRGB, *featureCloud, 1, viewP(GPDistMin), "GPDistMin");
visualizeFeature(*cloudRGB, *featureCloud, 2, viewP(GPDistPoint), "GPDistPoint");
visualizeFeature(*cloudRGB, *featureCloud, 3, viewP(GPDistVar), "GPDistVar");
visualizeFeature(*cloudRGB, *featureCloud, 4, viewP(PCA1), "PCA1"); // 4 = PCA1
visualizeFeature(*cloudRGB, *featureCloud, 5, viewP(PCA2), "PCA2"); // 3 = GPVar
visualizeFeature(*cloudRGB, *featureCloud, 6, viewP(PCA3), "PCA3"); // 0 = GPMean
visualizeFeature(*cloudRGB, *featureCloud, 7, viewP(PCANX), "PCANX");
visualizeFeature(*cloudRGB, *featureCloud, 8, viewP(PCANY), "PCANY");
visualizeFeature(*cloudRGB, *featureCloud, 9, viewP(PCANZ), "PCANZ"); // 9 = PCANZ
visualizeFeature(*cloudRGB, *featureCloud, 10, viewP(PointDist), "PointDist");
visualizeFeature(*cloudRGB, *featureCloud, 11, viewP(RSS), "RSS");
visualizeFeature(*cloudRGB, *featureCloud, 12, viewP(Reflectance), "Reflectance");
ROS_INFO("Computed all neighborhood features");
std::vector<float>* centroid = new std::vector<float>();
std::vector<float>* stds = new std::vector<float>();
//*** Testing ***//
if (training == false)
{
Eigen::Matrix3f confMat = Eigen::Matrix3f::Zero();
if (testdata==true)
{
*featureCloud = *featureCloudAcc;
}
pcl::copyPointCloud(*cloudRGB,*classificationCloud);
// Load feature normalization parameters
std::ifstream input_file((folder + "centroid").c_str());
std::istream_iterator<float> start(input_file), end;
std::vector<float> numbers(start, end);
std::copy(numbers.begin(), numbers.end(), std::back_inserter(*centroid));
std::ifstream input_file2((folder + "stds").c_str());
std::istream_iterator<float> start2(input_file2), end2;
std::vector<float> numbers2(start2, end2);
std::copy(numbers2.begin(), numbers2.end(), std::back_inserter(*stds));
// Normalize features
// if (pcl::io::savePCDFile ("testFeaturesNonNormalized.pcd", *featureCloud) != 0)
// return (false);
ros::Time tNormalization1 = ros::Time::now();
normalizeFeatures(*featureCloud,*featureCloud, ¢roid, &stds);
ros::Time tNormalization2 = ros::Time::now();
ros::Duration tNormalization = tNormalization2-tNormalization1;
if (executionTimes==true)
ROS_INFO("Normalization time = %f",(float)(tNormalization.nsec)/1000000);
pcl::PointIndices::Ptr unclassifiedIndices (new pcl::PointIndices ());
// if (pcl::io::savePCDFile ("testFeatures.pcd", *featureCloud) != 0)
// return (false);
CvSVM SVM;
//int dim = sizeof(featureCloud->points[0])/sizeof(float);
int dim = useFeatures.size();//sizeof(useFeatures)/sizeof(int);
SVM.load((folder + "svm").c_str());
//SVM.load((folder + "svm2014-11-06-11-22-59").c_str());
//SVM.load((folder + "svm2014-11-07-14-00-31").c_str());
//SVM.load((folder + "svm2014-11-07-13-16-28").c_str());
ros::Time tClassification1 = ros::Time::now();
//int nMissingLabels = 0;
for (size_t i=0;i<classificationCloud->points.size();i++)
{
float dataSVM[dim];
for (int d=0;d<dim;d++)
{
//dataSVM[d] = featureCloud->points[i].data[d];
dataSVM[d] = featureCloud->points[i].data[useFeatures[d]];
}
Mat labelsMat(1, dim, CV_32FC1, dataSVM);
float response = SVM.predict(labelsMat);
if (response == 1.0)
classificationCloud->points[i].b = 255;
else if (response == 2.0)
classificationCloud->points[i].g = 255;
else if (response == 3.0)
classificationCloud->points[i].r = 255;
else
ROS_INFO("Another class label = %f",response);
if (testdata==true)
{
int groundTruth;
if (labels[i].compare("ground") == 0)
groundTruth = 1;
else if (labels[i].compare("vegetation") == 0)
groundTruth = 2;
else if (labels[i].compare("object") == 0)
groundTruth = 3;
confMat(groundTruth-1,(int)response-1)++;
}
}
ros::Time tClassification2 = ros::Time::now();
ros::Duration tClassification = tClassification2-tClassification1;
if (executionTimes==true)
ROS_INFO("Classification time = %f",(float)(tClassification.nsec)/100000);
//ROS_INFO("Number of missing class labels = %i", nMissingLabels);
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC(classificationCloud);
viewer->addPointCloud<pcl::PointXYZRGB>(classificationCloud,colorC,"Classification",viewP(Classification));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Classification");
viewer->addText ("Classification", 10, 10, fontsize, 1, 1, 1, "Classification text", viewP(Classification));
// Test set - calculate performance statistics
if (testdata == true)
{
std::cout << confMat << std::endl;
confMats.push_back(confMat);
if (true)//k==files.size()-1)
{
ofstream myfile;
string resultsFolder = "/home/mikkel/catkin_ws/src/Results/";
myfile.open ((resultsFolder + "run_number_here.txt").c_str());
// Distance threshold
myfile << "DistanceThreshold:" << distThr << ";\n";
// Neighborhood parameters
myfile << "Neighborhood:" << rmin << ";" << rmindist << ";" << rmax << ";" << rmaxdist << ";\n";
// Features
myfile << "Features:";
for (int d=0;d<dim;d++)
myfile << useFeatures[d] << ";";
myfile << "\n";
myfile << "ConfusionMatrix:";
for (size_t i=0;i<confMats.size();i++)
{
for (int r=0;r<confMats[i].rows();r++)
for (int c=0;c<confMats[i].cols();c++)
myfile << confMats[i](r,c) << ";";
myfile << "\n";
}
myfile << "Accuracy:" << confMat.diagonal().sum()/confMat.sum() << ";\n";
myfile.close();
}
}
featureCloudAcc->clear();
labels.clear();
#else
for (size_t i=0;i<transformedCloud->size();i++)
{
pcl::PointXYZI pTmp2 = transformedCloud->points[i];
pcl::PointXYZRGB pTmp;
pTmp.x = pTmp2.x;
pTmp.y = pTmp2.y;
pTmp.z = pTmp2.z;
pTmp.r = 255;
pTmp.g = 255;
if (pTmp.z > 0.1) // Object
{
classificationCloud->push_back(pTmp);
}
}
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC(classificationCloud);
viewer->addPointCloud<pcl::PointXYZRGB>(classificationCloud,colorC,"Classification",viewP(Classification));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Classification");
viewer->addText ("Classification", 10, 10, fontsize, 1, 1, 1, "Classification text", viewP(Classification));
}
#endif
// REGION GROWING
//ROS_INFO("region growing 5");
pcl::PointCloud <pcl::PointXYZRGB>::Ptr objectCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::PointCloud <pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZRGB>);
for (size_t i=0;i<classificationCloud->size();i++)
{
pcl::PointXYZRGB pTmp = classificationCloud->points[i];
if ((pTmp.r == 255) || (pTmp.g == 255)) // Object
{
objectCloud->push_back(pTmp);
}
}
pcl::RegionGrowing<pcl::PointXYZRGB, pcl::Normal> reg;
reg.setInputCloud (objectCloud);
//reg.setIndices (indices);
pcl::search::Search<pcl::PointXYZRGB>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> > (new pcl::search::KdTree<pcl::PointXYZRGB>);
reg.setSearchMethod (tree);
//reg.setDistanceThreshold (0.0001f);
//reg.setResidualThreshold(0.01f);
//ROS_INFO("res = %f",reg.getSmoothnessThreshold());
//reg.setPointColorThreshold (6);
//reg.setRegionColorThreshold (5);
reg.setMinClusterSize (1);
reg.setResidualThreshold(min_cluster_distance);
reg.setCurvatureTestFlag(false);
// reg.setResidualTestFlag(true);
// reg.setNormalTestFlag(false);
// reg.setSmoothModeFlag(false);
std::vector <pcl::PointIndices> clusters;
reg.extract (clusters);
//ROS_INFO("clusters = %d", clusters.size());
//std::cout << "testefter" << std::endl;
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
pcl::PointCloud <pcl::PointXYZRGB>::Ptr clustersFilteredCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector <pcl::PointIndices> clustersFiltered;
//pcl::PointXYZRGB min_pt, max_pt;
Eigen::Vector4f min_pt, max_pt;
obstacle_detection::boundingbox bbox;
obstacle_detection::boundingboxes bboxes;
bboxes.header.stamp = timestamp;
bboxes.header.frame_id = "/velodyne";
if (visualizationFlag)
viewer->removeAllShapes(viewP(SegmentsFiltered));
for (size_t i=0;i<clusters.size();i++)
{
//ROS_INFO("cluster size = %d",clusters[i].indices.size());
if (clusters[i].indices.size() > 100)
{
clustersFiltered.push_back(clusters[i]);
for (size_t pp=0;pp<clusters[i].indices.size();pp++)
{
clustersFilteredCloud->push_back(colored_cloud->points[clusters[i].indices[pp]]);
}
// Calculate bounding box
pcl::getMinMax3D(*colored_cloud, clusters[i], min_pt, max_pt);
bbox.start.x = min_pt[0];
bbox.start.y = min_pt[1];
bbox.start.z = min_pt[2];
bbox.width.x = max_pt[0]-min_pt[0];
bbox.width.y = max_pt[1]-min_pt[1];
bbox.width.z = max_pt[2]-min_pt[2];
bbox.header.stamp = timestamp;
bbox.header.frame_id = "/velodyne";
bboxes.boundingboxes.push_back(bbox);
if (visualizationFlag)
viewer->addCube (min_pt[0], max_pt[0], min_pt[1], max_pt[1], min_pt[2], max_pt[2], 1.0f, 1.0f, 1.0f, (boost::format("%04d") % i).str().c_str(), viewP(SegmentsFiltered));
}
}
pubBBoxesPointer->publish(bboxes);
//
// //pcl::visualization::CloudViewer viewer ("Cluster viewer");
// //viewer.showCloud (colored_cloud);
//
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC2(colored_cloud);
viewer->addPointCloud<pcl::PointXYZRGB>(colored_cloud,colorC2,"Segments",viewP(Segments));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Segments");
viewer->addText ("Segments", 10, 10, fontsize, 1, 1, 1, "Segments text", viewP(Segments));
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC3(clustersFilteredCloud);
viewer->addPointCloud<pcl::PointXYZRGB>(clustersFilteredCloud,colorC3,"SegmentsFiltered",viewP(SegmentsFiltered));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "SegmentsFiltered");
viewer->addText ("Segments", 10, 10, fontsize, 1, 1, 1, "SegmentsFiltered text", viewP(SegmentsFiltered));
}
//
//
// // BOUNDING BOXES
// viewer->addCube (min_pt.x, max_pt.x, min_pt.y, max_pt.y, min_pt.z, max_pt.z);
// std_msgs::Float64 testF;
// //testF.data = 10;
// testF.data = 10;
//
// Compute principal directions
// Eigen::Vector4f pcaCentroid;
// pcl::compute3DCentroid(*clustersFilteredCloud, pcaCentroid);
// Eigen::Matrix3f covariance;
// computeCovarianceMatrixNormalized(*clustersFilteredCloud, pcaCentroid, covariance);
// Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
// Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
// eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
//
// // Transform the original cloud to the origin where the principal components correspond to the axes.
// Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
// projectionTransform.block<3,3>(0,0) = eigenVectorsPCA.transpose();
// projectionTransform.block<3,1>(0,3) = -1.f * (projectionTransform.block<3,3>(0,0) * pcaCentroid.head<3>());
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudPointsProjected (new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::transformPointCloud(*clustersFilteredCloud, *cloudPointsProjected, projectionTransform);
// // Get the minimum and maximum points of the transformed cloud.
// pcl::PointXYZRGB minPoint, maxPoint;
// pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
// const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());
//
// // Final transform
// const Eigen::Quaternionf bboxQuaternion(eigenVectorsPCA); //Quaternions are a way to do rotations https://www.youtube.com/watch?v=mHVwd8gYLnI
// const Eigen::Vector3f bboxTransform = eigenVectorsPCA * meanDiagonal + pcaCentroid.head<3>();
// viewer->addCube(bboxTransform, bboxQuaternion, maxPoint.x - minPoint.x, maxPoint.y - minPoint.y, maxPoint.z - minPoint.z, "bbox");
//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> colorTTF1(featureCloudTest, 0, 255, 0);
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorTTF1(featureCloudTest);
// PCL_INFO("featureCloudTest size = %i",featureCloudTest->points.size());
// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> colorTTF2(featureCloudTrain, 255, 0, 0);
// viewer->addPointCloud<pcl::PointXYZRGB>(featureCloudTest,colorTTF1,"TestTrainFeatures1",viewP(TestTrainFeatures));
// viewer->addPointCloud<pcl::PointXYZRGB>(featureCloudTrain,colorTTF2,"TestTrainFeatures2",viewP(TestTrainFeatures));
// viewer->addText ("TestTrainFeatures", 10, 10, "TestTrainFeatures text", viewP(TestTrainFeatures));
// sensor_msgs::PointCloud2 processedCloud;
// pcl::toROSMsg(*classificationCloud, processedCloud);
// processedCloud.header.stamp = ros::Time::now();//processedCloud->header.stamp;
// processedCloud.header.frame_id = "/velodyne";
// pubProcessedPointCloudPointer->publish(processedCloud);
//
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud2D(new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr objectCloud2D(new pcl::PointCloud<pcl::PointXYZRGB>);
//
// for (size_t i=0;i<classificationCloud->size();i++)
// {
// pcl::PointXYZRGB pTmp = classificationCloud->points[i];
// pTmp.z = 0; // 3D -> 2D
// if ((pTmp.r == 255) || (pTmp.g == 255)) // Object
// {
// objectCloud2D->push_back(pTmp);
// }
// else if (pTmp.b == 255) // Object
// {
// groundCloud2D->push_back(pTmp);
// }
// }
//
// //std::vector<int> indexVector;
// unsigned int leafNodeCounter = 0;
// unsigned int voxelDensity = 0;
// unsigned int totalPoints = 0;
// //int occupancyW = OCCUPANCY_WIDTH/OCCUPANCY_GRID_RESOLUTION;
// //int occupancyH = OCCUPANCY_DEPTH/OCCUPANCY_GRID_RESOLUTION;
// std::vector<unsigned int> ocGroundGrid(OCCUPANCY_WIDTH*OCCUPANCY_DEPTH, 0);
// std::vector<unsigned int> ocObjectGrid(OCCUPANCY_WIDTH*OCCUPANCY_DEPTH, 0);
// std::vector<signed char> ocGridFinal(OCCUPANCY_WIDTH*OCCUPANCY_DEPTH,-1);
//
// // Ground grid
// pcl::octree::OctreePointCloudDensity< pcl::PointXYZRGB > octreeGround (OCCUPANCY_GRID_RESOLUTION);
// octreeGround.setInputCloud (groundCloud2D);
// pcl::PointXYZ bot(-OCCUPANCY_DEPTH_M/2,-OCCUPANCY_WIDTH_M/2,-0.5);
// pcl::PointXYZ top(OCCUPANCY_DEPTH_M/2,OCCUPANCY_WIDTH_M/2,0.5);
// octreeGround.defineBoundingBox(bot.x, bot.y, bot.z, top.x, top.y, top.z); // I have already calculated the BBox of my point cloud
// octreeGround.addPointsFromInputCloud ();
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it1;
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it1_end = octreeGround.leaf_end();
// int minx = 1000;
// int miny = 1000;
// int maxx = -1000;
// int maxy = -1000;
// for (it1 = octreeGround.leaf_begin(); it1 != it1_end; ++it1)
// {
// pcl::octree::OctreePointCloudDensityContainer& container = it1.getLeafContainer();
// voxelDensity = container.getPointCounter();
// Eigen::Vector3f min_pt, max_pt;
// octreeGround.getVoxelBounds (it1, min_pt, max_pt);
// //ROS_INFO("x = %f, y = %f, z = %f, x2 = %f, y2 = %f, z2 = %f",min_pt[0],min_pt[1],min_pt[2],max_pt[0],max_pt[1],max_pt[2]);
// totalPoints += voxelDensity;
// if (min_pt[0] < minx)
// minx = min_pt[0];
// if (min_pt[0] > maxx)
// maxx = min_pt[0];
// if (min_pt[1] < miny)
// miny = min_pt[1];
// if (min_pt[1] > maxy)
// maxy = min_pt[1];
//
// int r = OCCUPANCY_DEPTH-(min_pt[0]+OCCUPANCY_DEPTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
// int c = OCCUPANCY_WIDTH-(min_pt[1]+OCCUPANCY_WIDTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
//
// // if ((r == 51) || (c==51))
// // ROS_INFO("r = %i,c = %i",r,c);
//
// //if (((int)min_pt[0] == -1) || ((int)min_pt[1]==-1))
// //ROS_INFO("min_pt[0] = %f, min_pt[1]=%f",min_pt[0],min_pt[1]);
// if (!((r < 0) || (c < 0) || (r > OCCUPANCY_DEPTH-1) || (c > OCCUPANCY_WIDTH-1)))
// ocGroundGrid[r+OCCUPANCY_DEPTH*c] = voxelDensity;
// //ROS_INFO("density = %i, r = %i, c=%i",voxelDensity, r, c);
// //ROS_INFO("x (%f) -> r (%i), y (%f) -> c (%i)",min_pt[0],r,min_pt[1],c);
// leafNodeCounter++;
// }
//
//
// // Object grid
// pcl::octree::OctreePointCloudDensity< pcl::PointXYZRGB > octreeObject (OCCUPANCY_GRID_RESOLUTION);
// octreeObject.setInputCloud (objectCloud2D);
// octreeObject.defineBoundingBox(bot.x, bot.y, bot.z, top.x, top.y, top.z); // I have already calculated the BBox of my point cloud
// octreeObject.addPointsFromInputCloud ();
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it2;
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it2_end = octreeObject.leaf_end();
// minx = 1000;
// miny = 1000;
// maxx = -1000;
// maxy = -1000;
// leafNodeCounter = 0;
// voxelDensity = 0;
// totalPoints = 0;
// int t1 = 0;
// int t2 = 0;
// int t3 = 0;
// for (it2 = octreeObject.leaf_begin(); it2 != it2_end; ++it2)
// {
// pcl::octree::OctreePointCloudDensityContainer& container = it2.getLeafContainer();
// voxelDensity = container.getPointCounter();
// Eigen::Vector3f min_pt, max_pt;
// octreeObject.getVoxelBounds (it2, min_pt, max_pt);
// //ROS_INFO("x = %f, y = %f, z = %f, x2 = %f, y2 = %f, z2 = %f",min_pt[0],min_pt[1],min_pt[2],max_pt[0],max_pt[1],max_pt[2]);
// totalPoints += voxelDensity;
// if (min_pt[0] < minx)
// minx = min_pt[0];
// if (min_pt[0] > maxx)
// maxx = min_pt[0];
// if (min_pt[1] < miny)
// miny = min_pt[1];
// if (min_pt[1] > maxy)
// maxy = min_pt[1];
//
// int r = OCCUPANCY_DEPTH-(min_pt[0]+OCCUPANCY_DEPTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
// int c = OCCUPANCY_WIDTH-(min_pt[1]+OCCUPANCY_WIDTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
// if (!((r < 0) || (c < 0) || (r > OCCUPANCY_DEPTH-1) || (c > OCCUPANCY_WIDTH-1)))
// ocObjectGrid[r+OCCUPANCY_DEPTH*c] = voxelDensity;
// //ROS_INFO("object points = %i -> %i: x (%f) -> r (%i), y (%f) -> c (%i)",voxelDensity,ocGridFinal[r+OCCUPANCY_DEPTH*c],min_pt[0],r,min_pt[1],c);
// //ocGrid[r+OCCUPANCY_DEPTH*c] = voxelDensity;
// //ROS_INFO("density = %i, r = %i, c=%i",voxelDensity, r, c);
//
// leafNodeCounter++;
// }
//
// for (int r=0;r<OCCUPANCY_DEPTH;r++)
// {
// for (int c=0;c<OCCUPANCY_WIDTH;c++)
// {
// if ((ocGroundGrid[r+OCCUPANCY_DEPTH*c] == 0) && (ocObjectGrid[r+OCCUPANCY_DEPTH*c] <= 1))
// {
// ocGridFinal[r+OCCUPANCY_DEPTH*c] = -1.0; // 0.5 default likelihood
// t1++;
// }
// else if ((ocObjectGrid[r+OCCUPANCY_DEPTH*c] == 0))// && (ocGroundGrid[r+OCCUPANCY_DEPTH*c] > 0))
// {
// ocGridFinal[r+OCCUPANCY_DEPTH*c] = OCCUPANCY_MIN_PROB;
// t2++;
// }
// else
// {
// ocGridFinal[r+OCCUPANCY_DEPTH*c] = 50+(OCCUPANCY_MAX_PROB-50)*ocObjectGrid[r+OCCUPANCY_DEPTH*c]/(ocObjectGrid[r+OCCUPANCY_DEPTH*c]+ocGroundGrid[r+OCCUPANCY_DEPTH*c]);
// t3++;
// }
// }
// }
//
// nav_msgs::OccupancyGrid occupancyGrid;
// occupancyGrid.info.resolution = OCCUPANCY_GRID_RESOLUTION;
// occupancyGrid.header.stamp = timestamp;//ros::Time::now();
// occupancyGrid.header.frame_id = "/velodyne";
// occupancyGrid.info.width = OCCUPANCY_WIDTH;
// occupancyGrid.info.height = OCCUPANCY_DEPTH;
// //geometry_msgs::Pose lidarPose;
// geometry_msgs::Point lidarPoint;
// lidarPoint.x = 0;
// lidarPoint.y = 0;
// lidarPoint.z = 0;
// geometry_msgs::Quaternion lidarQuaternion;
// lidarQuaternion.w = 1;
// lidarQuaternion.x = 0;
// lidarQuaternion.y = 0;
// lidarQuaternion.z = 0;
// occupancyGrid.info.origin.position = lidarPoint;
// occupancyGrid.info.origin.orientation = lidarQuaternion;
// occupancyGrid.data = ocGridFinal;
//
// //ROS_INFO("total points = %i, total leaves = %i",totalPoints,leafNodeCounter);
// //ROS_INFO("minx = %i, maxx = %i, miny = %i, maxy = %i",minx,maxx,miny,maxy);
// //ROS_INFO("t1 = %i, t2 = %i, t3 = %i",t1,t2,t3);
//
//
// pubOccupancyGridPointer->publish(occupancyGrid);
// int l = 0;
// while (*++itLeafs)
// {
// //Iteratively explore only the leaf nodes..
// std::vector<PointXYZ> points;
// itLeafs->getData(points);
// l++;
// }
//
// ROS_INFO("l = %i",l);
//pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (OCCUPANCY_GRID_RESOLUTION);
// sensor_msgs::PointCloud2::Ptr cloud_filtered (new sensor_msgs::PointCloud2 ());
// pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
// sor.setInputCloud (processedCloud);
// sor.setLeafSize (0.01f, 0.01f, 0.01f);
// sor.filter (*cloud_filtered);
if (n==0)
{
//viewer->setCameraPosition(-20,0,10,1,0,2,1,0,2,v1);
//viewer->setCameraPosition(-20,0,10,1,0,2,1,0,2,v2);
//viewer->loadCameraParameters("pcl_video.cam");
}
#if (OLD_METHOD)
}
#endif
if (visualizationFlag)
viewer->spinOnce();
// Save screenshot
// std::stringstream tmp;
// tmp << n;
// viewer->saveScreenshot("/home/mikkel/images/" + (boost::format("%04d") % n).str() + ".png");
// n++;
}
