/************************************************************************************************** Software License Agreement (BSD License) Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: *Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. *Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. *Neither the name of the University of Aveiro nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ***************************************************************************************************/ #include /** \brief Library functions for Navigability_Map. This library provides a stack of function for a Accessibility Map. \file navigability_map.cpp \author Diogo Matos \date June 2013 */ using namespace std; using namespace cv; void Navigability_Map::Filter_PointCloud(pcl::PointCloud::Ptr &cloud_in,pcl::PointCloud::Ptr &pc_filter) { pcl::PassThrough pass; //Xfilter pass.setInputCloud (cloud_in); pass.setFilterFieldName ("x"); pass.setFilterLimits (Xmin_filter, Xmax_filter); pass.filter (*pc_filter); //Yfilter pass.setInputCloud (pc_filter); pass.setFilterFieldName ("y"); pass.setFilterLimits (Ymin_filter, Ymax_filter); pass.filter (*pc_filter); //zfilter pass.setInputCloud (pc_filter); pass.setFilterFieldName ("z"); pass.setFilterLimits (Zmin_filter, Zmax_filter); pass.filter (*pc_filter); //////// /////////// /////// DEBUG STUF //////////// ////// //////////// // // Car filter /// // pc_filter=cloud_in; // pcl::PointCloud::Ptr aux_Xcloud (new pcl::PointCloud); // pass.setInputCloud (pc_filter); // pass.setFilterFieldName ("x"); // pass.setFilterLimits (0, 3.5); // pass.setFilterLimitsNegative (true); // pass.filter (*aux_Xcloud); // // pcl::PointCloud::Ptr aux_Ycloud (new pcl::PointCloud); // pass.setInputCloud (pc_filter); // pass.setFilterFieldName ("y"); // pass.setFilterLimits (-1.2, 1.2); // pass.setFilterLimitsNegative (true); // pass.filter (*aux_Ycloud); // // *pc_filter=*aux_Ycloud+*aux_Xcloud; // //save cloud // sensor_msgs::PointCloud2 msg; // pcl::PointCloud pc; // pc=*pc_filter; // pcl::toROSMsg(pc, msg); // std::string name = "cenas.pcd"; // pcl::io::savePCDFile(name.c_str(), msg); } void Navigability_Map::Normal_Estimation(pcl::PointCloud::Ptr &cloud) { normals.reset(new pcl::PointCloud); // Normal estimation pcl::NormalEstimation n; pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); tree->setInputCloud (cloud); n.useSensorOriginAsViewPoint(); n.setViewPoint (0.8, 0, 4); //attention to this !!!!!!!!!!!!!!!!!!!!! , this the position in of the laser scan in the car n.setInputCloud (cloud); n.setSearchMethod (tree); //neighbours search method if (Use_Radius_Search) n.setRadiusSearch(Radius_neighbors); else n.setKSearch (K_neighbors); //compute n.compute (*normals); //////// /////////// /////// DEBUG STUF //////////// ////// //////////// // //save normals pcd file // pcl::PointCloud::Ptr cloud_with_normals (new pcl::PointCloud); // pcl::PointCloud::Ptr cenas (new pcl::PointCloud); // cenas=cloud; // pcl::concatenateFields (*cenas, *normals, *cloud_with_normals); // sensor_msgs::PointCloud2 msg; // pcl::PointCloud pc; // pc=*cloud_with_normals; // pcl::toROSMsg(pc, msg); // std::string name = "cenas.pcd"; // pcl::io::savePCDFile(name.c_str(), msg); } void Navigability_Map::setGrid_parameter(double max_Xval,double max_Yval) { //_____________________ //| | //| Save Total Row | //|____________________| total_row=(int)roundl((max_Xval/Sx)+0.5); //+0.5 to round up //______________________ //| | //| Get Center Col | //|____________________| int auxCenterCol=(int)roundl((max_Yval/Sy)+0.5); //+0.5 to round up int center_col; if (auxCenterCol % 2) //auxCenterCol is odd center_col=auxCenterCol; else center_col=auxCenterCol+1; //______________________ //| | //| Save Total Col | //|____________________| total_col=center_col*2+1; //______________________ //| | //| Save Center Col | //|____________________| CARaxis_col=center_col; //Grid center is coincident with the axis } void Navigability_Map::inicialize_grid(void) { //first resize the Grid acording to the parameters grid.resize(total_row,total_col); grid_nodePtr node_data; for(uint row=0;row& cloud) { int row_pos; int col_pos; for (size_t i = 0; i < cloud.points.size (); ++i) { //do not save car data if (cloud.points[i].x<3.8 && (cloud.points[i].y<1.4 && cloud.points[i].y>-1.4)) continue; //Point posistion in the Grid //x row_pos=(int)(cloud.points[i].x/Sx); //y if (cloud.points[i].y>0) col_pos=CARaxis_col+(int)((cloud.points[i].y/Sy)+0.5); //axis car is the center of the mid cell so +0.5 else col_pos=CARaxis_col+(int)((cloud.points[i].y/Sy)-0.5); //axis car is the center of the mid cell so -0.5 //pointer to node_data grid_nodePtr node_data= grid(row_pos,col_pos); // ___________________________________________ //| | //| Matrix Points and Direction_Cosine | //|__________________________________________| node_data->Matrix_Points.conservativeResize(node_data->Matrix_Points.rows()+1,3); //Set data to Matrix //Points node_data->Matrix_Points(node_data->Matrix_Points.rows()-1,0)=(double)cloud.points[i].x; node_data->Matrix_Points(node_data->Matrix_Points.rows()-1,1)=(double)cloud.points[i].y; node_data->Matrix_Points(node_data->Matrix_Points.rows()-1,2)=(double)cloud.points[i].z; //Angle if (!isnan(normals->points[i].normal[0]) && !isnan(normals->points[i].normal[1]) && !isnan(normals->points[i].normal[2])) { node_data->Angles.conservativeResize(node_data->Angles.rows()+1,3); node_data->Angles(node_data->Angles.rows()-1,0)=acos((double)normals->points[i].normal[0]); node_data->Angles(node_data->Angles.rows()-1,1)=acos((double)normals->points[i].normal[1]); node_data->Angles(node_data->Angles.rows()-1,2)=acos((double)normals->points[i].normal[2]); node_data->has_normal=true; } //num_points node_data->num_points=node_data->Matrix_Points.rows(); } } void Navigability_Map::calcGrid_data(void) { grid_nodePtr node_data; for(uint row=0;rownum_points>0) { //get points for (uint i=0; iMatrix_Points.rows(); i++) med_z+=node_data->Matrix_Points(i,2); //Calc mean med_z/=node_data->num_points; //set data to the grid (mean val) node_data->Zmed=med_z; //Calc standard_deviance for Z if (node_data->num_points>1) { for (uint i=0; iMatrix_Points.rows(); i++) standard_deviance_z+=pow(node_data->Matrix_Points(i,2)-med_z,2); standard_deviance_z=sqrt(standard_deviance_z/(node_data->num_points-1)); //set data to the grid (standard_deviance_z) node_data->Standard_Deviation_Z=standard_deviance_z; }//endl Calc standard_deviance for Z /************* Calc Z_confidence *************/ if (node_data->num_points>1) node_data->Z_confidence=1-(node_data->Standard_Deviation_Z/Standard_Deviation_max); //factor else node_data->Z_confidence=default_confidence; //for cell with only 1 points give a residual confidence if (node_data->Z_confidence<0) node_data->Z_confidence=0; /************* calc med Angle *************/ if (node_data->has_normal) { for (uint i=0; iAngles.rows(); i++) { med_angle_X+=node_data->Angles(i,0); med_angle_Y+=node_data->Angles(i,1); med_angle_Z+=node_data->Angles(i,2); } med_angle_X/=node_data->Angles.rows(); med_angle_Y/=node_data->Angles.rows(); med_angle_Z/=node_data->Angles.rows(); //set data node_data->med_angle_X=med_angle_X; node_data->med_angle_Y=med_angle_Y; node_data->med_angle_Z=med_angle_Z; //Calc standard_deviance Angles if (node_data->num_points>1) { for (uint i=0; iAngles.rows(); i++) { standard_deviance_anglex+=pow(node_data->Angles(i,0)-med_angle_X,2); standard_deviance_angley+=pow(node_data->Angles(i,1)-med_angle_Y,2); standard_deviance_anglez+=pow(node_data->Angles(i,2)-med_angle_Z,2); } standard_deviance_anglex=sqrt(standard_deviance_anglex/(node_data->Angles.rows()-1)); standard_deviance_angley=sqrt(standard_deviance_angley/(node_data->Angles.rows()-1)); standard_deviance_anglez=sqrt(standard_deviance_anglez/(node_data->Angles.rows()-1)); //set data to the grid double Angle_confidence_x=1-(standard_deviance_anglex/Standard_Deviation_anglex_max_confidence); double Angle_confidence_y=1-(standard_deviance_angley/Standard_Deviation_angley_max_confidence); double Angle_confidence_z=1-(standard_deviance_anglez/Standard_Deviation_anglez_max_confidence); if (Angle_confidence_x<0) node_data->angle_confidence_x=0; else node_data->angle_confidence_x=Angle_confidence_x; if (Angle_confidence_y<0) node_data->angle_confidence_y=0; else node_data->angle_confidence_y=Angle_confidence_y; if (Angle_confidence_z<0) node_data->angle_confidence_z=0; else node_data->angle_confidence_z=Angle_confidence_z; } else //set a residual value { node_data->angle_confidence_x=default_confidence; node_data->angle_confidence_y=default_confidence; node_data->angle_confidence_z=default_confidence; } }//end if (node_data->has_normal) //// calc med Angle }//end node_data->num_points>0 }//end for col }//end for row } void Navigability_Map::fill_data_cells(void) { std::vector vector_node; grid_nodePtr node_data; grid_nodePtr aux_data; int empty_neighbours=0; int number_neighbours=0; std::vector Zval_neighbours; std::vector angle_x_neighbours; std::vector angle_y_neighbours; std::vector angle_z_neighbours; std::vector Zconfidence_neighbours; std::vector angle_x_confidence_neighbours; std::vector angle_y_confidence_neighbours; std::vector angle_z_confidence_neighbours; for(uint row=0;rownum_points==0) { vector_node=cell_neighbours(row,col,empty_neighbours,number_neighbours); if (!empty_neighbours && number_neighbours>3) //this cell has neighbours so lets set data acording to them { //acess the data in the cell neighbours for (uint i=0; i<8; i++) { aux_data=vector_node[i]; // ___________________________ //| | //| Get Normal | //|__________________________| if (aux_data->has_normal && !aux_data->interpolate_normal_data) { neighbours_has_normal=true; // //angle med_angle_X+=aux_data->med_angle_X; med_angle_Y+=aux_data->med_angle_Y; med_angle_Z+=aux_data->med_angle_Z; // angle mediana angle_x_neighbours.push_back(aux_data->med_angle_X); angle_y_neighbours.push_back(aux_data->med_angle_Y); angle_z_neighbours.push_back(aux_data->med_angle_Z); //confidence angle_x_confidence+=aux_data->angle_confidence_x; angle_y_confidence+=aux_data->angle_confidence_y; angle_z_confidence+=aux_data->angle_confidence_z; angle_x_confidence_neighbours.push_back(aux_data->angle_confidence_x); angle_y_confidence_neighbours.push_back(aux_data->angle_confidence_y); angle_z_confidence_neighbours.push_back(aux_data->angle_confidence_z); number_normal_neighbours++; } // _________________________________________ //| | //| Get Z val & Z Confidence | //|_________________________________________| if (aux_data->num_points>0) { Zval_neighbours.push_back(aux_data->Zmed); Zconfidence_neighbours.push_back(aux_data->Z_confidence); } }//end for // ___________________________ //| | //| use zmediana | //|__________________________| std::sort (Zval_neighbours.begin(), Zval_neighbours.end()); std::sort (Zconfidence_neighbours.begin(), Zconfidence_neighbours.end()); double Zmediana=0; double Zmediana_confidence=0; if (Zval_neighbours.size() % 2) /* is odd */ { Zmediana=Zval_neighbours.at(((Zval_neighbours.size()+1)/2)-1); Zmediana_confidence=Zconfidence_neighbours.at(((Zconfidence_neighbours.size()+1)/2)-1); } else { int pos_vect1=((Zval_neighbours.size()+1)/2)-1; int pos_vect2=(Zval_neighbours.size()+1)/2; Zmediana=(Zval_neighbours.at(pos_vect1)+Zval_neighbours.at(pos_vect2))/2; Zmediana_confidence=(Zconfidence_neighbours.at(pos_vect1)+Zconfidence_neighbours.at(pos_vect2))/2; } // //set the data to this new node node_data->Zmed=Zmediana; node_data->Z_confidence=Zmediana_confidence; node_data->interpolate_z_data=true; // // ___________________________ // //| | // //| Set the mediana normal | // //|__________________________| if (neighbours_has_normal && number_normal_neighbours>1) { std::sort (angle_x_neighbours.begin(), angle_x_neighbours.end()); std::sort (angle_y_neighbours.begin(), angle_y_neighbours.end()); std::sort (angle_z_neighbours.begin(), angle_z_neighbours.end()); std::sort (angle_x_confidence_neighbours.begin(), angle_x_confidence_neighbours.end()); std::sort (angle_y_confidence_neighbours.begin(), angle_y_confidence_neighbours.end()); std::sort (angle_z_confidence_neighbours.begin(), angle_z_confidence_neighbours.end()); double xmediana=0; double ymediana=0; double zmediana=0; double x_confidence_mediana=0; double y_confidence_mediana=0; double z_confidence_mediana=0; if (angle_x_neighbours.size() % 2) /* is odd */ { xmediana=angle_x_neighbours.at(((angle_x_neighbours.size()+1)/2)-1); ymediana=angle_y_neighbours.at(((angle_y_neighbours.size()+1)/2)-1); zmediana=angle_z_neighbours.at(((angle_z_neighbours.size()+1)/2)-1); x_confidence_mediana=angle_x_confidence_neighbours.at(((angle_x_confidence_neighbours.size()+1)/2)-1); y_confidence_mediana=angle_y_confidence_neighbours.at(((angle_y_confidence_neighbours.size()+1)/2)-1); z_confidence_mediana=angle_z_confidence_neighbours.at(((angle_z_confidence_neighbours.size()+1)/2)-1); } else { int pos_vect1=((angle_x_neighbours.size()+1)/2)-1; int pos_vect2=(angle_x_neighbours.size()+1)/2; xmediana=(angle_x_neighbours.at(pos_vect1)+angle_x_neighbours.at(pos_vect2))/2; ymediana=(angle_y_neighbours.at(pos_vect1)+angle_y_neighbours.at(pos_vect2))/2; zmediana=(angle_z_neighbours.at(pos_vect1)+angle_z_neighbours.at(pos_vect2))/2; x_confidence_mediana=(angle_x_confidence_neighbours.at(pos_vect1)+angle_x_confidence_neighbours.at(pos_vect2))/2; y_confidence_mediana=(angle_y_confidence_neighbours.at(pos_vect1)+angle_y_confidence_neighbours.at(pos_vect2))/2; z_confidence_mediana=(angle_z_confidence_neighbours.at(pos_vect1)+angle_z_confidence_neighbours.at(pos_vect2))/2; } node_data->med_angle_X=xmediana; node_data->med_angle_Y=ymediana; node_data->med_angle_Z=zmediana; node_data->angle_confidence_x=x_confidence_mediana; node_data->angle_confidence_y=y_confidence_mediana; node_data->angle_confidence_z=z_confidence_mediana; node_data->has_normal=true; node_data->interpolate_normal_data=true; } else { node_data->has_normal=true; node_data->interpolate_normal_data=true; node_data->med_angle_X=med_angle_X/number_normal_neighbours; node_data->med_angle_Y=med_angle_Y/number_normal_neighbours; node_data->med_angle_Z=med_angle_Z/number_normal_neighbours; node_data->angle_confidence_x=angle_x_confidence/number_normal_neighbours; node_data->angle_confidence_y=angle_y_confidence/number_normal_neighbours; node_data->angle_confidence_z=angle_z_confidence/number_normal_neighbours; } }//end (!empty_neighbours && number_neighbours>3) //this cell has neighbours so lets set data acording to them }//end node_data->num_points==0 // ___________________________ //| | //| Cell with no normal | //|__________________________| if (!node_data->has_normal && node_data->num_points>0) { vector_node=cell_neighbours(row,col,empty_neighbours,number_neighbours); if (!empty_neighbours && number_neighbours>2) //this cell has neighbours so lets set data acording to them { //acess the data in the cell neighbours for (uint i=0; i<8; i++) { aux_data=vector_node[i]; // ___________________________ //| | //| Get Normal | //|__________________________| if (aux_data->has_normal && !aux_data->interpolate_normal_data) { med_angle_X+=aux_data->med_angle_X; med_angle_Y+=aux_data->med_angle_Y; med_angle_Z+=aux_data->med_angle_Z; //angle mediana angle_x_neighbours.push_back(aux_data->med_angle_X); angle_y_neighbours.push_back(aux_data->med_angle_Y); angle_z_neighbours.push_back(aux_data->med_angle_Z); //confidence angle_x_confidence+=aux_data->angle_confidence_x; angle_y_confidence+=aux_data->angle_confidence_y; angle_z_confidence+=aux_data->angle_confidence_z; angle_x_confidence_neighbours.push_back(aux_data->angle_confidence_x); angle_y_confidence_neighbours.push_back(aux_data->angle_confidence_y); angle_z_confidence_neighbours.push_back(aux_data->angle_confidence_z); number_normal_neighbours++; } } // ___________________________ //| | //| Set the med normal | //|__________________________| if (neighbours_has_normal && number_normal_neighbours>1) { node_data->interpolate_normal_data=true; node_data->has_normal=true; // use mediana for angle std::sort (angle_x_neighbours.begin(), angle_x_neighbours.end()); std::sort (angle_y_neighbours.begin(), angle_y_neighbours.end()); std::sort (angle_z_neighbours.begin(), angle_z_neighbours.end()); std::sort (angle_x_confidence_neighbours.begin(), angle_x_confidence_neighbours.end()); std::sort (angle_y_confidence_neighbours.begin(), angle_y_confidence_neighbours.end()); std::sort (angle_z_confidence_neighbours.begin(), angle_z_confidence_neighbours.end()); double xmediana=0; double ymediana=0; double zmediana=0; double x_confidence_mediana=0; double y_confidence_mediana=0; double z_confidence_mediana=0; if (angle_x_neighbours.size() % 2) /* is odd */ { xmediana=angle_x_neighbours.at(((angle_x_neighbours.size()+1)/2)-1); ymediana=angle_y_neighbours.at(((angle_y_neighbours.size()+1)/2)-1); zmediana=angle_z_neighbours.at(((angle_z_neighbours.size()+1)/2)-1); x_confidence_mediana=angle_x_confidence_neighbours.at(((angle_x_confidence_neighbours.size()+1)/2)-1); y_confidence_mediana=angle_y_confidence_neighbours.at(((angle_y_confidence_neighbours.size()+1)/2)-1); z_confidence_mediana=angle_z_confidence_neighbours.at(((angle_z_confidence_neighbours.size()+1)/2)-1); } else { int pos_vect1=((angle_x_neighbours.size()+1)/2)-1; int pos_vect2=(angle_x_neighbours.size()+1)/2; xmediana=(angle_x_neighbours.at(pos_vect1)+angle_x_neighbours.at(pos_vect2))/2; ymediana=(angle_y_neighbours.at(pos_vect1)+angle_y_neighbours.at(pos_vect2))/2; zmediana=(angle_z_neighbours.at(pos_vect1)+angle_z_neighbours.at(pos_vect2))/2; x_confidence_mediana=(angle_x_confidence_neighbours.at(pos_vect1)+angle_x_confidence_neighbours.at(pos_vect2))/2; y_confidence_mediana=(angle_y_confidence_neighbours.at(pos_vect1)+angle_y_confidence_neighbours.at(pos_vect2))/2; z_confidence_mediana=(angle_z_confidence_neighbours.at(pos_vect1)+angle_z_confidence_neighbours.at(pos_vect2))/2; } node_data->med_angle_X=xmediana; node_data->med_angle_Y=ymediana; node_data->med_angle_Z=zmediana; node_data->angle_confidence_x=x_confidence_mediana; node_data->angle_confidence_y=y_confidence_mediana; node_data->angle_confidence_z=z_confidence_mediana; } else { node_data->has_normal=true; node_data->interpolate_normal_data=true; node_data->med_angle_X=med_angle_X/number_normal_neighbours; node_data->med_angle_Y=med_angle_Y/number_normal_neighbours; node_data->med_angle_Z=med_angle_Z/number_normal_neighbours; node_data->angle_confidence_x=angle_x_confidence/number_normal_neighbours; node_data->angle_confidence_y=angle_y_confidence/number_normal_neighbours; node_data->angle_confidence_z=angle_z_confidence/number_normal_neighbours; } }//end (!empty_neighbours && number_neighbours>2) //this cell has neighbours so lets set data acording to them }//end (!node_data->has_normal && node_data->num_points>0) }//end for col }//end for row } void Navigability_Map::set_Cells_accessibility(void) { std::vector vector_node; grid_nodePtr node_data; grid_nodePtr aux_data; int empty_neighbours=0; int number_neighbours=0; double z_accessibility=0; double angleX_accessibility=0; double angleY_accessibility=0; double angleZ_accessibility=0; for(uint row=0;rownum_points>0 && node_data->has_normal) || (node_data->interpolate_z_data && node_data->interpolate_normal_data) || (node_data->num_points>0 && node_data->interpolate_normal_data)) { vector_node=cell_neighbours(row,col,empty_neighbours,number_neighbours); //acess the data in the cell neighbours for (uint i=0; i<8; i++) { aux_data=vector_node[i]; //Z difference node_data->z_difference_neighbours.push_back(abs(aux_data->Zmed-node_data->Zmed)); //Z confidence node_data->z_confidence_neighbours.push_back(sqrt(aux_data->Z_confidence*node_data->Z_confidence)); //angle difference node_data->angleX_difference_neighbours.push_back(abs(aux_data->med_angle_X-node_data->med_angle_X)); node_data->angleY_difference_neighbours.push_back(abs(aux_data->med_angle_Y-node_data->med_angle_Y)); node_data->angleZ_difference_neighbours.push_back(abs(aux_data->med_angle_Z-node_data->med_angle_Z)); //angle confidence node_data->angleX_confidence_neighbours.push_back(sqrt(aux_data->angle_confidence_x*node_data->angle_confidence_x)); node_data->angleY_confidence_neighbours.push_back(sqrt(aux_data->angle_confidence_y*node_data->angle_confidence_y)); node_data->angleZ_confidence_neighbours.push_back(sqrt(aux_data->angle_confidence_z*node_data->angle_confidence_z)); //only for cell fully data if ((aux_data->num_points>0 && aux_data->has_normal) || (aux_data->interpolate_z_data && aux_data->interpolate_normal_data) || (aux_data->num_points>0 && aux_data->interpolate_normal_data)) node_data->has_fulldata_direction.push_back(true); else node_data->has_fulldata_direction.push_back(false); } double z_difference_med_confidence_aux=0; double angleX_difference_med_confidence_aux=0; double angleY_difference_med_confidence_aux=0; double angleZ_difference_med_confidence_aux=0; for (uint i=0; i<8; i++) { if (node_data->has_fulldata_direction[i]) { //Z z_difference_med_confidence_aux=node_data->z_difference_neighbours[i]/node_data->z_confidence_neighbours[i]; if (z_difference_med_confidence_aux>fator_confidence_neighbour_limit*Zmax_heigh_difference) z_difference_med_confidence+=fator_confidence_neighbour_limit*Zmax_heigh_difference; else z_difference_med_confidence+=z_difference_med_confidence_aux; //angle angleX_difference_med_confidence_aux=node_data->angleX_difference_neighbours[i]/node_data->angleX_confidence_neighbours[i]; if (angleX_difference_med_confidence_aux>fator_confidence_neighbour_limit*angleX_max_difference) angleX_difference_med_confidence+=fator_confidence_neighbour_limit*angleX_max_difference; else angleX_difference_med_confidence+=angleX_difference_med_confidence_aux; angleY_difference_med_confidence_aux=node_data->angleY_difference_neighbours[i]/node_data->angleY_confidence_neighbours[i]; if (angleY_difference_med_confidence_aux>fator_confidence_neighbour_limit*angleY_max_difference) angleY_difference_med_confidence+=fator_confidence_neighbour_limit*angleY_max_difference; else angleY_difference_med_confidence+=angleY_difference_med_confidence_aux; angleZ_difference_med_confidence_aux=node_data->angleZ_difference_neighbours[i]/node_data->angleZ_confidence_neighbours[i]; if (angleZ_difference_med_confidence_aux>fator_confidence_neighbour_limit*angleZ_max_difference) angleZ_difference_med_confidence+=fator_confidence_neighbour_limit*angleZ_max_difference; else angleZ_difference_med_confidence+=angleZ_difference_med_confidence_aux; //neighbours neighbours_full_data++; } } // _______________________________________________________________________________ //| | //| calc z_accessibility , based on the med of the neighbours and confidence | //|_____________________________________________________________________________| if (neighbours_full_data>0) { z_difference_med_confidence/=neighbours_full_data; z_accessibility=1-(abs(z_difference_med_confidence)/Zmax_heigh_difference); if (z_accessibility>=0) node_data->z_accessibility=z_accessibility; else node_data->z_accessibility=0; // _______________________________________________________________________ //| | //| calc angle , based on the med of the neighbours and confidence | //|_______________________________________________________________________| angleX_difference_med_confidence/=neighbours_full_data; angleX_accessibility=1-(abs(angleX_difference_med_confidence)/angleX_max_difference); if (angleX_accessibility>=0) node_data->angleX_accessibility=angleX_accessibility; else node_data->angleX_accessibility=0; angleY_difference_med_confidence/=neighbours_full_data; angleY_accessibility=1-(abs(angleY_difference_med_confidence)/angleY_max_difference); if (angleY_accessibility>=0) node_data->angleY_accessibility=angleY_accessibility; else node_data->angleY_accessibility=0; angleZ_difference_med_confidence/=neighbours_full_data; angleZ_accessibility=1-(abs(angleZ_difference_med_confidence)/angleZ_max_difference); if (angleZ_accessibility>=0) node_data->angleZ_accessibility=angleZ_accessibility; else node_data->angleZ_accessibility=0; } // ______________________________ //| | //| total accessibility | //|____________________________| node_data->total_accessibility=node_data->z_accessibility*node_data->angleX_accessibility*node_data->angleY_accessibility*node_data->angleZ_accessibility; }//end (node_data->num_points>0 || (node_data->interpolate_z_data && node_data->interpolate_normal_data) }//end for col }//end for row } std::vector Navigability_Map::cell_neighbours(int row_pos, int col_pos, int &empty_neighbours, int &number_neighbours) { std::vector vector_node; grid_nodePtr node_data; bool empty_cell=true; int counter=0; //4 main directions int N_pos=row_pos+1; int S_pos=row_pos-1; int E_pos=col_pos+1; int W_pos=col_pos-1; int max_row_pos=grid.rows()-1; int max_col_pos=grid.cols()-1; // Acess N direction if (N_pos>max_row_pos) //outside the grid node_data.reset(new grid_node); //set a default node else node_data=grid(N_pos,col_pos); //node has full data if (node_data->num_points>0 && node_data->has_normal && !node_data->interpolate_z_data && !node_data->interpolate_normal_data) { empty_cell=false; counter++; } vector_node.push_back(node_data); // Acess NE direction if (N_pos>max_row_pos || E_pos>max_col_pos) //outside the grid node_data.reset(new grid_node); //set a default node else node_data=grid(N_pos,E_pos); if (node_data->num_points>0 && node_data->has_normal && !node_data->interpolate_z_data && !node_data->interpolate_normal_data) { empty_cell=false; counter++; } vector_node.push_back(node_data); // Acess E direction if (E_pos>max_col_pos) //outside the grid node_data.reset(new grid_node); //set a default node else node_data=grid(row_pos,E_pos); if (node_data->num_points>0 && node_data->has_normal && !node_data->interpolate_z_data && !node_data->interpolate_normal_data) { empty_cell=false; counter++; } vector_node.push_back(node_data); // Acess SE direction if (S_pos<0 || E_pos>max_col_pos) //outside the grid node_data.reset(new grid_node); //set a default node else node_data=grid(S_pos,E_pos); if (node_data->num_points>0 && node_data->has_normal && !node_data->interpolate_z_data && !node_data->interpolate_normal_data) { empty_cell=false; counter++; } vector_node.push_back(node_data); // Acess S direction if (S_pos<0) //outside the grid node_data.reset(new grid_node); //set a default node else node_data=grid(S_pos,col_pos); if (node_data->num_points>0 && node_data->has_normal && !node_data->interpolate_z_data && !node_data->interpolate_normal_data) { empty_cell=false; counter++; } vector_node.push_back(node_data); // Acess SW direction if (S_pos<0 || W_pos<0) //outside the grid node_data.reset(new grid_node); //set a default node else node_data=grid(S_pos,W_pos); if (node_data->num_points>0 && node_data->has_normal && !node_data->interpolate_z_data && !node_data->interpolate_normal_data) { empty_cell=false; counter++; } vector_node.push_back(node_data); // Acess W direction if (W_pos<0) //outside the grid node_data.reset(new grid_node); //set a default node else node_data=grid(row_pos,W_pos); if (node_data->num_points>0 && node_data->has_normal && !node_data->interpolate_z_data && !node_data->interpolate_normal_data) { empty_cell=false; counter++; } vector_node.push_back(node_data); // Acess NW direction if (N_pos>max_row_pos || W_pos<0) //outside the grid node_data.reset(new grid_node); //set a default node else node_data=grid(N_pos,W_pos); if (node_data->num_points>0 && node_data->has_normal && !node_data->interpolate_z_data && !node_data->interpolate_normal_data) { empty_cell=false; counter++; } vector_node.push_back(node_data); //return if (empty_cell) empty_neighbours=1; number_neighbours=counter; return vector_node; } void Navigability_Map::polygon_groundtruth(void) { geometry_msgs::Point p; // //________________________ // // | // // 6 cloud ground truth | // //________________________| // /* p.x=4.4; p.y=-2.3; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=4.4; p.y=3.5; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //junto ao passeio p.x=7; p.y=7.6; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=19; p.y=7.8; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=19; p.y=-1.5; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //descer ao pe do carro da direita p.x=13.5; p.y=-1.5; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //zona entre os carros p.x=13.5; p.y=-5.3; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //zona entre os carros p.x=8; p.y=-3; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //contornar buraco p.x=8.55; p.y=-1.4; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=8.55; p.y=-0.25; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=7.4; p.y=-0.25; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=7.4; p.y=-1.4; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=8.5; p.y=-1.4; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=8.05; p.y=-2.7; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=4.4; p.y=-2.3; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //obstaculo p.x=7.8; p.y=-1.35; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=8.5; p.y=-1.35; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=8.5; p.y=-0.35; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=7.5; p.y=-0.35; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=7.5; p.y=-1.35; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=7.8; p.y=-1.35; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); */ // //________________________ // // | // // 9 cloud ground truth | // //________________________| /* p.x=4.4; p.y=-2.1; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=4.4; p.y=2; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=6; p.y=6.2; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=7.2; p.y=4.5; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=8; p.y=5.7; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=9.5; p.y=4; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=9; p.y=3; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=18; p.y=-0.8; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); // ir para o lado direito p.x=15.5; p.y=-5.75; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); // descer ponte p.x=6.8; p.y=-2.4; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=6.3; p.y=-3.3; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=4.8; p.y=-2.8; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=4.4; p.y=-2.1; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); // obstáculo p.x=15.5; p.y=-5.8; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); // descer ponte p.x=7; p.y=-2.5; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=6.8; p.y=-3.05; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=15.2; p.y=-6.45; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=15.5; p.y=-5.8; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); */ //________________________ // | // 11 cloud ground truth | //________________________| /* p.x=4.6; p.y=-1.5; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=4.6; p.y=1.25; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=12; p.y=3.9; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //ponto ao pe da descida p.x=14.5; p.y=2.2; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //descer um pouco p.x=9.3; p.y=0.2; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //contornar devisoria p.x=9.8; p.y=-1; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=18; p.y=-1; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=18; p.y=-4; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=16; p.y=-8; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=13.5; p.y=-8; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=11.5; p.y=-4; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=10; p.y=-4.5; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=6.8; p.y=-6.5; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=4.6; p.y=-1.5; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //obstaculo p.x=11.6; p.y=-4.3; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=10.3; p.y=-4.7; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=11.1; p.y=-6.5; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=12.5; p.y=-6.2; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=11.6; p.y=-4.3; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); */ //________________________ // | // 14 cloud ground truth | //________________________| p.x=4.4; p.y=-1; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=4.4; p.y=4.8; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=9; p.y=5.3; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //iniciar subida p.x=11; p.y=-2.3; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //contornar poste p.x=8.05; p.y=-2.5; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=8.05; p.y=-2.95; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=8.45; p.y=-2.95; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=8.45; p.y=-2.55; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=11; p.y=-2.35; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //voltar a subir p.x=11.5; p.y=-4.8; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=10.8; p.y=-4.9; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=6.3; p.y=-4.8; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=4.5; p.y=-2.8; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); p.x=4.4; p.y=-1; p.z=1.2; polygon_points.push_back(p); xy_polygon.push_back(Point2f(p.x,p.y)); //obstáculo p.x=8.05; p.y=-2.5; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=8.05; p.y=-2.95; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=8.45; p.y=-2.95; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=8.45; p.y=-2.5; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); p.x=8.05; p.y=-2.5; p.z=1.2; obstacle_points.push_back(p); obstacle_polygon.push_back(Point2f(p.x,p.y)); } void Navigability_Map::getCell_inpolygon(void) { grid_nodePtr node_data; double x_cell=0; double y_cell=0; for(uint row=0;rownum_points>0 && node_data->has_normal) || (node_data->interpolate_z_data && node_data->interpolate_normal_data) || (node_data->num_points>0 && node_data->interpolate_normal_data)) { //X posistion x_cell = row*Sx+Sx/2; //Y position y_cell = col*Sy-CARaxis_col*Sy; //_________________ // | // polygon test | //________________| // create a pointer to the data as an array of points (via a conversion to // a Mat() object) Point2d test_pt; test_pt.x = x_cell; test_pt.y = y_cell; if (pointPolygonTest(Mat(xy_polygon), test_pt, false) > 0) //false, dont calc dist ; { //Cell is in polygon, save row and col Matrix_Cell_inpolygon_rowcol.conservativeResize(Matrix_Cell_inpolygon_rowcol.rows()+1,2); Matrix_Cell_inpolygon_rowcol(Matrix_Cell_inpolygon_rowcol.rows()-1,0)=row; Matrix_Cell_inpolygon_rowcol(Matrix_Cell_inpolygon_rowcol.rows()-1,1)=col; } if (pointPolygonTest(Mat(obstacle_polygon), test_pt, false) > 0) //false, dont calc dist ; { //Cell is in polygon, save row and col Matrix_obstacle_inpolygon_rowcol.conservativeResize(Matrix_obstacle_inpolygon_rowcol.rows()+1,2); Matrix_obstacle_inpolygon_rowcol(Matrix_obstacle_inpolygon_rowcol.rows()-1,0)=row; Matrix_obstacle_inpolygon_rowcol(Matrix_obstacle_inpolygon_rowcol.rows()-1,1)=col; } } } } } void Navigability_Map::dataCell_inpolygon(void) { grid_nodePtr node_data; int row_poly=0; int col_poly=0; int row_obs=0; int col_obs=0; int total_cells_poly=Matrix_Cell_inpolygon_rowcol.rows(); int cell_goodacc_poly=0; int total_cells_obs=Matrix_obstacle_inpolygon_rowcol.rows(); int cell_badacc_obs=0; //acess data in the cell for (int i=0; itotal_accessibility>0.25) cell_goodacc_poly++; } //acess data in the cell for (int i=0; itotal_accessibility<=0.25) cell_badacc_obs++; } double algorithm_efficiency=0; algorithm_efficiency=((double)cell_goodacc_poly/(double)total_cells_poly)*100; std::cout<<"total cells in polygon: "<0.25: "<