void DebugInformationRecorder::end_scopes(int pc_offset, bool is_safepoint) {
assert(_recording_state == (is_safepoint? rs_safepoint: rs_non_safepoint),
"nesting of recording calls");
debug_only(_recording_state = rs_null);
// Try to compress away an equivalent non-safepoint predecessor.
// (This only works because we have previously recognized redundant
// scope trees and made them use a common scope_decode_offset.)
if (_pcs_length >= 2 && recording_non_safepoints()) {
PcDesc* last = last_pc();
PcDesc* prev = prev_pc();
// If prev is (a) not a safepoint and (b) has the same
// stream pointer, then it can be coalesced into the last.
// This is valid because non-safepoints are only sought
// with pc_desc_near, which (when it misses prev) will
// search forward until it finds last.
// In addition, it does not matter if the last PcDesc
// is for a safepoint or not.
if (_prev_safepoint_pc < prev->pc_offset() && prev->is_same_info(last)) {
assert(prev == last-1, "sane");
prev->set_pc_offset(pc_offset);
_pcs_length -= 1;
NOT_PRODUCT(++dir_stats.chunks_elided);
}
}
// We have just recorded this safepoint.
// Remember it in case the previous paragraph needs to know.
if (is_safepoint) {
_prev_safepoint_pc = pc_offset;
}
}
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;
std::cout<<"Ransac"<<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::RandomSample<pcl::PointXYZ> rs(true);
rs.setSample(20000);
rs.setInputCloud(curr_pc);
std::vector<int> indices;
rs.filter (indices);
pcl::PointCloud<pcl::PointXYZ> curr_cld_out;
rs.filter(curr_cld_out);
*curr_pc = curr_cld_out;
std::cout<<curr_pc->size()<<std::endl;
rs.setInputCloud(prev_pc);
rs.filter(indices);
pcl::PointCloud<pcl::PointXYZ> prev_cld_out;
rs.filter(prev_cld_out);
*prev_pc = prev_cld_out;
/*
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> esTrSVD;
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 trMat;
esTrSVD.estimateRigidTransformation(*prev_pc,*curr_pc,trMat);
std::cout<<trMat<<std::endl;
*/
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
//pcl::registration::TransformationEstimationPointToPlaneLLS < pcl::PointXYZ,pcl::PointXYZ,float>::Ptr trans_lls (new pcl::registration::TransformationEstimationPointToPlaneLLS < pcl::PointXYZ,pcl::PointXYZ,float>);
pcl::registration::TransformationEstimationLM< pcl::PointXYZ,pcl::PointXYZ,float >::Ptr trans_LM (new pcl::registration::TransformationEstimationLM< pcl::PointXYZ,pcl::PointXYZ,float >);
icp.setInputSource(prev_pc);
icp.setInputTarget(curr_pc);
icp.setTransformationEstimation (trans_LM);
//icp.setMaximumIterations (2);
//icp.setRANSACIterations(5);
icp.setTransformationEpsilon (1e-8);
icp.setMaxCorrespondenceDistance (0.5);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 trMat;
trMat = icp.getFinalTransformation();
//*curr_pc = pcl_pc;
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::transformPointCloud (*prev_pc, *transformed_cloud, trMat);
pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
// Define R,G,B colors for the point cloud
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (prev_pc, 255, 255, 255);
// We add the point cloud to the viewer and pass the color handler
viewer.addPointCloud (prev_pc, source_cloud_color_handler, "original_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
viewer.addCoordinateSystem (1.0, 0);
viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
viewer.spinOnce ();
}
//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;
}
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;
}
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;
}
