pcl::PointCloud<pcl::PointXYZRGB> CPlaneExtraction::extractHorizontalSurfaceFromNormals(
pcl::PointCloud<pcl::PointXYZRGB> &point_cloud, bool surface) {
Eigen::Vector3f axis = Eigen::Vector3f(1.0, 0, 0); //x
//ROS_DEBUG("before in %d", (int)point_cloud.points.size ());
pcl::PointCloud<pcl::PointXYZRGB> cloud_projected;
pcl::PointIndices inliers;
pcl::ModelCoefficients coefficients;
pcl::SACSegmentationFromNormals<pcl::PointXYZRGB, pcl::Normal> seg;
pcl::PointCloud<pcl::Normal> cloud_normals;
pcl::PointCloud<pcl::PointNormal> cloud_pointNormals;
cloud_normals = this->toolBox.estimatingNormals(point_cloud, 10);
//cloud_pointNormals = this->toolBox.movingLeastSquares(point_cloud,0.005f);
seg.setOptimizeCoefficients(true);
// seg.setModelType (pcl::SACMODEL_NORMAL_PLANE + pcl::SACMODEL_PARALLEL_PLANE);
seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
//seg.setAxis(axis);
seg.setNormalDistanceWeight(0.1); //0.1
seg.setMaxIterations(10000); //10000
seg.setDistanceThreshold(0.1); //0.1 //must be low to get a really restricted horizontal plane
//seg.setProbability(0.99);
seg.setInputCloud(point_cloud.makeShared());
seg.setInputNormals(cloud_normals.makeShared());
seg.segment(inliers, coefficients);
if (inliers.indices.size() == 0) {
ROS_ERROR("[extractHorizontalSurfaceFromNormals] Could not estimate a planar model for the given dataset.");
return cloud_projected;
}
pcl::ProjectInliers<pcl::PointXYZRGB> proj;
proj.setModelType(pcl::SACMODEL_NORMAL_PLANE);
proj.setInputCloud(point_cloud.makeShared());
proj.setModelCoefficients(boost::make_shared<pcl::ModelCoefficients>(
coefficients));
proj.filter(cloud_projected);
//ROS_DEBUG("after in %d", (int)cloud_projected.points.size ());
//pcl::copyPointCloud(point_cloud,inliers,cloud_projected);
//point_cloud = cloud_projected;
return cloud_projected;
}
void mario::removePlane( const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & cloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr & dst, double threshould )
{
dst = cloud->makeShared();
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (threshould);
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
//for (size_t i = 0; i < inliers->indices.size (); ++i) {
// cloud->points[inliers->indices[i]].r = 255;
// cloud->points[inliers->indices[i]].g = 0;
// cloud->points[inliers->indices[i]].b = 0;
//}
//フィルタリング
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
extract.setInputCloud( cloud );
extract.setIndices( inliers );
// true にすると平面を除去、false にすると平面以外を除去
extract.setNegative( true );
extract.filter( *dst );
}
pcl::PointCloud<pcl::PointXYZ>::Ptr MovingObjectsIdentificator::removeLargePlanes(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
if(verbose) std::cout << "Removing large planes ... ";
pcl::PointCloud<pcl::PointXYZ>::Ptr resultCloud (cloud->makeShared());
pcl::SACSegmentation<pcl::PointXYZ> segmentation;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
segmentation.setOptimizeCoefficients(true);
segmentation.setModelType(pcl::SACMODEL_PLANE);
segmentation.setMethodType(pcl::SAC_RANSAC);
segmentation.setMaxIterations(ransacMaxIterations);
segmentation.setDistanceThreshold(ransacDistanceThreshold);
int pointsCount = resultCloud->points.size();
while(resultCloud->points.size() > 0.3 * pointsCount) {
segmentation.setInputCloud(resultCloud);
segmentation.segment(*inliers, *coefficients);
if(inliers->indices.size() <= largePlaneMinimumSize) {
break;
}
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(resultCloud);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filterDirectly(resultCloud);
}
if(verbose) std::cout << "done!" << std::endl;
return resultCloud;
}
void registration(pcl::PointCloud<pcl::PointXYZRGB>& cloud, pcl::PointCloud<pcl::PointXYZRGB>& model, pcl::PointCloud<pcl::PointXYZRGB>& cloud_out, pcl::PointCloud<pcl::PointXYZRGB>& tmp_rgb, Eigen::Matrix4f& m)
{
pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
icp.setInputSource(cloud.makeShared ());
icp.setInputTarget(model.makeShared ());
pcl::PointCloud<pcl::PointXYZRGB> Final;
icp.align(Final);
m = icp.getFinalTransformation();
pcl::transformPointCloud (cloud, cloud, m);
pcl::transformPointCloud (tmp_rgb, tmp_rgb, m);
pcl::copyPointCloud(model, cloud_out);
cloud_out += cloud;
return;
}
Eigen::Vector3i extractC3HLACSignature117(pcl::VoxelGrid<PointT> grid, pcl::PointCloud<PointT> cloud, std::vector< std::vector<float> > &feature, int color_threshold_r, int color_threshold_g, int color_threshold_b, const float voxel_size, const int subdivision_size, const int offset_x , const int offset_y, const int offset_z ){
feature.resize( 0 );
pcl::PointCloud<pcl::C3HLACSignature117> c3_hlac_signature;
pcl::C3HLAC117Estimation<PointT, pcl::C3HLACSignature117> c3_hlac_;
c3_hlac_.setRadiusSearch (0.000000001); // not used actually.
c3_hlac_.setSearchMethod ( boost::make_shared<pcl::KdTreeFLANN<PointT> > () ); // not used actually.
c3_hlac_.setColorThreshold( color_threshold_r, color_threshold_g, color_threshold_b );
if( c3_hlac_.setVoxelFilter (grid, subdivision_size, offset_x, offset_y, offset_z, voxel_size) ){
c3_hlac_.setInputCloud ( cloud.makeShared() );
t1 = my_clock();
c3_hlac_.compute( c3_hlac_signature );
t2 = my_clock();
#ifndef QUIET
ROS_INFO (" %d c3_hlac estimated. (%f sec)", (int)c3_hlac_signature.points.size (), t2-t1);
#endif
const int hist_num = c3_hlac_signature.points.size();
feature.resize( hist_num );
for( int h=0; h<hist_num; h++ ){
feature[ h ].resize( DIM_C3HLAC_117_1_3_ALL );
for( int i=0; i<DIM_C3HLAC_117_1_3_ALL; i++)
feature[ h ][ i ] = c3_hlac_signature.points[ h ].histogram[ i ];
}
}
return c3_hlac_.getSubdivNum();
}
int main (int argc, char** argv)
{
cloud.width = 5;
cloud.height = 4 ;
cloud.is_dense = true;
cloud.resize(20);
cloud[0].x = 100; cloud[0].y = 8; cloud[0].z = 5;
cloud[1].x = 228; cloud[1].y = 21; cloud[1].z = 2;
cloud[2].x = 341; cloud[2].y = 31; cloud[2].z = 10;
cloud[3].x = 472; cloud[3].y = 40; cloud[3].z = 15;
cloud[4].x = 578; cloud[4].y = 48; cloud[4].z = 3;
cloud[5].x = 699; cloud[5].y = 60; cloud[5].z = 12;
cloud[6].x = 807; cloud[6].y = 71; cloud[6].z = 14;
cloud[7].x = 929; cloud[7].y = 79; cloud[7].z = 16;
cloud[8].x = 1040; cloud[8].y = 92; cloud[8].z = 18;
cloud[9].x = 1160; cloud[9].y = 101; cloud[9].z = 38;
cloud[10].x = 1262; cloud[10].y = 109; cloud[10].z = 28;
cloud[11].x = 1376; cloud[11].y = 121; cloud[11].z = 32;
cloud[12].x = 1499; cloud[12].y = 128; cloud[12].z = 35;
cloud[13].x = 1620; cloud[13].y = 143; cloud[13].z = 28;
cloud[14].x = 1722; cloud[14].y = 150; cloud[14].z = 30;
cloud[15].x = 1833; cloud[15].y = 159; cloud[15].z = 15;
cloud[16].x = 1948; cloud[16].y = 172; cloud[16].z = 12;
cloud[17].x = 2077; cloud[17].y = 181; cloud[17].z = 33;
cloud[18].x = 2282; cloud[18].y = 190; cloud[18].z = 23;
cloud[19].x = 2999; cloud[19].y = 202; cloud[19].z = 29;
pca.setInputCloud (cloud.makeShared ());
testing::InitGoogleTest (&argc, argv);
return (RUN_ALL_TESTS ());
}
pcl::PCA<PointT>::PCA (const pcl::PointCloud<PointT>& X, bool basis_only)
{
Base ();
basis_only_ = basis_only;
setInputCloud (X.makeShared ());
compute_done_ = initCompute ();
}
template <class _type> int cutOffFilter(pcl::PointCloud<_type>& pointsIn, pcl::PointCloud<_type>& pointsOut)
{
pcl::PointCloud<_type> aux1, aux2;
pcl::PassThrough<_type> pass;
// Set x-limits
pass.setInputCloud(pointsIn.makeShared());
pass.setFilterFieldName("x");
pass.setFilterLimits(cutOffLimits.x.min,cutOffLimits.x.max);
pass.filter(aux1);
// Set y-limits
pass.setInputCloud(aux1.makeShared());
pass.setFilterFieldName("y");
pass.setFilterLimits(cutOffLimits.y.min,cutOffLimits.y.max);
pass.filter(aux2);
// Set z-limits
pass.setInputCloud(aux2.makeShared());
pass.setFilterFieldName("z");
pass.setFilterLimits(cutOffLimits.z.min,cutOffLimits.z.max);
pass.filter(pointsOut);
return 0;
}
Coordinate<typeM> mario::redDetection( const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & cloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr & dst )
{
dst = cloud->makeShared();
long double x=0,y=0,z=0;
int rcount=0;
static double RED_VAL = FileIO::getConst("RED_VAL");
static double RED_RATE = FileIO::getConst("RED_RATE");
for(int count=0;count<dst->points.size();count++){
if( dst->points[count].r > RED_VAL &&
dst->points[count].r > dst->points[count].g*RED_RATE &&
dst->points[count].r > dst->points[count].b*RED_RATE ){
dst->points[count].r = 0;
dst->points[count].g = 255;
dst->points[count].b = 0;
x += dst->points[count].x;
y += dst->points[count].y;
z += dst->points[count].z;
rcount++;
}
}
x/=rcount;
y/=rcount;
z/=rcount;
cout<<"RedDetection:"<<x<<" "<<y<<" "<<z<<" :"<<rcount<<endl;
return Coordinate<typeM>(x,y,z);
}
template <class _type> int voxelFilter(pcl::PointCloud<_type>& pointsIn, pcl::PointCloud<_type>& pointsOut)
{
pcl::VoxelGrid<_type> grid;
grid.setLeafSize(this->voxelLeafSizes.x, this->voxelLeafSizes.y, this->voxelLeafSizes.z);
grid.setInputCloud(pointsIn.makeShared());
grid.filter(pointsOut);
}
void OnNewMapFrame(pcl::PointCloud< pcl::PointXYZ > mapFrame)
{
_viewer.removeAllPointClouds(0);
_viewer.addPointCloud(mapFrame.makeShared(), "map");
_viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "map");
_viewer.spinOnce();
}
void downsampling(pcl::PointCloud<pcl::PointXYZRGB>& cloud, float th)
{
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud.makeShared());
sor.setLeafSize (th, th, th);
sor.filter (cloud);
return;
}
inline void
filter_depth_discontinuity(
const pcl::PointCloud<PointT> &in,
pcl::PointCloud<PointT> &out,
int neighbors = 2,
float threshold = 0.3,
float distance_min = 1,
float distance_max = 300,
float epsilon = 0.5
)
{
//std::cout << "neigbors " << neighbors << " epsilon " << epsilon << " distance_max " << distance_max <<std::endl;
boost::shared_ptr<pcl::search::KdTree<PointT> > tree_n( new pcl::search::KdTree<PointT>() );
tree_n->setInputCloud(in.makeShared());
tree_n->setEpsilon(epsilon);
for(int i = 0; i< in.points.size(); ++i){
std::vector<int> k_indices;
std::vector<float> square_distance;
if ( in.points.at(i).z > distance_max || in.points.at(i).z < distance_min) continue;
//Position in image is known
tree_n->nearestKSearch(in.points.at(i), neighbors, k_indices, square_distance);
//std::cout << "hier " << i << " z " << in.points.at(i).z <<std::endl;
#ifdef USE_SQUERE_DISTANCE
const float point_distance = distance_to_origin<PointT>(in.points.at(i));
#else
const float point_distance = in.points.at(i).z;
#endif
float value = 0; //point_distance;
unsigned int idx = 0;
for(int n = 0; n < k_indices.size(); ++n){
#ifdef USE_SQUERE_DISTANCE
float distance_n = distance_to_origin<PointT>(in.points.at(k_indices.at(n)));
#else
float distance_n = in.points.at(k_indices.at((n))).z;
#endif
if(value < distance_n - point_distance){
idx = k_indices.at(n);
value = distance_n - point_distance;
}
}
if(value > threshold){
out.push_back(in.points.at(i));
out.at(out.size()-1).intensity = sqrt(value);
}
}
}
void mpcl_compute_normals(pcl::PointCloud<pcl::PointXYZ> &cloud,
int ksearch,
pcl::PointCloud<pcl::Normal> &out)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setSearchMethod (tree);
ne.setInputCloud (cloud.makeShared());
ne.setKSearch (ksearch);
ne.compute (out);
}
template <typename PointInT, typename PointOutT> void
pcl_1_8::Edge<PointInT, PointOutT>::sobelMagnitudeDirection (
const pcl::PointCloud<PointInT> &input_x,
const pcl::PointCloud<PointInT> &input_y,
pcl::PointCloud<PointOutT> &output)
{
convolution_.setInputCloud (input_x.makeShared());
pcl::PointCloud<pcl::PointXYZI>::Ptr kernel_x (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<pcl::PointXYZI>);
kernel_.setKernelType (kernel<pcl::PointXYZI>::SOBEL_X);
kernel_.fetchKernel (*kernel_x);
convolution_.setKernel (*kernel_x);
convolution_.filter (*magnitude_x);
convolution_.setInputCloud (input_y.makeShared());
pcl::PointCloud<pcl::PointXYZI>::Ptr kernel_y (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<pcl::PointXYZI>);
kernel_.setKernelType (kernel<pcl::PointXYZI>::SOBEL_Y);
kernel_.fetchKernel (*kernel_y);
convolution_.setKernel (*kernel_y);
convolution_.filter (*magnitude_y);
const int height = input_x.height;
const int width = input_x.width;
output.resize (height * width);
output.height = height;
output.width = width;
for (size_t i = 0; i < output.size (); ++i)
{
output[i].magnitude_x = (*magnitude_x)[i].intensity;
output[i].magnitude_y = (*magnitude_y)[i].intensity;
output[i].magnitude =
sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
(*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
output[i].direction =
atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
}
}
void DynModelExporter2::createMarkerForConvexHull(pcl::PointCloud<pcl::PointXYZ>& plane_cloud, pcl::ModelCoefficients::Ptr& plane_coefficients, visualization_msgs::Marker& marker)
{
// init marker
marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
marker.action = visualization_msgs::Marker::ADD;
// project the points of the plane on the plane
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setInputCloud (plane_cloud.makeShared());
proj.setModelCoefficients (plane_coefficients);
proj.filter(*cloud_projected);
// create the convex hull in the plane
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::ConvexHull<pcl::PointXYZ > chull;
chull.setInputCloud (cloud_projected);
chull.reconstruct(*cloud_hull);
// work around known bug in ROS Diamondback perception_pcl: convex hull is centered around centroid of input cloud (fixed in pcl svn revision 443)
// thus: we shift the mean of cloud_hull to the mean of cloud_projected (fill dx, dy, dz and apply when creating the marker points)
Eigen::Vector4f meanPointCH, meanPointCP;
pcl::compute3DCentroid(*cloud_projected, meanPointCP);
pcl::compute3DCentroid(*cloud_hull, meanPointCH);
//float dx = 0;//meanPointCP[0]-meanPointCH[0];
//float dy = 0;//meanPointCP[1]-meanPointCH[1];
//float dz = 0;//meanPointCP[2]-meanPointCH[2];
// create colored part of plane by creating marker for each triangle between neighbored points on contour of convex hull an midpoint
marker.points.clear();
for (unsigned int j = 0; j < cloud_hull->points.size(); ++j)
{
geometry_msgs::Point p;
p.x = cloud_hull->points[j].x; p.y = cloud_hull->points[j].y; p.z = cloud_hull->points[j].z;
marker.points.push_back( p );
p.x = cloud_hull->points[(j+1)%cloud_hull->points.size() ].x; p.y = cloud_hull->points[(j+1)%cloud_hull->points.size()].y; p.z = cloud_hull->points[(j+1)%cloud_hull->points.size()].z;
marker.points.push_back( p );
p.x = meanPointCP[0]; p.y = meanPointCP[1]; p.z = meanPointCP[2];
marker.points.push_back( p );
}
// scale of the marker
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 1;
}
void filtering(pcl::PointCloud<pcl::PointXYZRGB>& cloud, pcl::PointCloud<pcl::PointXYZRGB>& model)
{
std::cout << "filtering" << std::endl;
m_size.min_x = 100.0;
m_size.max_x = -100.0;
m_size.min_y = 100.0;
m_size.max_y = -100.0;
m_size.min_z = 100.0;
m_size.max_z = -100.0;
for(int i=0;i<model.points.size();i++){
if(model.points[i].x < m_size.min_x) m_size.min_x = model.points[i].x;
if(model.points[i].x > m_size.max_x) m_size.max_x = model.points[i].x;
if(model.points[i].y < m_size.min_y) m_size.min_y = model.points[i].y;
if(model.points[i].y > m_size.max_y) m_size.max_y = model.points[i].y;
if(model.points[i].z < m_size.min_z) m_size.min_z = model.points[i].z;
if(model.points[i].z > m_size.max_z) m_size.max_z = model.points[i].z;
}
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud.makeShared());
pass.setFilterFieldName ("x");
pass.setFilterLimits (m_size.min_x-0.3, m_size.max_x+0.3);
pass.filter (cloud);
pass.setInputCloud (cloud.makeShared());
pass.setFilterFieldName ("y");
pass.setFilterLimits (m_size.min_y-0.3, m_size.max_y+0.3);
pass.filter (cloud);
pass.setInputCloud (cloud.makeShared());
pass.setFilterFieldName ("z");
pass.setFilterLimits (m_size.min_z + 0.5, m_size.max_z+0.2);
pass.filter (cloud);
return;
}
//--------------------------------------------------------------------------------
//questa callback riceve in ingresso l'array di waypoints e per ogni coppia di
//waypoints adiacenti invoca il planner
//--------------------------------------------------------------------------------
void goalSelectionCallback(geometry_msgs::PoseArray waypoints){
//dimensione dell'array di waypoints
size_t n = waypoints.poses.size();
for( size_t i = 0; i < n; i++){
//istanzio un planner per ogni coppia di waypoints
PathPlanning new_planner(nh);
planner = &new_planner;
nav_msgs::Path path;
//al primo step il punto iniziale è la posa del robot
if( i == 0 ) {
planner->set_input(traversability_pcl,wall_pcl,wall_kdTree,traversability_kdtree,robot_idx);
}
//agli step successivi il punto iniziale è il goal dello step precedente
else {
pcl::PointXYZI in_point = convert(waypoints.poses.at(i-1));
//faccio il KNearestNeighbor search giusto per utilizzare la planner->set_input(..) scritta dagli altri
//poi dobbiamo scriverci la nostra set_input(..) e tutto questo non sarà più necessario
pcl::KdTreeFLANN<pcl::PointXYZI> kdtree;
kdtree.setInputCloud(traversability_pcl.makeShared());
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
kdtree.nearestKSearch(in_point, 1, pointIdxNKNSearch, pointNKNSquaredDistance);
size_t input_idx = pointIdxNKNSearch[0];
planner->set_input(traversability_pcl,wall_pcl,wall_kdTree,traversability_kdtree,input_idx);
}
//qui setto il goal
pcl::PointXYZI goal_point = convert(waypoints.poses.at(i));
planner->set_goal(goal_point);
goal_selection = true;
ROS_INFO("goal selection");
//qui lancio il planner
if(planner->planning(path)){//<--questa funzione va riscritta!!!
path_pub.publish(path);
}
else{
ROS_INFO("no path exist for desired goal, please choose another goal");
goal_selection = true;
}
ROS_INFO("path_computed");
}
}
void mpcl_compute_normals_PointXYZRGBA(const pcl::PointCloud<pcl::PointXYZRGBA>& cloud,
int ksearch,
double searchRadius,
pcl::PointCloud<pcl::Normal> &out)
{
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
ne.setSearchMethod (tree);
ne.setInputCloud (cloud.makeShared());
if (ksearch >= 0)
ne.setKSearch (ksearch);
if (searchRadius >= 0.0)
ne.setRadiusSearch (searchRadius);
ne.compute (out);
}
void pointCloudCallback(const sensor_msgs::PointCloud2& traversability_msg) {
//pcl::PointCloud<pcl::PointXYZI> traversability_pcl;
pcl::fromROSMsg(traversability_msg, traversability_pcl);
ROS_INFO("path planner input set");
if (traversability_pcl.size() > 0 && wall_flag){
tf::StampedTransform robot_pose;
getRobotPose(robot_pose);
pcl::PointXYZI robot;
robot.x = robot_pose.getOrigin().x();
robot.y = robot_pose.getOrigin().y();
robot.z = robot_pose.getOrigin().z();
//pcl::KdTreeFLANN<pcl::PointXYZI> traversability_kdtree;
traversability_kdtree.setInputCloud(traversability_pcl.makeShared());
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
traversability_kdtree.nearestKSearch(robot, 1, pointIdxNKNSearch, pointNKNSquaredDistance);
robot_idx = pointIdxNKNSearch[0];
//----------------------------------------------------------------------------------------------------------------
//ho commentato questa parte perchè vogliamo disaccoppiare il planning dall'acquisizione della Point Cloud
//----------------------------------------------------------------------------------------------------------------
// planner->set_input(traversability_pcl, wall_pcl, wall_kdTree, traversability_kdtree, pointIdxNKNSearch[0]);
// wall_flag = false;
// if (goal_selection){
// nav_msgs::Path path;
// ROS_INFO("compute path");
// if(goal_selection){
// if(planner->planning(path)){
// path_pub.publish(path);
// }
// else{
// ROS_INFO("no path exist for desired goal, please choose another goal");
// goal_selection = true;
// }
// ROS_INFO("path_computed");
// }
// }
}
}
void CloudAssimilator::filterCloud(pcl::PointCloud<pcl::PointXYZ>& combined_cloud, pcl::PointCloud<pcl::PointXYZ>& filtered_cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr combined_cloud_ptr = combined_cloud.makeShared();
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(combined_cloud_ptr);
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
double radius = m_neighborhood_radius;
// ROS_INFO("Filtering based on %d neighbors in a %g radius", m_min_neighbors, m_neighborhood_radius);
filtered_cloud.points.clear();
for(unsigned int i = 0; i < combined_cloud.points.size(); i++)
{
int num_neighbors = kdtree.radiusSearch(combined_cloud.points[i], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
if(num_neighbors > m_min_neighbors)
{
filtered_cloud.points.push_back(combined_cloud.points[i]);
}
}
filtered_cloud.height = 1;
filtered_cloud.width = filtered_cloud.points.size();
// double dx = combined_cloud.points[0].x - combined_cloud.points[1].x;
// double dy = combined_cloud.points[0].y - combined_cloud.points[1].y;
// double dz = combined_cloud.points[0].z - combined_cloud.points[1].z;
// double sanity_distance = sqrt(dx*dx+dy*dy+dz*dz);
// std::cerr << sanity_distance << std::endl;
// pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
// sor.setInputCloud(obstacle_cloud_ptr);
// sor.setMeanK(3);
// sor.setStddevMulThresh(1.0);
// sor.filter(obstacle_cloud_filtered);
// pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
// outrem.setInputCloud(obstacle_cloud_ptr);
// outrem.setRadiusSearch(3000.0);
// outrem.setMinNeighborsInRadius(1);
// outrem.filter(obstacle_cloud_filtered);
}
void segmentate(pcl::PointCloud<pcl::PointXYZRGB>& cloud, double threshould) {
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 (threshould);
seg.setInputCloud (cloud.makeShared ());
seg.segment (*inliers, *coefficients);
for (size_t i = 0; i < inliers->indices.size (); ++i) {
cloud.points[inliers->indices[i]].r = 255;
cloud.points[inliers->indices[i]].g = 0;
cloud.points[inliers->indices[i]].b = 0;
}
}
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
PCProc<pcl::PointXYZRGBA> pc1;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr output1 (new pcl::PointCloud<pcl::PointXYZRGBA>);
if(bVoxelGrid)
{
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr inputc = cloud;//.makeShared();
pc1.downSampling(inputc,output1 , leafsz,leafsz,leafsz);
}
else
{
output1 = cloud->makeShared();
}
///////////////////////////////////////
// passthrough
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr output2 (new pcl::PointCloud<pcl::PointXYZRGBA>);
pc1.PassThroughZ(output1, output2, 1, 3);
cloud_filtered = output2;
///////////////////////////////////////
// normal display
pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
ne.setInputCloud (output2);
// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
ne.setSearchMethod (tree);
// Output datasets
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);
// Compute the features
ne.compute (*cloud_normals);
//viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud, cloud_normals, 1, 0.01, "normals1", 0);
int nf = cloud_filtered->size();
int n = cloud->size();
lpMapping[0] = n;
lpMapping[1] = nf;
lpMapping[2] = bVoxelGrid;
lpMapping[3] = leafsz;
;
int i;
for(i=0; i<nf; i++)//DATA_LEN/6
{
float x = cloud_filtered->points[i].x;
lpMapping[HEADER_LEN + 6*i +0] = cloud_filtered->points[i].x;
lpMapping[HEADER_LEN + 6*i +1] = cloud_filtered->points[i].y;
lpMapping[HEADER_LEN + 6*i +2] = cloud_filtered->points[i].z;
lpMapping[HEADER_LEN + 6*i +3] = cloud_filtered->points[i].r;
lpMapping[HEADER_LEN + 6*i +4] = cloud_filtered->points[i].g;
lpMapping[HEADER_LEN + 6*i +5] = cloud_filtered->points[i].b;
}
//viewer.addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
if (!viewer.wasStopped())
{
viewer.showCloud (cloud_filtered);
}
}
/*!
* @brief メソッドPointCloudMethod::extractPlane().平面を検出するメソッド
* @param pcl::PointCloud<pcl::PointXYZ>::Ptr inputPointCloud
* @return pcl::PointCloud<pcl::PointXYZ>::Ptr outputPointCloud
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr PointCloudMethod::extractPlane(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &inputPointCloud, bool optimize, double threshold, bool negative)
{
cout << "before Extract Plane => " << inputPointCloud->size() << endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
//セグメンテーションオブジェクトの生成
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
//オプション
seg.setOptimizeCoefficients(optimize);
//Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(threshold);
seg.setInputCloud(inputPointCloud->makeShared());
seg.segment(*inliers, *coefficients);
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZRGB>());
//int i = 0, nr_points = (int)inputPointCloud->points.size();
//while (inputPointCloud->points.size() > 0.3*nr_points)
//{
// seg.setInputCloud(inputPointCloud);
// seg.segment(*inliers, *coefficients);
// if (inliers->indices.size() == 0)
// {
// cout << "Could not estimate a planar model for the given dataset." << endl;
// break;
// }
// pcl::ExtractIndices<pcl::PointXYZRGB> extract;
// extract.setInputCloud(inputPointCloud);
// extract.setIndices(inliers);
// extract.setNegative(false);
// extract.filter(*filtered);
// extract.setNegative(true);
// extract.filter(*cloud_f);
// *inputPointCloud = *cloud_f;
//}
//pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
//tree->setInputCloud(filtered);
//vector<pcl::PointIndices> cluster_indices;
//pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
//ec.setClusterTolerance(0.02); //cm
//ec.setMinClusterSize(100);
//ec.setMaxClusterSize(25000);
//ec.setSearchMethod(tree);
//ec.setInputCloud(filtered);
//ec.extract(cluster_indices);
//int j = 0;
//for (vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
//{
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
// for (vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
// {
// cloud_cluster->points.push_back(filtered->points[*pit]);
// }
// cloud_cluster->width = cloud_cluster->points.size();
// cloud_cluster->height = 1;
// cloud_cluster->is_dense = true;
// filtered = cloud_cluster;
// j++;
//}
/*for (size_t i = 0; i < inliers->indices.size(); ++i){
inputPointCloud->points[inliers->indices[i]].r = 255;
inputPointCloud->points[inliers->indices[i]].g = 255;
inputPointCloud->points[inliers->indices[i]].b = 255;
}*/
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(inputPointCloud);
extract.setIndices(inliers);
extract.setNegative(negative);
extract.filter(*filtered);
cout << "after Extract Plane => " << filtered->size() << endl;
return filtered;
}
/* ------------------------------------------------------------------------------------------ */
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud) {
pthread_mutex_lock(&lock);
point_cloud_ptr = cloud->makeShared();
pthread_mutex_unlock(&lock);
}
void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
if ((cloud->width * cloud->height) == 0)
return;
//ROS_INFO ("Received %d data points in frame %s with the following fields: %s", (int)cloud->width * cloud->height, cloud->header.frame_id.c_str (), pcl::getFieldsList (*cloud).c_str ());
//std::cout << "fromROSMsg?" << std::endl;
pcl::fromROSMsg (*cloud, cloud_xyz_);
//std::cout << " fromROSMsg done." << std::endl;
t0 = my_clock();
if( limitPoint( cloud_xyz_, cloud_xyz, distance_th ) > 10 ){
//std::cout << " limit done." << std::endl;
std::cout << "compute normals and voxelize...." << std::endl;
//****************************************
//* compute normals
n3d.setInputCloud (cloud_xyz.makeShared());
n3d.setRadiusSearch (normals_radius_search);
normals_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
n3d.setSearchMethod (normals_tree);
n3d.compute (cloud_normal);
pcl::concatenateFields (cloud_xyz, cloud_normal, cloud_xyz_normal);
t0_2 = my_clock();
//* voxelize
getVoxelGrid( grid, cloud_xyz_normal, cloud_downsampled, voxel_size );
std::cout << " ...done.." << std::endl;
const int pnum = cloud_downsampled.points.size();
float x_min = 10000000, y_min = 10000000, z_min = 10000000;
float x_max = -10000000, y_max = -10000000, z_max = -10000000;
for( int p=0; p<pnum; p++ ){
if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
}
//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
//std::cout << grid.getMinBoxCoordinates() << std::endl;
std::cout << "search start..." << std::endl;
//****************************************
//* object detection start
t1 = my_clock();
search_obj.cleanData();
search_obj.setGRSD( dim, grid, cloud_xyz_normal, cloud_downsampled, voxel_size, box_size );
t1_2 = my_clock();
if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
search_obj.searchWithoutRotation();
t2 = my_clock();
//* object detection end
//****************************************
std::cout << " ...search done." << std::endl;
tAll += t2 - t0;
process_count++;
std::cout << "normal estimation :"<< t0_2 - t0 << " sec" << std::endl;
std::cout << "voxelize :"<< t1 - t0_2 << " sec" << std::endl;
std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
std::cout << "search : "<< t2 - t1_2 << " sec" <<std::endl;
std::cout << "all processes : "<< t2 - t0 << " sec" << std::endl;
std::cout << "AVERAGE : "<< tAll / process_count << " sec" << std::endl;
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1);
marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
visualization_msgs::MarkerArray marker_array_msg_;
//* show the limited space
visualization_msgs::Marker marker_;
marker_.header.frame_id = "openni_rgb_optical_frame";
marker_.header.stamp = ros::Time::now();
marker_.ns = "BoxEstimation";
marker_.id = -1;
marker_.type = visualization_msgs::Marker::CUBE;
marker_.action = visualization_msgs::Marker::ADD;
marker_.pose.position.x = (x_max+x_min)/2;
marker_.pose.position.y = (y_max+y_min)/2;
marker_.pose.position.z = (z_max+z_min)/2;
marker_.pose.orientation.x = 0;
marker_.pose.orientation.y = 0;
marker_.pose.orientation.z = 0;
marker_.pose.orientation.w = 1;
marker_.scale.x = x_max-x_min;
marker_.scale.y = x_max-x_min;
marker_.scale.z = x_max-x_min;
marker_.color.a = 0.1;
marker_.color.r = 1.0;
marker_.color.g = 0.0;
marker_.color.b = 0.0;
marker_.lifetime = ros::Duration();
marker_pub_.publish(marker_);
for( int q=0; q<rank_num; q++ ){
if( search_obj.maxDot( q ) < detect_th ) break;
std::cout << search_obj.maxX( q ) << " " << search_obj.maxY( q ) << " " << search_obj.maxZ( q ) << std::endl;
std::cout << "dot " << search_obj.maxDot( q ) << std::endl;
//if( (search_obj.maxX( q )!=0)||(search_obj.maxY( q )!=0)||(search_obj.maxZ( q )!=0) ){
//* publish marker
visualization_msgs::Marker marker_;
//marker_.header.frame_id = "base_link";
marker_.header.frame_id = "openni_rgb_optical_frame";
marker_.header.stamp = ros::Time::now();
marker_.ns = "BoxEstimation";
marker_.id = q;
marker_.type = visualization_msgs::Marker::CUBE;
marker_.action = visualization_msgs::Marker::ADD;
marker_.pose.position.x = search_obj.maxX( q ) * region_size + sliding_box_size_x/2 + x_min;
marker_.pose.position.y = search_obj.maxY( q ) * region_size + sliding_box_size_y/2 + y_min;
marker_.pose.position.z = search_obj.maxZ( q ) * region_size + sliding_box_size_z/2 + z_min;
marker_.pose.orientation.x = 0;
marker_.pose.orientation.y = 0;
marker_.pose.orientation.z = 0;
marker_.pose.orientation.w = 1;
marker_.scale.x = sliding_box_size_x;
marker_.scale.y = sliding_box_size_y;
marker_.scale.z = sliding_box_size_z;
marker_.color.a = 0.5;
marker_.color.r = 0.0;
marker_.color.g = 1.0;
marker_.color.b = 0.0;
marker_.lifetime = ros::Duration();
// std::cerr << "BOX MARKER COMPUTED, WITH FRAME " << marker_.header.frame_id << " POSITION: "
// << marker_.pose.position.x << " " << marker_.pose.position.y << " "
// << marker_.pose.position.z << std::endl;
// marker_pub_.publish (marker_);
// }
marker_array_msg_.markers.push_back(marker_);
}
//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl;
marker_array_pub_.publish(marker_array_msg_);
}
std::cout << "Waiting msg..." << std::endl;
}
//***********************************************************
// segmentation
// セグメンテーションを行う．
//***********************************************************
void segmentation(pcl::PointCloud<pcl::PointXYZRGB>& cloud, double th, int c)
{
std::cout << "segmentation" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
for(int i=0;i<cloud.points.size();i++){
if((m_size.max_z <= cloud.points[i].z) && (cloud.points[i].z <= m_size.max_z+0.3)){
tmp_cloud->points.push_back(cloud.points[i]);
}
}
pcl::copyPointCloud(*tmp_cloud, cloud);
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud.makeShared ());
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (th);
ec.setMinClusterSize (200);
ec.setMaxClusterSize (300000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud.makeShared ());
ec.extract (cluster_indices);
std::cout << "クラスタリング開始" << std::endl;
int m = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZHSV>::Ptr cloud_cluster_hsv (new pcl::PointCloud<pcl::PointXYZHSV>);
pcl::PointXYZ g;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){
cloud_cluster_rgb->points.push_back (cloud.points[*pit]);
}
cloud_cluster_rgb->width = cloud_cluster_rgb->points.size ();
cloud_cluster_rgb->height = 1;
cloud_cluster_rgb->is_dense = true;
//クラスタ重心を求める
g = g_pos(*cloud_cluster_rgb);
if(((g.x < m_size.min_x) || (m_size.max_x < g.x)) ||
((g.y < m_size.min_y) || (m_size.max_y < g.y))){
continue;
}
m++;
if((m_size.max_z + 0.02 < g.z) && (g.z < m_size.max_z + 0.12)){
object_map map;
map.cloud_rgb = *cloud_cluster_rgb;
map.g = g;
map.tf = c;
make_elevation(map);
// RGB -> HSV
pcl::PointCloudXYZRGBtoXYZHSV(*cloud_cluster_rgb, *cloud_cluster_hsv);
for(int i=0;i<MAP_H;i++){
for(int j=0;j<MAP_S;j++){
map.histogram[i][j] = 0.000;
}
}
map.cloud_hsv = *cloud_cluster_hsv;
// H-Sヒストグラムの作成
for(int i=0;i<cloud_cluster_hsv->points.size();i++){
if(((int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360) >= 32) || ((int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360) < 0)) continue;
if(((int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H) >= 32) || ((int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H) < 0)) continue;
// 正規化のため,セグメントの点数で割る.
map.histogram[(int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360)][(int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H)] += 1.000/(float)cloud_cluster_hsv->points.size();
}
Object_Map.push_back(map);
*tmp_rgb += *cloud_cluster_rgb;
}
}
pcl::copyPointCloud(*tmp_rgb, cloud);
return;
}
void getVoxelGrid( pcl::VoxelGrid<T> &grid, pcl::PointCloud<T> input_cloud, pcl::PointCloud<T>& output_cloud, const float voxel_size ){
grid.setLeafSize (voxel_size, voxel_size, voxel_size);
grid.setInputCloud ( input_cloud.makeShared() );
grid.setSaveLeafLayout(true);
grid.filter (output_cloud);
}
template <typename PointInT, typename PointOutT> void
pcl_1_8::Edge<PointInT, PointOutT>::canny (
const pcl::PointCloud<PointInT> &input_x,
const pcl::PointCloud<PointInT> &input_y,
pcl::PointCloud<PointOutT> &output)
{
float tHigh = hysteresis_threshold_high_;
float tLow = hysteresis_threshold_low_;
const int height = input_x.height;
const int width = input_x.width;
output.resize (height * width);
output.height = height;
output.width = width;
// Noise reduction using gaussian blurring
pcl::PointCloud<pcl::PointXYZI>::Ptr gaussian_kernel (new pcl::PointCloud<pcl::PointXYZI>);
kernel_.setKernelSize (3);
kernel_.setKernelSigma (1.0);
kernel_.setKernelType (kernel<pcl::PointXYZI>::GAUSSIAN);
kernel_.fetchKernel (*gaussian_kernel);
convolution_.setKernel (*gaussian_kernel);
PointCloudIn smoothed_cloud_x;
convolution_.setInputCloud (input_x.makeShared());
convolution_.filter (smoothed_cloud_x);
PointCloudIn smoothed_cloud_y;
convolution_.setInputCloud (input_y.makeShared());
convolution_.filter (smoothed_cloud_y);
// Edge detection usign Sobel
pcl::PointCloud<PointXYZIEdge>::Ptr edges (new pcl::PointCloud<PointXYZIEdge>);
sobelMagnitudeDirection (smoothed_cloud_x, smoothed_cloud_y, *edges.get ());
// Edge discretization
discretizeAngles (*edges);
pcl::PointCloud<pcl::PointXYZI>::Ptr maxima (new pcl::PointCloud<pcl::PointXYZI>);
suppressNonMaxima (*edges, *maxima, tLow);
// Edge tracing
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
continue;
(*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
cannyTraceEdge ( 1, 0, i, j, *maxima);
cannyTraceEdge (-1, 0, i, j, *maxima);
cannyTraceEdge ( 1, 1, i, j, *maxima);
cannyTraceEdge (-1, -1, i, j, *maxima);
cannyTraceEdge ( 0, -1, i, j, *maxima);
cannyTraceEdge ( 0, 1, i, j, *maxima);
cannyTraceEdge (-1, 1, i, j, *maxima);
cannyTraceEdge ( 1, -1, i, j, *maxima);
}
}
// Final thresholding
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
if ((*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
output (j, i).magnitude = 255;
else
output (j, i).magnitude = 0;
}
}
}
void wallCallback(const sensor_msgs::PointCloud2& wall_msg){
pcl::fromROSMsg(wall_msg, wall_pcl);
wall_kdTree.setInputCloud(wall_pcl.makeShared());
wall_flag = true;
}
