void pass_through(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out,
const Eigen::VectorXf lims)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud_filtered_x (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_filtered_y (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PassThrough<pcl::PointXYZ> pass;
if(lims.size() != 6)
{
ROS_WARN("Limits for pass-through wrong size");
return;
}
pass.setInputCloud(cloud_in);
pass.setFilterFieldName("x");
pass.setFilterLimits(lims(0), lims(1));
pass.filter(*cloud_filtered_x);
pass.setInputCloud(cloud_filtered_x);
pass.setFilterFieldName("y");
pass.setFilterLimits(lims(2), lims(3));
pass.filter(*cloud_filtered_y);
pass.setInputCloud(cloud_filtered_y);
pass.setFilterFieldName("z");
pass.setFilterLimits(lims(4), lims(5));
pass.filter(*cloud_out);
return;
}
void ptf_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_x(new pcl::PointCloud<pcl::PointXYZ>),
cloud_filtered_xy(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_temp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_wall(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_wall_projected(new pcl::PointCloud<pcl::PointXYZ>);
// the aim is to filter out the wall planes from the image ....
pcl::fromROSMsg(*cloud_msg, *cloud);
// ROS_INFO("cloud size here1 is %d, %d", cloud->width , cloud->height);
ROS_INFO("number of points in cloud %d", cloud->points.size());
if(cloud->width * cloud->height > 0){
pcl::PassThrough<pcl::PointXYZ> pass_x;
// removing nans and unimportant points.
pass_x.setInputCloud(cloud);
pass_x.setFilterFieldName("x");
pass_x.setFilterLimits(x_min, x_max);
pass_x.filter(*cloud_filtered_x);
ROS_INFO("cloud size here 2 is %d", cloud_filtered_x->points.size());
pcl::PassThrough<pcl::PointXYZ> pass_y;
pass_y.setInputCloud(cloud_filtered_x);
pass_y.setFilterFieldName("y");
pass_y.setFilterLimits(y_min, y_max);
pass_y.filter(*cloud_filtered_xy);
ROS_INFO("cloud size here 3 is %d", cloud_filtered_xy->points.size());
pub_cloud2.publish(cloud_filtered_xy);
pcl::PassThrough<pcl::PointXYZ> pass_z;
pass_z.setInputCloud(cloud_filtered_xy);
pass_z.setFilterFieldName("z");
pass_z.setFilterLimits(z_min, z_max);
pass_z.filter(*cloud_filtered_xyz);
cloud_filtered_temp = cloud_filtered_xy;
ROS_INFO("cloud size here 4 is %d", cloud_filtered_xyz->points.size());
if(cloud_filtered_xy->points.size() > 1000){
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.05);
seg.setInputCloud(cloud_filtered_temp);
seg.segment(*inliers, *coefficients);
int num_points = (int) cloud_filtered_temp->points.size();
if(inliers->indices.size() > POINT_THRESHOLD ){
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered_temp);
extract.setIndices (inliers);
extract.setNegative(true);
extract.filter(*cloud_filtered_temp);
// pcl::SACSegmentation<pcl::PointXYZ> seg1;
// seg1.setOptimizeCoefficients(true);
// seg1.setModelType (pcl::SACMODEL_PLANE);
// seg1.setMethodType (pcl::SAC_RANSAC);
// seg1.setMaxIterations(1000);
// seg1.setDistanceThreshold(0.01);
// seg1.setInputCloud(cloud_filtered_temp);
// seg1.segment(*inliers, *coefficients);
}
}
ROS_INFO("cloud size here 5 is %d", cloud_filtered_temp->points.size());
// // seperate the walls
pcl::PassThrough<pcl::PointXYZ> pass_wall;
pass_wall.setInputCloud(cloud_filtered_xy);
pass_wall.setFilterFieldName("y");
pass_wall.setFilterLimits(0.005, 0.55);
pass_wall.filter(*cloud_filtered_wall);
// pub_cloud1.publish(cloud_filtered_wall);
// pub_cloud3.publish(cloud_filtered_wall);
pcl::ModelCoefficients::Ptr wall_coefficients (new pcl::ModelCoefficients ());
wall_coefficients->values.resize (4);
wall_coefficients->values[0] = wall_coefficients->values[1] = 0;
wall_coefficients->values[2] = 1.0;
wall_coefficients->values[3] = 0;
// Create the filtering object
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setInputCloud (cloud_filtered_temp);
proj.setModelCoefficients (wall_coefficients);
proj.filter(*cloud_wall_projected);
pub_cloud3.publish(cloud_filtered_temp);
// ROS_INFO("cloud size here 4 is %d", cloud_wall_projected->points.size());
}
}
void
marker_cb (const visualization_msgs::MarkerArray::ConstPtr& inputMarkerArray)
{
colour_recognizer::ObjectArray objectArrayTemp;
visualization_msgs::MarkerArray tempMarkerArray;
myPointCloud::Ptr cloud_filtered_sum (new myPointCloud);
// myPointCloud::Ptr cloud_filtered_sum ;
int flag=0;
// find out mean of all points within a marker array
for(int i=0;i<inputMarkerArray->markers.size();i++){
colour_recognizer::ObjectProperty objectTemp;
float xSum=0.0;
float ySum=0.0;
float zSum=0.0;
for(int j=0;j<inputMarkerArray->markers[i].points.size();j++){
xSum+=inputMarkerArray->markers[i].points[j].x;
ySum+=inputMarkerArray->markers[i].points[j].y;
zSum+=inputMarkerArray->markers[i].points[j].z;
}
float xPos=xSum/inputMarkerArray->markers[i].points.size();
float yPos=ySum/inputMarkerArray->markers[i].points.size();
float zPos=zSum/inputMarkerArray->markers[i].points.size();
// standard deviation of all points wihtin a marker array
float xVarSum=0.0;
float yVarSum=0.0;
float zVarSum=0.0;
for(int j=0;j<inputMarkerArray->markers[i].points.size();j++){
xVarSum+=pow(inputMarkerArray->markers[i].points[j].x - xPos,2);
yVarSum+=pow(inputMarkerArray->markers[i].points[j].y - yPos,2);
zVarSum+=pow(inputMarkerArray->markers[i].points[j].z - zPos,2);
}
float xStd=sqrt(xVarSum/inputMarkerArray->markers[i].points.size());
float yStd=sqrt(yVarSum/inputMarkerArray->markers[i].points.size());
float zStd=sqrt(zVarSum/inputMarkerArray->markers[i].points.size());
// making a marker cube with dimensions of 2 SD along the sides, 4 SD would ensure 98% of the points, but
// the cloud does not align very well with a large cube. 2 SD seems satisfactory
visualization_msgs::Marker tempMarker;
tempMarker.header.frame_id = inputMarkerArray->markers[i].header.frame_id;
tempMarker.header.stamp = ros::Time();
tempMarker.ns = "my_namespace";
tempMarker.id = inputMarkerArray->markers[i].id;
tempMarker.type = visualization_msgs::Marker::CUBE;
tempMarker.action = visualization_msgs::Marker::ADD;
tempMarker.pose.position.x = xPos;
tempMarker.pose.position.y = yPos;
tempMarker.pose.position.z = zPos;
tempMarker.pose.orientation.x = 0.0;
tempMarker.pose.orientation.y = 0.0;
tempMarker.pose.orientation.z = 0.0;
tempMarker.pose.orientation.w = 1.0;
tempMarker.scale.x = 2*xStd;
tempMarker.scale.y = 2*yStd;
tempMarker.scale.z = 2*zStd;
tempMarker.color.a = 1.0;
tempMarker.color.r = 0.0;
tempMarker.color.g = 1.0;
tempMarker.color.b = 0.0;
tempMarkerArray.markers.push_back(tempMarker);
myPointCloud::Ptr cloud_filtered_z (new myPointCloud);
myPointCloud::Ptr cloud_filtered_y (new myPointCloud);
myPointCloud::Ptr cloud_filtered_x (new myPointCloud);
// myPointCloud::Ptr cloud_filtered_z;
// myPointCloud::Ptr cloud_filtered_y;
// myPointCloud::Ptr cloud_filtered_x;
pcl::PassThrough<pcl::PointXYZRGB> pass_z(true);
pcl::PassThrough<pcl::PointXYZRGB> pass_y;
pcl::PassThrough<pcl::PointXYZRGB> pass_x;
IndicesPtr zPointer (new std::vector<int>);
IndicesPtr yPointer (new std::vector<int>);
IndicesPtr xPointer (new std::vector<int>);
pass_z.setInputCloud (globalCloud);
pass_z.setFilterFieldName ("z");
pass_z.setFilterLimits (zPos-zStd, zPos+zStd);
pass_z.filter (*zPointer);
// pass_z.setFilterFieldName ("y");
// pass_z.setFilterLimits (yPos-yStd, yPos+yStd);
pass_z.setIndices (zPointer);
pass_z.setFilterFieldName ("x");
pass_z.setFilterLimits (xPos-xStd, xPos+xStd);
pass_z.filter (*xPointer);
pass_z.setIndices(xPointer);
pass_z.setFilterFieldName ("y");
pass_z.setFilterLimits (yPos-yStd, yPos+yStd);
// pass_z.filter (*zPointer);
//pass.setFilterLimitsNegative (true);
pass_z.filter (*cloud_filtered_y);
// pass_y.setInputCloud (cloud_filtered_z);
// pass_y.setFilterFieldName ("y");
// pass_y.setFilterLimits (yPos-yStd, yPos+yStd);
// //pass.setFilterLimitsNegative (true);
// pass_z.filter (*cloud_filtered_y);
// pass_x.setInputCloud (cloud_filtered_y);
// pass_x.setFilterFieldName ("x");
// pass_x.setFilterLimits (xPos-xStd, xPos+xStd);
// //pass.setFilterLimitsNegative (true);
// pass_x.filter (*cloud_filtered_x);
int sizeCloud = cloud_filtered_y->size();
if(sizeCloud==0){
sizeCloud+=1;
}
if(flag==0){
flag=1;
*cloud_filtered_sum=*cloud_filtered_y;
// ROS_INFO("size: %zu",(*cloud_filtered_y).size());
int redColor=0;
int blueColor=0;
int greenColor=0;
// float xLoc=0.0;
// float yLoc=0.0;
// float zLoc=0.0;
for(int pointCount=0;pointCount<cloud_filtered_y->size();pointCount++){
redColor += (*cloud_filtered_y)[pointCount].r;
greenColor += (*cloud_filtered_y)[pointCount].g;
blueColor += (*cloud_filtered_y)[pointCount].b;
// xLoc+= (*cloud_filtered_y)[pointCount].x;
// yLoc+= (*cloud_filtered_y)[pointCount].y;
// zLoc+= (*cloud_filtered_y)[pointCount].z;
// ROS_INFO("G value: %d",(*cloud_filtered_y)[pointCount].g);
// ROS_INFO("R value: %d",(*cloud_filtered_y)[pointCount].r);
// ROS_INFO("B value: %d",(*cloud_filtered_y)[pointCount].b);
// ROS_INFO("type : %s", typeid((*cloud_filtered_y)[pointCount].g).name());
}
// ROS_INFO("end");
int redColorAvg = ((int)(redColor/sizeCloud));
int greenColorAvg = ((int)(greenColor/sizeCloud));
int blueColorAvg = ((int)(blueColor/sizeCloud));
int minDist = 999999;
int minIndex = 0;
for (int colourCount = 0;colourCount<colorNumbersUsed;colourCount++){
int colourDist = (redColorAvg - redArray[colourCount])*(redColorAvg - redArray[colourCount]) + (greenColorAvg - greenArray[colourCount])*(greenColorAvg - greenArray[colourCount]) + (blueColorAvg - blueArray[colourCount])*(blueColorAvg - blueArray[colourCount]) ;
if(colourDist<minDist){
minDist = colourDist;
minIndex = colourCount;
}
}
colour_recognizer::ObjectProperty tempProperty;
tempProperty.id=inputMarkerArray->markers[i].id;
tempProperty.colour = colourNameArray[minIndex];
tempProperty.locationX=xPos;
tempProperty.locationY=yPos;
tempProperty.locationZ=zPos;
tempProperty.objectType = "dunno";
tempProperty.colourHist[0]=redColorAvg;
tempProperty.colourHist[1]=greenColorAvg;
tempProperty.colourHist[2]=blueColorAvg;
// arrayTemp[0]=int(redColor/cloud_filtered_y->size());
// arrayTemp[1]=int(greenColor/cloud_filtered_y->size();
// arrayTemp[2]=int(blueColor/cloud_filtered_y->size());
// tempProperty.colourHist=arrayTemp;
objectArrayTemp.objects.push_back(tempProperty);
}
else{
*cloud_filtered_sum+=*cloud_filtered_y;
int redColor=0;
int blueColor=0;
int greenColor=0;
// float xLoc=0.0;
// float yLoc=0.0;
// float zLoc=0.0;
for(int pointCount=0;pointCount<cloud_filtered_y->size();pointCount++){
redColor += (*cloud_filtered_y)[pointCount].r;
greenColor += (*cloud_filtered_y)[pointCount].g;
blueColor += (*cloud_filtered_y)[pointCount].b;
// xLoc+= (*cloud_filtered_y)[pointCount].x;
// yLoc+= (*cloud_filtered_y)[pointCount].y;
// zLoc+= (*cloud_filtered_y)[pointCount].z;
// ROS_INFO("G value: %d",(*cloud_filtered_y)[pointCount].g);
// ROS_INFO("R value: %d",(*cloud_filtered_y)[pointCount].r);
// ROS_INFO("B value: %d",(*cloud_filtered_y)[pointCount].b);
// ROS_INFO("type : %s", typeid((*cloud_filtered_y)[pointCount].g).name());
}
// ROS_INFO("end");
int redColorAvg = ((int)(redColor/sizeCloud));
int greenColorAvg = ((int)(greenColor/sizeCloud));
int blueColorAvg = ((int)(blueColor/sizeCloud));
int minDist = 999999;
int minIndex = 0;
for (int colourCount = 0;colourCount<colorNumbersUsed;colourCount++){
int colourDist = (redColorAvg - redArray[colourCount])*(redColorAvg - redArray[colourCount]) + (greenColorAvg - greenArray[colourCount])*(greenColorAvg - greenArray[colourCount]) + (blueColorAvg - blueArray[colourCount])*(blueColorAvg - blueArray[colourCount]) ;
if(colourDist<minDist){
minDist = colourDist;
minIndex = colourCount;
}
}
colour_recognizer::ObjectProperty tempProperty;
tempProperty.id=inputMarkerArray->markers[i].id;
tempProperty.colour = colourNameArray[minIndex];
tempProperty.locationX=xPos;
tempProperty.locationY=yPos;
tempProperty.locationZ=zPos;
tempProperty.objectType = "dunno";
tempProperty.colourHist[0]=redColorAvg;
tempProperty.colourHist[1]=greenColorAvg;
tempProperty.colourHist[2]=blueColorAvg;
// arrayTemp[0]=int(redColor/cloud_filtered_y->size());
// arrayTemp[1]=int(greenColor/cloud_filtered_y->size();
// arrayTemp[2]=int(blueColor/cloud_filtered_y->size());
// tempProperty.colourHist=arrayTemp;
objectArrayTemp.objects.push_back(tempProperty);
}
// cloud_filtered_x->~myPointCloud();
// cloud_filtered_y->~myPointCloud();
// cloud_filtered_z->~myPointCloud();
//pubObjectLocation.publish()
}
//publishing a delayed point cloud and the marker array
// pubObjectLocation.publish(tempMarkerArray);
pubObjectLocation.publish(objectArrayTemp);
pubCloud.publish(*cloud_filtered_sum);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr ObjectDetector::filterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
ROS_INFO("Before passthrough: %d", cloud->width * cloud->height);
if((cloud->width * cloud->height) != 0){
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_x(new pcl::PointCloud<pcl::PointXYZ>), cloud_filtered_y (new pcl::PointCloud<pcl::PointXYZ>), cloud_filtered_z (new pcl::PointCloud<pcl::PointXYZ>);
// Filter x outliers
pcl::PassThrough<pcl::PointXYZ> pass_x;
pass_x.setInputCloud (cloud);
pass_x.setFilterFieldName ("x");
pass_x.setFilterLimits (-x_max, x_max);
pass_x.filter (*cloud_filtered_x);
// Filter y outliers
pcl::PassThrough<pcl::PointXYZ> pass_y;
pass_y.setInputCloud (cloud_filtered_x);
pass_y.setFilterFieldName ("y");
pass_y.setFilterLimits (floor_threshold, y_max);
pass_y.filter (*cloud_filtered_y);
// Filter z outliers
pcl::PassThrough<pcl::PointXYZ> pass_z;
pass_z.setInputCloud (cloud_filtered_y);
pass_z.setFilterFieldName ("z");
pass_z.setFilterLimits (-z_max, 0.0);
pass_z.filter (*cloud_filtered_z);
ROS_INFO("Before RANSAC: %d", cloud_filtered_z->width * cloud_filtered_z->height);
if((cloud_filtered_z->width * cloud_filtered_z->height) > min_points_left)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ransac (new pcl::PointCloud<pcl::PointXYZ>);
cloud_ransac = cloud_filtered_z;
pcl::ModelCoefficients::Ptr ransac_coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Fit plane
seg.setModelType (pcl::SACMODEL_PLANE);
// Use RANSAC
seg.setMethodType (pcl::SAC_RANSAC);
// Set maximum number of iterations
seg.setMaxIterations (max_it_runtime);
// Set distance to the model threshold
seg.setDistanceThreshold (wall_threshold);
// Segment the largest planar component from the cloud
seg.setInputCloud (cloud_ransac);
seg.segment (*inliers, *ransac_coefficients);
while(inliers->indices.size() > ransac_removal_threshold){
// Extract the inliers
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_ransac);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*cloud_ransac);
pcl::SACSegmentation<pcl::PointXYZ> seg2;
// Optional
seg2.setOptimizeCoefficients (true);
// Fit plane
seg2.setModelType (pcl::SACMODEL_PLANE);
// Use RANSAC
seg2.setMethodType (pcl::SAC_RANSAC);
// Set maximum number of iterations
seg2.setMaxIterations (max_it_runtime);
// Set distance to the model threshold
seg2.setDistanceThreshold (wall_threshold);
// Segment the largest planar component from the cloud
seg2.setInputCloud (cloud_ransac);
seg2.segment (*inliers, *ransac_coefficients);
}
ROS_INFO("After RANSAC: %d", cloud_ransac->width * cloud_ransac->height);
if((cloud_ransac->width * cloud_ransac->height) > min_points_left){
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_inc_walls (new pcl::PointCloud<pcl::PointXYZ>), cloud_filtered_only_walls (new pcl::PointCloud<pcl::PointXYZ>), cloud_projected_iw (new pcl::PointCloud<pcl::PointXYZ>), cloud_projected_ow (new pcl::PointCloud<pcl::PointXYZ>), cloud_projected_filtered (new pcl::PointCloud<pcl::PointXYZ>);
// Points containing both walls + objects
pcl::PassThrough<pcl::PointXYZ> pass_iw;
pass_iw.setInputCloud (cloud_ransac);
pass_iw.setFilterFieldName ("y");
pass_iw.setFilterLimits (floor_threshold, object_wall_threshold);
pass_iw.filter (*cloud_filtered_inc_walls);
// Points only containing walls
pcl::PassThrough<pcl::PointXYZ> pass_ow;
pass_ow.setInputCloud (cloud_ransac);
pass_ow.setFilterFieldName ("y");
pass_ow.setFilterLimits (object_wall_threshold, y_max);
pass_ow.filter (*cloud_filtered_only_walls);
if(((cloud_filtered_inc_walls->width * cloud_filtered_inc_walls->height) != 0) && ((cloud_filtered_only_walls->width * cloud_filtered_only_walls->height) != 0) )
{
// Create a set of planar coefficients with X=Z=0,Y=1
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
coefficients->values.resize (4);
coefficients->values[0] = coefficients->values[2] = 0;
coefficients->values[1] = 1.0;
coefficients->values[3] = 0;
// Eliminate y-axis by projection to 2D
pcl::ProjectInliers<pcl::PointXYZ> proj1;
proj1.setModelType (pcl::SACMODEL_PLANE);
proj1.setInputCloud (cloud_filtered_inc_walls);
proj1.setModelCoefficients (coefficients);
proj1.filter (*cloud_projected_iw);
pcl::ProjectInliers<pcl::PointXYZ> proj2;
proj2.setModelType (pcl::SACMODEL_PLANE);
proj2.setInputCloud (cloud_filtered_only_walls);
proj2.setModelCoefficients (coefficients);
proj2.filter (*cloud_projected_ow);
// Remove projected walls from cloud_projected_iw
if((cloud_projected_iw->width * cloud_projected_iw->height) != 0)
{
// Create kdtree
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud (cloud_projected_iw);
double radius = wall_threshold;
pcl::PointIndices::Ptr wallpoint_indices(new pcl::PointIndices);
// Create a set of already included indices
boost::unordered::unordered_set<int> inc_indices;
// Iterate over all points in cloud containing only walls
for (int i = 0; i < cloud_projected_ow->size(); i++)
{
pcl::PointXYZ owPoint = cloud_projected_ow->at(i) ;
std::vector<int> k_indices;
std::vector<float> k_distances;
// Find points which are closes neighbors to the walls
if(kdtree.radiusSearch (owPoint, radius, k_indices, k_distances) > 0){
// Iterate over points
for(std::vector<int>::size_type i = 0; i != k_indices.size(); i++) {
int index = k_indices[i];
// If no match is found for index, add to set & wallpoint indices
if (inc_indices.find(index) == inc_indices.end()){
inc_indices.insert(index);
wallpoint_indices->indices.push_back(index);
}
}
}
}
if(wallpoint_indices->indices.size() > 0){
ROS_INFO("Filter points: %d", int(wallpoint_indices->indices.size()));
// Remove the wall points
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered_inc_walls);
extract.setIndices (wallpoint_indices);
extract.setNegative (true);
extract.filter (*cloud_projected_filtered);
}
else{
cloud_projected_filtered = cloud_filtered_inc_walls;
}
ROS_INFO("After WF: %d", cloud_projected_filtered->width * cloud_projected_filtered->height);
return cloud_projected_filtered;
}
else
{
return cloud_filtered_inc_walls; // correct?
}
}
else
{
return cloud_ransac; // correct?
}
}
else
{
return cloud_ransac;
}
}
else
{
return cloud_filtered_z;
}
}
else
{
return cloud;
}
}
