pcl::PointCloud<MapCloudGenerator::PointT>::Ptr MapCloudGenerator::generate(const std::vector<KeyFrameSnapshot::Ptr>& keyframes, double resolution) const {
if(keyframes.empty()) {
return nullptr;
}
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
cloud->reserve(keyframes.front()->cloud->size() * keyframes.size());
for(const auto& keyframe : keyframes) {
Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();
for(const auto& src_pt : keyframe->cloud->points) {
PointT dst_pt;
dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
dst_pt.intensity = src_pt.intensity;
cloud->push_back(dst_pt);
}
}
cloud->width = cloud->size();
cloud->height = 1;
cloud->is_dense = false;
pcl::octree::OctreePointCloud<PointT> octree(resolution);
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
octree.getOccupiedVoxelCenters(filtered->points);
filtered->width = filtered->size();
filtered->height = 1;
filtered->is_dense = false;
return filtered;
}
std::vector<float>
WorkerStemFit::compute_distances(std::vector<pcl::ModelCoefficients> &cylinders)
{
std::vector<float> distances ( _cloud->points.size () );
std::fill ( distances.begin (), distances.end (), 0.5 );
pcl::octree::OctreePointCloudSearch<PointI> octree ( 0.02f );
octree.setInputCloud ( _cloud );
octree.addPointsFromInputCloud ();
for (size_t i = 0; i < cylinders.size(); i++) {
pcl::ModelCoefficients cylinder = cylinders.at(i);
std::vector<int> indices = indexOfPointsNearCylinder ( octree, cylinder );
for ( size_t i = 0; i < indices.size (); i++ ) {
PointI point = _cloud->points[indices[i]];
simpleTree::Cylinder cyl (cylinder);
float dist = cyl.distToPoint ( point );
if ( std::abs ( dist ) < std::abs ( distances[indices[i]] ) ) {
distances[indices[i]] = dist;
}
}
}
return distances;
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
pcl::octree::OctreePointCloudSearch<PointInT> octree (octree_leaf_size_);
octree.setInputCloud (input_);
octree.addPointsFromInputCloud ();
typename pcl::PointCloud<PointInT>::VectorType occupied_cells;
octree.getOccupiedVoxelCenters (occupied_cells);
// Determine the voxels crosses along the line segments
// formed by every pair of occupied cells.
std::vector< std::vector<int> > line_histograms;
for (size_t i = 0; i < occupied_cells.size (); ++i)
{
Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();
for (size_t j = i+1; j < occupied_cells.size (); ++j)
{
typename pcl::PointCloud<PointInT>::VectorType intersected_cells;
Eigen::Vector3f end = occupied_cells[j].getVector3fMap ();
octree.getApproxIntersectedVoxelCentersBySegment (origin, end, intersected_cells, 0.5f);
// Intersected cells are ordered from closest to furthest w.r.t. the origin.
std::vector<int> histogram;
for (size_t k = 0; k < intersected_cells.size (); ++k)
{
std::vector<int> indices;
octree.voxelSearch (intersected_cells[k], indices);
int label = emptyLabel ();
if (indices.size () != 0)
{
label = getDominantLabel (indices);
}
histogram.push_back (label);
}
line_histograms.push_back(histogram);
}
}
std::vector< std::vector<int> > transition_histograms;
computeTransitionHistograms (line_histograms, transition_histograms);
std::vector<float> distances;
computeDistancesToMean (transition_histograms, distances);
std::vector<float> gfpfh_histogram;
computeDistanceHistogram (distances, gfpfh_histogram);
output.clear ();
output.width = 1;
output.height = 1;
output.points.resize (1);
std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram);
}
void OctreeTerrain:: execute()
{
//qWarning()<<"octree terrain starts";
emit percentage( 5);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tmp(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tmp2(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tmp3(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tmp4(new pcl::PointCloud<pcl::PointXYZI>);
//velky cyklus
emit percentage( 10);
octree(m_resolution*5, m_baseCloud->get_Cloud(), cloud_tmp, cloud_tmp2);
emit percentage( 70);
octree(m_resolution/2, cloud_tmp2, cloud_tmp3, cloud_tmp4);
emit percentage( 80);
*cloud_tmp += *cloud_tmp3;
m_vegetation->set_Cloud(cloud_tmp);
m_terrain->set_Cloud(cloud_tmp4);
emit percentage( 90);
sendData();
}
void OctreeVisualizer::incomingOctreeCallback()
{
scan_utils::Octree<char> octree(0, 0, 0, 0, 0, 0, 1, 0);
octree.setFromMsg(octree_message_);
std::list<scan_utils::Triangle> triangles;
octree.getAllTriangles(triangles);
V_Vector3 vertices;
V_Vector3 normals;
vertices.resize( triangles.size() * 3 );
normals.resize( triangles.size() );
size_t vertexIndex = 0;
size_t normalIndex = 0;
std::list<scan_utils::Triangle>::iterator it = triangles.begin();
std::list<scan_utils::Triangle>::iterator end = triangles.end();
for ( ; it != end; it++ )
{
Ogre::Vector3& v1 = vertices[vertexIndex++];
Ogre::Vector3& v2 = vertices[vertexIndex++];
Ogre::Vector3& v3 = vertices[vertexIndex++];
Ogre::Vector3& n = normals[normalIndex++];
v1 = Ogre::Vector3( it->p1.x, it->p1.y, it->p1.z );
v2 = Ogre::Vector3( it->p2.x, it->p2.y, it->p2.z );
v3 = Ogre::Vector3( it->p3.x, it->p3.y, it->p3.z );
robotToOgre(v1);
robotToOgre(v2);
robotToOgre(v3);
n = ( v2 - v1 ).crossProduct( v3 - v1 );
n.normalise();
}
triangles_mutex_.lock();
vertices_.clear();
normals_.clear();
vertices.swap( vertices_ );
normals.swap( normals_ );
new_message_ = true;
triangles_mutex_.unlock();
}
//get intersection points
void get_intr_points(MyPointCloud& source_mpc, MyPointCloud& sample_mpc, float search_r, int* intr_points_num){
*intr_points_num=0;
PointCloudPtr source_cloud(new PointCloud);
MyPointCloud2PointCloud(source_mpc, source_cloud);
PointCloudPtr sample_cloud(new PointCloud);
MyPointCloud2PointCloud(sample_mpc, sample_cloud);
PointCloudPtr intr_cloud(new PointCloud);
float resolution = 0.005f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);
octree.setInputCloud (source_cloud);
octree.addPointsFromInputCloud ();
for(int i=0;i<sample_mpc.mypoints.size();i++){
// Neighbors within radius search
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
float radius = search_r;
if (octree.radiusSearch(sample_cloud->at(i), radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
if(pointIdxRadiusSearch.size()>0){
PointCloudPtr cloud_tem(new PointCloud);
for (size_t j = 0; j < pointIdxRadiusSearch.size (); ++j){
cloud_tem->push_back(source_cloud->points[ pointIdxRadiusSearch[j]]);
}
Point finded_pt;
if(findNearestNeighbor(cloud_tem, intr_cloud, sample_cloud->at(i), finded_pt)){
intr_cloud->push_back(finded_pt);
(*intr_points_num)+=1;
}
}
}
}
}
int getDifferencesCloud(const PCPtr& src,
const PCPtr& tgt,
PCPtr& diff,
float octreeRes)
{
pcl::octree::OctreePointCloudChangeDetector<PCXYZ> octree (octreeRes);
std::vector<int> newPointIdxVector;
octree.setInputCloud(src);
octree.addPointsFromInputCloud();
octree.switchBuffers();
octree.setInputCloud(tgt);
octree.addPointsFromInputCloud();
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
diff->points.reserve(newPointIdxVector.size());
for (std::vector<int>::iterator it = newPointIdxVector.begin(); it != newPointIdxVector.end(); it++)
diff->points.push_back(tgt->points[*it]);
return newPointIdxVector.size();
}
void Viewer::SelfTest(int nx,int ny,int nz)
{
Log::printf("Testing viewer...\n");
Box3f world_box(Vec3f(0,0,0),Vec3f(nx,ny,nz));
SmartPointer<Batch> cube=Graph::cuboid(3)->getBatch();
std::vector<SmartPointer<Batch> > batches;
for (int x=0;x<nx;x++)
for (int y=0;y<ny;y++)
for (int z=0;z<nz;z++)
{
SmartPointer<Batch> batch(new Batch(*cube));
batch->matrix=Mat4f::translate(x,y,z) * Mat4f::scale(0.8f,0.8f,0.8f);
batches.push_back(batch);
}
SmartPointer<Octree> octree(new Octree(batches));
Viewer v(octree);
v.Run();
v.Wait();
}
int main(int argc, char** argv)
{
// create a random point cloud
srand((unsigned int) time(NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// Generate pointcloud data
cloud->width = 100;
cloud->height = 1;
cloud->points.resize(cloud->width * cloud->height);
for(size_t i = 0; i < cloud->points.size(); ++i)
{
cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
}
// octree
const float resolution = 128.0f;
//pcl::octree::OctreePointCloud<pcl::PointXYZ> octree(resolution);
//octree.setInputCloud(cloud);
//octree.addPointsFromInputCloud();
GPMap::OctreePointCloudVoxelGaussian3D<pcl::PointXYZ> octree(resolution);
octree.addPointsFromCloud(cloud);
//pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> centroid(resolution);
//centroid.setInputCloud(cloud);
//centroid.addPointsFromInputCloud();
//pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::LeafNodeIterator leafIter(centroid);
//while(*++leafIter)
//{
// dynamic_cast<OctreeContainerDataTVectorGaussian3D*>(*leafIter)->update(*input_);
//}
return 0;
}
void runTest(PointCloud& cloud, const std::string& name, std::default_random_engine& random)
{
std::cout << "****************************************" << std::endl;
std::cout << "Testing : " << name << std::endl;
std::cout << "****************************************" << std::endl;
Octree octree(cloud, 30);
std::cout << "Octree loaded !" << std::endl;
std::vector<SharedPlane> planes;
auto begin = std::chrono::steady_clock::now();
// (depthThreshold, epsilon, numStartPoints, numPoints, steps, countRatio, generateur, planes, colors, dCos)
octree.detectPlanes(100, 0.05, 10, 30, 10, 0.005, random, planes, cloud.colors(), std::cos(3.1415/180 * /*Angle in degrees: */ 15));
auto end = std::chrono::steady_clock::now();
std::chrono::duration<double> elapsed_secs = end - begin;
std::cout << std::endl << planes.size() << " planes :" << std::endl;
std::sort(planes.begin(), planes.end(), [](const SharedPlane& a, const SharedPlane& b){return a->getCount() < b->getCount();});
std::ofstream out((name + ".planes").c_str());
for (unsigned int i = 0 ; i < planes.size() ; ++i)
{
std::cout << *planes[i] << std::endl;
out << *planes[planes.size() - i - 1] << std::endl;
}
out.close();
std::cout << std::endl << "Running time: " << elapsed_secs.count() << " seconds." << std::endl;
std::cout << "Saving..." << std::endl;
cloud.toPly(name + ".ply", true);
std::cout << "****************************************" << std::endl;
std::cout << "End of test : " << name << std::endl;
std::cout << "****************************************" << std::endl;
}
std::vector<SmartPointer<Batch> > Batch::Optimize(std::vector<SmartPointer<Batch> > batches,int max_vertices_per_batch,int max_depth,float LOOSE_K)
{
std::vector<SmartPointer<Batch> > ret;
Clock t1;
//Log::printf("Optimizing the octree....\n");
//Log::printf(" Number of input batches %d\n",(int)batches.size());
//calculate world box
Box3f world_box;
for (int i=0;i<(int)batches.size();i++)
world_box.add(batches[i]->getBox());
//build the octree for doing optimization
Octree octree(world_box,max_depth,LOOSE_K);
for (int i=0;i<(int)batches.size();i++)
octree.getNode(batches[i]->getBox())->batches.push_back(batches[i]);
std::stack<OctreeNode*> stack;
stack.push(octree.root);
//count number of vertices
int tot_number_of_vertices=0;
while (stack.size())
{
OctreeNode* node=stack.top();
stack.pop();
for (int i=0;i<8;i++)
{if (node->childs[i]) stack.push(node->childs[i]);}
//rebuild the batches
std::set<int> merged;
int N=(int)node->batches.size();
for (int I=0;I<N;I++)
{
if (merged.find(I)!=merged.end()) continue;
SmartPointer<Batch> A=node->batches[I];
if (A->vertices)
{
int nva=A->vertices->size()/3;
tot_number_of_vertices+=nva;
for (int J=I+1;nva < max_vertices_per_batch && J<N;J++)
{
//already merged, skip
if (merged.find(J)!=merged.end()) continue;
SmartPointer<Batch> B=node->batches[J];
if (B->vertices)
{
int nvb=B->vertices->size()/3;
if ((nva+nvb)<=max_vertices_per_batch) //vertex limits
{
SmartPointer<Batch> C=Batch::Merge(A,B);
if (!C)
continue;
A=C;
merged.insert(J);
nva+=nvb;
}
}
}
//Log::printf(" num vertices %d\n",A->vertices->size()/3);
}
ret.push_back(A);
}
}
//Log::printf(" total number vertices %d\n",(int)tot_number_of_vertices);
//Log::printf(" Number of output batches %d\n",(int)ret.size());
//Log::printf(" Batch vertex media %d\n",(int)(tot_number_of_vertices/(float)ret.size()));
//Log::printf("...done in %d msec\n",(int)t1.msec());
return ret;
}
int
main (int argc, char** argv)
{
CloudPtr cloud_in (new Cloud);
CloudPtr cloud_out (new Cloud);
pcl::ScopeTime time("performance");
float endTime;
pcl::io::loadPCDFile (argv[1], *cloud_in);
std::cout << "Input size: " << cloud_in->width << " by " << cloud_in->height << std::endl;
///////////////////////////////////////////////////////////////////////////////////////////
//
pcl::PassThrough<PointType> pass;
pass.setInputCloud (cloud_in);
pass.setFilterLimitsNegative(1);
//pass.setKeepOrganized(true);
pass.setFilterFieldName ("x");
pass.setFilterLimits (1300, 2200.0);
pass.filter (*cloud_out);
pass.setInputCloud(cloud_out);
pass.setFilterFieldName ("y");
pass.setFilterLimits (8000, 10000);
pass.filter (*cloud_out);
//
pass.setFilterFieldName ("z");
pass.setFilterLimits (-2000, -200);
pass.filter (*cloud_out);
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZI> octree (10.0);
// Add points from cloudA to octree
octree.setInputCloud (cloud_out);
octree.addPointsFromInputCloud ();
// Switch octree buffers: This resets octree but keeps previous tree structure in memory.
octree.switchBuffers ();
// Add points from cloudB to octree
octree.setInputCloud (cloud_in);
octree.addPointsFromInputCloud ();
std::vector<int> newPointIdxVector;
// Get vector of point indices from octree voxels which did not exist in previous buffer
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
//
///////////////////////////////////////////////////////////////////////////////////////////
//
// std::vector<int> unused;
// pcl::removeNaNFromPointCloud (*cloud_in, *cloud_in, unused);
//
///////////////////////////////////////////////////////////////////////////////////////////
//
// pcl::search::Search<PointType>::Ptr searcher (new pcl::search::KdTree<PointType> (false));
// searcher->setInputCloud (cloud_in);
// PointType point;
// point.x = -10000;
// point.y = 0;
// point.z = 0;
// std::vector<int> k_indices;
// std::vector<float> unused2;
// //searcher->radiusSearch (point, 100, k_indices, unused2);
// searcher->nearestKSearch (point, 500000, k_indices, unused2);
// cloud_out->width = k_indices.size ();
// cloud_out->height = 1;
// cloud_out->is_dense = false;
// cloud_out->points.resize (cloud_out->width);
// for (size_t i = 0; i < cloud_out->points.size (); ++i)
// {
// cloud_out->points[i] = cloud_in->points[k_indices[i]];
// }
//
///////////////////////////////////////////////////////////////////////////////////////////
//
// cloud_out->width = cloud_in->width / 10;
// cloud_out->height = 1;
// cloud_out->is_dense = false;
// cloud_out->points.resize (cloud_out->width);
// for (size_t i = 0; i < cloud_out->points.size (); ++i)
// {
// cloud_out->points[i] = cloud_in->points[i * 10];
// }
//
///////////////////////////////////////////////////////////////////////////////////////////
// pcl::BilateralFilter<PointType> filter;
// filter.setInputCloud (cloud_in);
// pcl::octree::OctreeLeafDataTVector<int> leafT;
// pcl::search::Search<PointType>::Ptr searcher (new pcl::search::Octree
// < PointType,
// pcl::octree::OctreeLeafDataTVector<int> ,
// pcl::octree::OctreeBase<int, pcl::octree::OctreeLeafDataTVector<int> >
// > (500) );
// //pcl::search::Octree <PointType> ocTreeSearch(1);
// filter.setSearchMethod (searcher);
// double sigma_s, sigma_r;
//
// filter.setHalfSize (500);
// time.reset();
// filter.filter (*cloud_out);
// time.getTime();
// std::cout << "Benchmark Bilateral filer using: kdtree searching, sigma_s = 50, sigma_r = default" << std::endl;
// std::cout << "Results: clocks = " << end - start << ", clockspersec = " << CLOCKS_PER_SEC << std::endl;
// std::ofstream filestream;
// filestream.open ("timer_results.txt");
// char filename[50];
// for (sigma_s = 1000; sigma_s <= 1000; sigma_s *= 10)
// for (sigma_r = 0.001; sigma_r <= 1000; sigma_r *= 10)
// {
// std::cout << "sigma_s = " << sigma_s << ", sigma_r = " << sigma_r << " clockspersec = " << CLOCKS_PER_SEC
// << std::endl;
// filter.setHalfSize (sigma_s);
// filter.setStdDev (sigma_r);
// start = clock ();
// filter.filter (*cloud_out);
// end = clock ();
// sprintf (filename, "bruteforce-s%g-r%g.pcd", sigma_s, sigma_r);
// pcl::io::savePCDFileBinary (filename, *cloud_out);
// filestream << "sigma_s = " << sigma_s << ", sigma_r = " << sigma_r << " time = " << end - start
// << " clockspersec = " << CLOCKS_PER_SEC << std::endl;
// }
// filestream.close ();
// std::cout << "Output size: " << cloud_out->width << " by " << cloud_out->height << std::endl;
CloudPtr cloud_n (new Cloud);
CloudPtr cloud_buff (new Cloud);
pcl::io::loadPCDFile ("only_leaves.pcd", *cloud_n);
pcl::copyPointCloud(*cloud_in, newPointIdxVector, *cloud_buff);
// pcl::io::savePCDFileBinary<pcl::PointXYZI>("delt.pcd",*cloud_buff);
cloud_buff->operator+=(*cloud_n);
pcl::io::savePCDFileBinary<pcl::PointXYZI>("cropped_cloud.pcd",*cloud_buff);
std::cout << std::endl << "Goodbye World" << std::endl << std::endl;
return (0);
}
void
NMBasedCloudIntegration<PointT>::compute (typename pcl::PointCloud<PointT>::Ptr & output)
{
if(input_clouds_.empty())
{
LOG(ERROR) << "No input clouds set for cloud integration!";
return;
}
big_cloud_info_.clear();
collectInfo();
if(param_.reason_about_points_)
reasonAboutPts();
pcl::octree::OctreePointCloudPointVector<PointT> octree( param_.octree_resolution_ );
typename pcl::PointCloud<PointT>::Ptr big_cloud ( new pcl::PointCloud<PointT>());
big_cloud->width = big_cloud_info_.size();
big_cloud->height = 1;
big_cloud->points.resize( big_cloud_info_.size() );
for(size_t i=0; i < big_cloud_info_.size(); i++)
big_cloud->points[i] = big_cloud_info_[i].pt;
octree.setInputCloud( big_cloud );
octree.addPointsFromInputCloud();
typename pcl::octree::OctreePointCloudPointVector<PointT>::LeafNodeIterator leaf_it;
const typename pcl::octree::OctreePointCloudPointVector<PointT>::LeafNodeIterator it2_end = octree.leaf_end();
size_t kept = 0;
size_t total_used = 0;
std::vector<PointInfo> filtered_cloud_info ( big_cloud_info_.size() );
for (leaf_it = octree.leaf_begin(); leaf_it != it2_end; ++leaf_it)
{
pcl::octree::OctreeContainerPointIndices& container = leaf_it.getLeafContainer();
// add points from leaf node to indexVector
std::vector<int> indexVector;
container.getPointIndices (indexVector);
if(indexVector.empty())
continue;
std::vector<PointInfo> voxel_pts ( indexVector.size() );
for(size_t k=0; k < indexVector.size(); k++)
voxel_pts[k] = big_cloud_info_ [indexVector[k]];
PointInfo p;
size_t num_good_pts = 0;
if(param_.average_)
{
for(const PointInfo &pt_tmp : voxel_pts)
{
if (pt_tmp.distance_to_depth_discontinuity > param_.min_px_distance_to_depth_discontinuity_)
{
p.moving_average( pt_tmp );
num_good_pts++;
}
}
if( num_good_pts < param_.min_points_per_voxel_ )
continue;
total_used += num_good_pts;
}
else // take only point with min weight
{
for(const PointInfo &pt_tmp : voxel_pts)
{
if ( pt_tmp.distance_to_depth_discontinuity > param_.min_px_distance_to_depth_discontinuity_)
{
num_good_pts++;
if ( pt_tmp.weight < p.weight || num_good_pts == 1)
p = pt_tmp;
}
}
if( num_good_pts < param_.min_points_per_voxel_ )
continue;
total_used++;
}
filtered_cloud_info[kept++] = p;
}
LOG(INFO) << "Number of points in final noise model based integrated cloud: " << kept << " used: " << total_used << std::endl;
if(!output)
output.reset(new pcl::PointCloud<PointT>);
if(!output_normals_)
output_normals_.reset( new pcl::PointCloud<pcl::Normal>);
filtered_cloud_info.resize(kept);
output->points.resize(kept);
output_normals_->points.resize(kept);
output->width = output_normals_->width = kept;
output->height = output_normals_->height = 1;
output->is_dense = output_normals_->is_dense = true;
PointT na;
na.x = na.y = na.z = std::numeric_limits<float>::quiet_NaN();
input_clouds_used_.resize( input_clouds_.size() );
for(size_t i=0; i<input_clouds_used_.size(); i++) {
input_clouds_used_[i].reset( new pcl::PointCloud<PointT> );
input_clouds_used_[i]->points.resize( input_clouds_[i]->points.size(), na);
input_clouds_used_[i]->width = input_clouds_[i]->width;
input_clouds_used_[i]->height = input_clouds_[i]->height;
}
for(size_t i=0; i<filtered_cloud_info.size(); i++)
{
output->points[i] = filtered_cloud_info[i].pt;
output_normals_->points[i] = filtered_cloud_info[i].normal;
int origin = filtered_cloud_info[i].origin;
input_clouds_used_[origin]->points[filtered_cloud_info[i].pt_idx] = filtered_cloud_info[i].pt;
}
cleanUp();
}
// The MAIN function, from here we start the application and run the main loop
int main()
{
///*************************************************************************************///
///******************************** PCL LOADING PART ***********************************///
///*************************************************************************************///
// (This part will be separated later!)
// Later, this will be according to the command line argument
// Now I test it, with "fovam2a_bin_compressed.pcd"
if (pcl::io::loadPCDFile<pcl::PointXYZRGB> ("fovam2a_bin_compressed.pcd", *cloud) == -1) // load the file
{
PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
return (-1);
}
std::cout << "Loaded " << cloud->width * cloud->height << " data points from test_pcd.pcd with the following fields: " << std::endl;
std::cout << cloud->width << std::endl;
std::cout << cloud->height << std::endl;
// Set the initial minimum value of each coordinate:
min_x = cloud->points[0].x;
min_y = cloud->points[0].y;
min_z = cloud->points[0].z;
// Calculate the minimum value of each coordinate:
for (size_t i = 0; i < cloud->points.size (); ++i)
{
if(cloud->points[i].x < min_x) min_x = cloud->points[i].x;
if(cloud->points[i].y < min_y) min_y = cloud->points[i].y;
if(cloud->points[i].z < min_z) min_z = cloud->points[i].z;
}
// Transform the cloud to the origin of its coordinate system, for easier handling of the cloud data.
// This part of the code should be removed later. (Just helped me at the beginning)
for(size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x -= min_x;
cloud->points[i].y -= min_y;
cloud->points[i].z -= min_z;
}
///*************************************************************************************///
///******************************** GLFW INIT PART ***********************************///
///*************************************************************************************///
// Init GLFW
glfwInit();
// Set all the required options for GLFW
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
glfwWindowHint(GLFW_RESIZABLE, GL_FALSE);
// Create a GLFWwindow object that we can use for GLFW's functions
GLFWwindow* window = glfwCreateWindow(WIDTH, HEIGHT, "PCL with OpenGL", nullptr, nullptr);
glfwMakeContextCurrent(window);
// Set the required callback functions
glfwSetKeyCallback(window, key_callback);
glfwSetCursorPosCallback(window, mouse_callback);
glfwSetScrollCallback(window, scroll_callback);
// Options
glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_DISABLED);
// This is how GLEW knows that it should use a modern approach for retrieving function pointers and extensions
glewExperimental = GL_TRUE;
// Initialize GLEW to setup the OpenGL Function pointers
glewInit();
// Define the viewport dimensions
glViewport(0, 0, WIDTH, HEIGHT);
glEnable(GL_DEPTH_TEST);
glPointSize(1.3f);
///*************************************************************************************///
///******************************** VBO LOADING PART ***********************************///
///*************************************************************************************///
Shader ourShader("shader.vs", "shader.frag"); // load the shaders
std::size_t v_size = cloud->points.size() * 6; // size of my points
// (all of them contains 6 member)
// I will fill up this vector with all the data from cloud->points
std::vector<GLfloat> vertices(v_size);
// for efficient data reading, I start a new thread for reading the half of my data.
std::future<void> result( std::async([&]()
{
for(size_t i = 1; i < cloud->points.size (); i+=2)
{
size_t num = (i * 6);
vertices[num + 0] = cloud->points[i].x;
vertices[num + 1] = cloud->points[i].y;
vertices[num + 2] = cloud->points[i].z;
vertices[num + 3] = (float)cloud->points[i].r / 256.f;
vertices[num + 4] = (float)cloud->points[i].g / 256.f;
vertices[num + 5] = (float)cloud->points[i].b / 256.f;
}
}));
// another half of my points
for(size_t i = 0; i < cloud->points.size (); i+=2)
{
size_t num = (i * 6);
vertices[num + 0] = cloud->points[i].x;
vertices[num + 1] = cloud->points[i].y;
vertices[num + 2] = cloud->points[i].z;
vertices[num + 3] = (float)cloud->points[i].r / 256.f;
vertices[num + 4] = (float)cloud->points[i].g / 256.f;
vertices[num + 5] = (float)cloud->points[i].b / 256.f;
}
result.get(); // wait for the other thread
// Now, I've filled up my vector!
std::cout << "*****!!!DONE! (read)!!!*****" << std::endl;
/// Create the VBO from the data:
GLuint VBO, VAO;
glGenVertexArrays(1, &VAO);
glGenBuffers(1, &VBO);
// Bind the Vertex Array Object first, then bind and set the vertex buffer(s) and the attribute pointer(s).
glBindVertexArray(VAO);
glBindBuffer(GL_ARRAY_BUFFER, VBO);
glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * vertices.size(), vertices.data(), GL_STATIC_DRAW);
// Position attribute
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(GLfloat), (GLvoid*)0);
glEnableVertexAttribArray(0);
// Color attribute
glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(GLfloat), (GLvoid*)(3 * sizeof(GLfloat)));
glEnableVertexAttribArray(1);
glBindVertexArray(0); // Unbind VAO
std::cout << "*****!!!DONE! (VBO load)!!!*****" << std::endl;
ourShader.Use();
/// Load transformation matrices
GLuint MatrixID_modelview = glGetUniformLocation(ourShader.Program, "modelview");
GLuint MatrixID_proj = glGetUniformLocation(ourShader.Program, "projection");
// Send our transformations to the currently bound shader
glUniformMatrix4fv(MatrixID_modelview, 1, GL_FALSE, &ModelView[0][0]);
glUniformMatrix4fv(MatrixID_proj, 1, GL_FALSE, &Proj[0][0]);
vertices.clear();
//cloud.reset();
VeryNaiveSphere mySphere(500, 500, glm::vec3(10000.f, 10000.f, 10000.f));
mySphere.init_sphere();
float resolution = 128.0f; // for the leaf level of the octree
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree (resolution);
// Fill up my tree:
octree.setInputCloud (cloud);
octree.addPointsFromInputCloud();
///*************************************************************************************///
///******************************** MAIN LOOP PART ***********************************///
///*************************************************************************************///
std::cout << "*****!!!LOOP!!!*****" << std::endl;
// Main loop
while (!glfwWindowShouldClose(window))
{
// Set frame time
GLfloat currentFrame = glfwGetTime();
deltaTime = currentFrame - lastFrame;
lastFrame = currentFrame;
// Check if any events have been activiated (key pressed, mouse moved etc.) and call corresponding response functions
glfwPollEvents();
Do_Camera_movement();
Do_Sphere_movement(mySphere, octree);
// Render
// Clear the colorbuffer and the depthbuffer
glClearColor(0.2f, 0.2f, 0.2f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Draw
ourShader.Use();
Proj = glm::perspective(camera.Zoom, (float)WIDTH / (float)HEIGHT, cam_near, cam_far);
ModelView = camera.GetViewMatrix();
glUniformMatrix4fv(MatrixID_proj, 1, GL_FALSE, &Proj[0][0]);
glUniformMatrix4fv(MatrixID_modelview, 1, GL_FALSE, &ModelView[0][0]);
glBindVertexArray(VAO);
glDrawArrays(GL_POINTS, 0, v_size / 6);
glBindVertexArray(0);
mySphere.draw(MatrixID_modelview, ModelView);
// Swap the screen buffers
glfwSwapBuffers(window);
}
// Properly de-allocate all resources once they've outlived their purpose
glDeleteVertexArrays(1, &VAO);
glDeleteBuffers(1, &VBO);
// Terminate GLFW, clearing any resources allocated by GLFW.
glfwTerminate();
return 0;
}
int
main (int argc, char** argv)
{
srand ((unsigned int) time (NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
// Generate pointcloud data
cloud->width = 1000;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
}
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);
octree.setInputCloud (cloud);
octree.addPointsFromInputCloud ();
pcl::PointXYZ searchPoint;
searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
// Neighbors within voxel search
std::vector<int> pointIdxVec;
if (octree.voxelSearch (searchPoint, pointIdxVec))
{
std::cout << "Neighbors within voxel search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z << ")"
<< std::endl;
for (size_t i = 0; i < pointIdxVec.size (); ++i)
std::cout << " " << cloud->points[pointIdxVec[i]].x
<< " " << cloud->points[pointIdxVec[i]].y
<< " " << cloud->points[pointIdxVec[i]].z << std::endl;
}
// K nearest neighbor search
int K = 10;
std::vector<int> pointIdxNKNSearch;
std::vector<float> pointNKNSquaredDistance;
std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K=" << K << std::endl;
if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
{
for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxNKNSearch[i] ].x
<< " " << cloud->points[ pointIdxNKNSearch[i] ].y
<< " " << cloud->points[ pointIdxNKNSearch[i] ].z
<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
}
// Neighbors within radius search
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
std::cout << "Neighbors within radius search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with radius=" << radius << std::endl;
if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxRadiusSearch[i] ].x
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].y
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].z
<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}
}
void processCloud(const sensor_msgs::PointCloud2 msg)
{
//********* Retirive and process raw pointcloud************
std::cout<<"Recieved cloud"<<std::endl;
std::cout<<"Create Octomap"<<std::endl;
//octomap::OcTree tree(res);
std::cout<<"Load points "<<std::endl;
pcl::PCLPointCloud2 cloud;
pcl_conversions::toPCL(msg,cloud);
pcl::PointCloud<pcl::PointXYZ> pcl_pc;
pcl::fromPCLPointCloud2(cloud,pcl_pc);
std::cout<<"Filter point clouds for NAN"<<std::endl;
std::vector<int> nan_indices;
pcl::removeNaNFromPointCloud(pcl_pc,pcl_pc,nan_indices);
//octomap::Pointcloud oct_pc;
//octomap::point3d origin(0.0f,0.0f,0.0f);
std::cout<<"Adding point cloud to octomap"<<std::endl;
//octomap::point3d origin(0.0f,0.0f,0.0f);
//for(int i = 0;i<pcl_pc.points.size();i++){
//oct_pc.push_back((float) pcl_pc.points[i].x,(float) pcl_pc.points[i].y,(float) pcl_pc.points[i].z);
//}
//tree.insertPointCloud(oct_pc,origin,-1,false,false);
//*********** Remove the oldest data, update the data***************
cloud_seq_loaded.push_back(pcl_pc);
std::cout<<cloud_seq_loaded.size()<<std::endl;
if(cloud_seq_loaded.size()>2){
cloud_seq_loaded.pop_front();
}
//*********** Process currently observerd and buffered data*********
if(cloud_seq_loaded.size()==2){
std::cout<< "Generating octomap"<<std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr prev_pc (new pcl::PointCloud<pcl::PointXYZ>);
*prev_pc = cloud_seq_loaded.front();
pcl::PointCloud<pcl::PointXYZ>::Ptr curr_pc (new pcl::PointCloud<pcl::PointXYZ>);
*curr_pc =pcl_pc;
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (32.0f);
octree.setInputCloud(prev_pc);
octree.addPointsFromInputCloud();
octree.switchBuffers();
octree.setInputCloud(curr_pc);
octree.addPointsFromInputCloud();
std::vector<int> newPointIdxVector;
// Get vector of point indices from octree voxels which did not exist in previous buffer
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
for (size_t i = 0; i < newPointIdxVector.size (); ++i){
std::cout << i << "# Index:" << newPointIdxVector[i]<< " Point:" << pcl_pc.points[newPointIdxVector[i]].x << " "<< pcl_pc.points[newPointIdxVector[i]].y << " "<< pcl_pc.points[newPointIdxVector[i]].z << std::endl;
//std::cout<< curr_coord<<std::endl;
}
//std::cout << "Node center: " << it.getCoordinate() << std::endl;
//std::cout << "Node size: " << it.getSize() << std::endl;
//std::cout << "Node value: " << it->getValue() << std::endl;
}
//********** print out the statistics ******************
//**************Process Point cloud in octree data structure *****************
/*
//******************Traverse the tree ********************
for(octomap::OcTree::tree_iterator it =tree.begin_tree(), end = tree.end_tree();it!= end;it++){
//manipulate node, e.g.:
std::cout << "_____________________________________"<<std::endl;
std::cout << "Node center: " << it.getCoordinate() << std::endl;
std::cout << "Node size: " << it.getSize() << std::endl;
std::cout << "Node depth: "<<it.getDepth() << std::endl;
std::cout << "Is Leaf : "<< it.isLeaf()<< std::endl;
std::cout << "_____________________________________"<<std::endl;
}
//**********************************************************
*/
std::cout<<"finished"<<std::endl;
std::cout<<std::endl;
}
int
main (int argc, char** argv)
{
srand ((unsigned int) time (NULL));
// Octree resolution - side length of octree voxels
float resolution = 32.0f;
// Instantiate octree-based point cloud change detection class
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );
// Generate pointcloud data for cloudA
cloudA->width = 128;
cloudA->height = 1;
cloudA->points.resize (cloudA->width * cloudA->height);
for (size_t i = 0; i < cloudA->points.size (); ++i)
{
cloudA->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
cloudA->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
cloudA->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
}
// Add points from cloudA to octree
octree.setInputCloud (cloudA);
octree.addPointsFromInputCloud ();
// Switch octree buffers: This resets octree but keeps previous tree structure in memory.
octree.switchBuffers ();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
// Generate pointcloud data for cloudB
cloudB->width = 128;
cloudB->height = 1;
cloudB->points.resize (cloudB->width * cloudB->height);
for (size_t i = 0; i < cloudB->points.size (); ++i)
{
cloudB->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
cloudB->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
cloudB->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
}
// Add points from cloudB to octree
octree.setInputCloud (cloudB);
octree.addPointsFromInputCloud ();
std::vector<int> newPointIdxVector;
// Get vector of point indices from octree voxels which did not exist in previous buffer
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
// Output points
std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
for (size_t i = 0; i < newPointIdxVector.size (); ++i)
std::cout << i << "# Index:" << newPointIdxVector[i]
<< " Point:" << cloudB->points[newPointIdxVector[i]].x << " "
<< cloudB->points[newPointIdxVector[i]].y << " "
<< cloudB->points[newPointIdxVector[i]].z << std::endl;
}
//--------------------------------------------------------------
map<int,TrackedCloudPtr> ObjectsThread::computeOcclusions(const list<TrackedCloudPtr>& potentialOcclusions)
{
map<int,TrackedCloudPtr> occlusions;
ofVec3f origin = PCXYZ_OFVEC3F(eyePos());
inCloudMutex.lock();
PCPtr cloud = PCPtr(new PC(*inRawCloud));
inRawCloud.reset();
inCloudMutex.unlock();
saveCloud("rawInternal.pcd",*cloud);
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> octree(Constants::CLOUD_VOXEL_SIZE*2);
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::AlignedPointTVector voxelList;
if(cloud->size() > 0)
{
octree.setInputCloud(cloud);
octree.defineBoundingBox();
octree.addPointsFromInputCloud();
gModel->objectsMutex.lock();
for (list<TrackedCloudPtr>::const_iterator iter = potentialOcclusions.begin(); iter != potentialOcclusions.end(); iter++)
{
if((*iter)->hasObject())
{
bool occludedPol = true;
PCPolyhedron* polyhedron = dynamic_cast<PCPolyhedron*>((*iter)->getTrackedObject().get());
polyhedron->resetOccludedFaces();
vector<IPolygonPtr> pols = polyhedron->getVisiblePolygons();
int occludedFaces = 0;
for(int i = 0; i < pols.size(); i++)
{
vector<ofVec3f> vexs = pols.at(i)->getMathModel().getVertexs();
int occludedVertexs = 0;
for(int o = 0; o < vexs.size(); o++)
{
bool occludedVertex = false;
ofVec3f end = vexs.at(o);
Eigen::Vector3f endPoint(end.x,end.y,end.z);
Eigen::Vector3f originPoint = PCXYZ_EIGEN3F(eyePos());
voxelList.clear();
int voxs = octree.getApproxIntersectedVoxelCentersBySegment(originPoint,endPoint,voxelList,Constants::CLOUD_VOXEL_SIZE*2);
for(int i = 0; i < voxelList.size(); i ++)
{
if(octree.isVoxelOccupiedAtPoint(voxelList.at(i)))
{
ofVec3f intersect (voxelList.at(i).x,voxelList.at(i).y,voxelList.at(i).z);
if(((intersect - end).length() > Constants::CLOUD_VOXEL_SIZE*5) &&
(intersect - origin).length() < (end - origin).length())
occludedVertexs++;
}
}
}
if(occludedVertexs >= 2)
{
polyhedron->setOccludedFace(pols.at(i)->getName());
occludedFaces++;
}
}
if(occludedFaces > 1)
{
occlusions[(*iter)->getTrackedObject()->getId()] = (*iter);
//cout << " occluded pol " << endl;
}
}
}
gModel->objectsMutex.unlock();
}
return occlusions;
}
/**
* texture function
*
* This function colors the 3D model returned by the mergePointClouds function
*
* @param point cloud mesh
* @param std::vector<Frame3D> frames
*/
void Functions3D::texture(pcl::PolygonMesh mesh, std::vector<Frame3D> frames, std::string filename)
{
// load values from the mesh
auto polygons = mesh.polygons;
// create cloud to save the points in
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());;
// create clouds to add the rgb points to
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>());;
// copy mesh cloud to both placeholders
pcl::fromPCLPointCloud2(mesh.cloud, *cloud);
pcl::fromPCLPointCloud2(mesh.cloud, *cloud_rgb);
// create texture mapping variable
auto texture_mapping = new pcl::TextureMapping<pcl::PointXYZ>();
// save uv coordinates
Eigen::Vector2f uv_coordinates;
/**
* Loop over all frames in the vector
*/
for (int i = frames.size()-1; i>=0; i--)
{
// Save reference instead of copy
const Frame3D ¤t_frame = frames[i];
// get data from the frame
auto depth = current_frame.depth_image_;
auto focal = current_frame.focal_length_;
auto depth_map_size = depth.size();
auto rgb_image = current_frame.rgb_image_;
// save and resize rgb image
cv::Mat rgb_img;
cv::resize(rgb_image, rgb_img, depth_map_size);
// inverse the camera pose
auto camera_pose = current_frame.getEigenTransform();
camera_pose(3,3) = 1;
auto inv_camera = camera_pose.inverse().eval();
// create texture mapping camera
pcl::texture_mapping::Camera camera;
camera.focal_length = focal;
camera.height = rgb_img.size().height;
camera.width = rgb_img.size().width;
// reverse the point cloud transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr trans_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::transformPointCloud(*cloud, *trans_cloud, inv_camera);
// create octree for texture mapping
pcl::TextureMapping<pcl::PointXYZ>::Octree::Ptr octree(new pcl::TextureMapping<pcl::PointXYZ>::Octree(0.01));
octree->setInputCloud(trans_cloud->makeShared());
octree->addPointsFromInputCloud();
/**
* Loop over all polygons
*/
for (pcl::Vertices polygon : polygons)
{
/**
* Loop over all vertices in the polygon
*/
for (uint32_t vertex : polygon.vertices)
{
// check if the point is occluded
auto vertex_point = trans_cloud->points.at(vertex);
if (!texture_mapping->isPointOccluded(vertex_point, octree))
{
// if it is not, get the uv coordinates
if (texture_mapping->getPointUVCoordinates(vertex_point, camera, uv_coordinates))
{
// get the coordinates
int x = round(uv_coordinates.x() * camera.width);
int y = round(camera.height - uv_coordinates.y() * camera.height);
float z = depth.at<ushort>(y, x) * 0.001;
// if they have the correct depth, save them
if (z < 1.2)
{
auto pixel = rgb_img.at<cv::Vec3b>(cv::Point(x, y));
cloud_rgb->points.at(vertex).b = pixel[0];
cloud_rgb->points.at(vertex).g = pixel[1];
cloud_rgb->points.at(vertex).r = pixel[2];
}
}
}
}
}
}
// set the rgb cloud as point cloud for the mesh
pcl::toPCLPointCloud2(*cloud_rgb, mesh.cloud);
// save the mesh to a file
pcl::io::saveVTKFile("../data/" + filename, mesh);
}
void processCloud(const sensor_msgs::PointCloud2 msg)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr curr_pc (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2 cloud;
pcl::PointCloud<pcl::PointXYZ> pcl_pc;
std::vector<int> nan_indices;
pcl::PointCloud<pcl::PointXYZ>::Ptr prev_pc (new pcl::PointCloud<pcl::PointXYZ>);
//********* Retirive and process raw pointcloud************
pcl_conversions::toPCL(msg,cloud);
pcl::fromPCLPointCloud2(cloud,pcl_pc);
pcl::removeNaNFromPointCloud(pcl_pc,pcl_pc,nan_indices);
*curr_pc =pcl_pc;
//*********** Remove old data, update the data***************
cloud_seq_loaded.push_back(pcl_pc);
std::cout<<cloud_seq_loaded.size()<<std::endl;
if(cloud_seq_loaded.size()>2){
cloud_seq_loaded.pop_front();
}
if(cloud_seq_loaded.size()==1){
static_pc = pcl_pc;
}
//*********** Process currently observerd and buffered data*********
if(cloud_seq_loaded.size()==2){
*prev_pc =static_pc; //cloud_seq_loaded.front(); ss
//*************Create octree structure and search
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (0.5);
octree.setInputCloud(prev_pc);
octree.addPointsFromInputCloud();
octree.switchBuffers();
octree.setInputCloud(curr_pc);
octree.addPointsFromInputCloud();
std::vector<int> newPointIdxVector;
// Get vector of point indices from octree voxels which did not exist in previous buffer
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr dynamic_points (new pcl::PointCloud<pcl::PointXYZ>);
dynamic_points->header.frame_id = "some_tf_frame";
for (size_t i = 0; i < newPointIdxVector.size (); ++i){
pcl::PointXYZ point;
point.x = pcl_pc.points[newPointIdxVector[i]].x;
point.y = pcl_pc.points[newPointIdxVector[i]].y;
point.z = pcl_pc.points[newPointIdxVector[i]].z;
dynamic_points->push_back(point);
//std::cout << i << "# Index:" << newPointIdxVector[i]<< " Point:" << pcl_pc.points[newPointIdxVector[i]].x << " "<< pcl_pc.points[newPointIdxVector[i]].y << " "<< pcl_pc.points[newPointIdxVector[i]].z << std::endl;
}
std::cout<<newPointIdxVector.size ()<<std::endl;
//***************Filter point cloud to detect nearby changes only *****************
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (dynamic_points);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 3.0);
pass.filter (*dynamic_points);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> dy_sor;
dy_sor.setInputCloud (dynamic_points);
dy_sor.setMeanK (50);
dy_sor.setStddevMulThresh (1.0);
dy_sor.filter (*dynamic_points);
//**********************Publish the data************************************
ros::NodeHandle k;
ros::Publisher pub = k.advertise<pcl::PointCloud<pcl::PointXYZ> >("dynamicPoints",2);
pub.publish(dynamic_points);
ros::Time time = ros::Time::now();
//Wait a duration of one second.
ros::Duration d = ros::Duration(1.5, 0);
d.sleep();
ros::spinOnce();
}
std::cout<<"finished"<<std::endl;
std::cout<<std::endl;
}