void PbMapMaker::mergePlanes(Plane &updatePlane, Plane &discardPlane) { // Update normal and center updatePlane.v3normal = updatePlane.areaVoxels*updatePlane.v3normal + discardPlane.areaVoxels*discardPlane.v3normal; updatePlane.v3normal = updatePlane.v3normal / norm(updatePlane.v3normal); // Update point inliers // *updatePlane.polygonContourPtr += *discardPlane.polygonContourPtr; // Merge polygon points *updatePlane.planePointCloudPtr += *discardPlane.planePointCloudPtr; // Add the points of the new detection and perform a voxel grid // Filter the points of the patch with a voxel-grid. This points are used only for visualization static pcl::VoxelGrid<pcl::PointXYZRGBA> merge_grid; merge_grid.setLeafSize(0.05,0.05,0.05); pcl::PointCloud<pcl::PointXYZRGBA> mergeCloud; merge_grid.setInputCloud (updatePlane.planePointCloudPtr); merge_grid.filter (mergeCloud); updatePlane.planePointCloudPtr->clear(); *updatePlane.planePointCloudPtr = mergeCloud; // if(configPbMap.use_color) // updatePlane.calcMainColor(); *discardPlane.polygonContourPtr += *updatePlane.planePointCloudPtr; updatePlane.calcConvexHull(discardPlane.polygonContourPtr); updatePlane.computeMassCenterAndArea(); // Move the points to fulfill the plane equation updatePlane.forcePtsLayOnPlane(); // Update area double area_recalc = updatePlane.planePointCloudPtr->size() * 0.0025; mpPlaneInferInfo->isFullExtent(updatePlane, area_recalc); updatePlane.areaVoxels= updatePlane.planePointCloudPtr->size() * 0.0025; }
void mario::filterA( const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & cloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr & dst) { static double OUT_FILTER_LOWER = FileIO::getConst("OUT_FILTER_LOWER"); static double OUT_FILTER_UPPER = FileIO::getConst("OUT_FILTER_UPPER"); static double DOWN_FILTER_LEAF = FileIO::getConst("DOWN_FILTER_LEAF"); // 大きいほど除去する static int const filterNum = 3; static pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudPtrs[filterNum+1]; // フィルタリング if( cloud ){ static pcl::PassThrough<pcl::PointXYZRGBA> pass; // 外れ値除去フィルタ static pcl::VoxelGrid< pcl::PointXYZRGBA > sor; // ダウンサンプリングフィルタ static bool isInitDone = false; if( isInitDone == false ){ // 外れ値除去フィルタの設定 pass.setFilterFieldName ("z"); pass.setFilterLimits (OUT_FILTER_LOWER, OUT_FILTER_UPPER); // ダウンサンプリングフィルタの設定 if( DOWN_FILTER_LEAF > 0 ){ sor.setLeafSize (DOWN_FILTER_LEAF,DOWN_FILTER_LEAF, DOWN_FILTER_LEAF); } for( int i=0; i<filterNum; i++ ){ cloudPtrs[i] = ( pcl::PointCloud<pcl::PointXYZRGBA>::Ptr )(new pcl::PointCloud<pcl::PointXYZRGBA>); } isInitDone = true; } int filterCount = 0; // はずれ値除去フィルタ if( OUT_FILTER_LOWER > 0 && OUT_FILTER_UPPER > 0 ){ pass.setInputCloud ( cloud ); ///pass.setFilterLimitsNegative (true); filterCount++; pass.filter ( *cloudPtrs[filterCount] ); } // ダウンサンプリングフィルタ if( DOWN_FILTER_LEAF > 0 ){ if( filterCount > 0 ){ sor.setInputCloud ( cloudPtrs[filterCount] ); }else{ sor.setInputCloud ( cloud ); } filterCount++; sor.filter ( *cloudPtrs[filterCount] ); } // 平面抽出 //auto inliers = segmentate( cloud_down_filtered, 0.001 ); //大きいほどアバウトに検出 //auto cloud_plane_filtered = filter( cloud_down_filtered, inliers, true ); //inliers = segmentate( cloud_plane_filtered, 0.001 ); //大きいほどアバウトに検出 // 格納されている順番に赤く着色 //redIteration( *cloud_down_filtered ); // 赤色を検出して緑色に変換 //redDetection( *cloudPtrs[filterCount] ); dst = cloudPtrs[filterCount]->makeShared(); } }
int main(int argc, char** argv) { ros::init(argc, argv, "downsample"); ros::NodeHandle n; downsample_pub = n.advertise<sensor_msgs::PointCloud2>("/downsample", 10); depth_sub = n.subscribe("/camera/depth/points", 10, depthPointsCallback); ros::Rate loop_rate(10); // Set variables for filters vox.setLeafSize (0.01, 0.01, 0.01); sor.setMeanK (10); sor.setStddevMulThresh (0.1); ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (20); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); //seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.02); while(ros::ok()) { ros::spin(); } return 0; }
PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera ){ PointCloud::Ptr newCloud = image2PointCloud(newFrame.rgb,newFrame.depth,camera); // 合并点云 PointCloud::Ptr output(new PointCloud()); pcl::transformPointCloud(*original,*output,T.matrix()); *newCloud += *output; // Voxel grid 滤波降采样 static pcl::VoxelGrid<PointT> voxel; static ParameterReader pd; double gridsize = atof(pd.getData("voxel_grid").c_str()); voxel.setLeafSize(gridsize,gridsize,gridsize); voxel.setInputCloud(newCloud); PointCloud::Ptr tmp(new PointCloud()); voxel.filter(*tmp); return tmp; }
int main(int argc, char** argv) { ros::init(argc, argv, "global_matching"); ros::NodeHandle nh; ros::Subscriber cloud_sub = nh.subscribe("/mapping/glocCloud", 1, laser_callback); pub_map=nh.advertise<sensor_msgs::PointCloud2>("/mapping/gloc/map",1); pub_guess=nh.advertise<sensor_msgs::PointCloud2>("/mapping/gloc/guess",1); pub_match=nh.advertise<sensor_msgs::PointCloud2>("/mapping/gloc/match",1); edgePub = nh.advertise<graph_slam::Edge> ("mapping/graph_slam/edges", 100); //setup gicp setup_gicp(); char buffer[10000]; realpath(argv[0], buffer); std::string fullpath = buffer; fullpath = std::string(fullpath, 0, fullpath.rfind("bin/")); fullpath.append(MAP_PATH); //read map cloud if (pcl::io::loadPCDFile<pcl::PointXYZ> (fullpath, *cloud_map) == -1) // load the file { ROS_ERROR_STREAM("No map cloud found"); return (-1); } ROS_INFO_STREAM("Starting global matching...."); sor.setLeafSize (LEAF_SIZE,LEAF_SIZE,LEAF_SIZE); sor.setInputCloud(cloud_map); sor.filter(*cloud_map_filt); ros::Rate r(10); while(ros::ok() && !stop_matching) { ros::spinOnce(); r.sleep(); } ROS_INFO_STREAM("Global Matching Stopped"); return 0; }
void initialize(bool toFilter) { // A passthrough filter to remove spurious NaNs pass.setInputCloud(cloud); pass.filter(*cloud_filtered); std::cout << "PointCloud after pass through has: " << cloud_filtered->points.size() << " data points." << std::endl; // Downsample the dataset using a leaf size of 1cm // After filtering the point cloud, all indices do not point to the // original points. Therefore disable this if called from initializeFromPointCLoud if (toFilter) { vg.setInputCloud(cloud_filtered); vg.filter(*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; } // Estimate point normals ne.setInputCloud(cloud_filtered); ne.compute(*cloud_normals); ROS_INFO("%lu normals estimated", cloud_normals->points.size()); }
void initialize(const std::string & pcd_name) { // Read in the cloud data reader.read(pcd_name, *cloud); std::cerr << "PointCloud has: " << cloud->points.size() << " data points." << std::endl; // pcl::io::loadPCDFile(pcd_name, *cloud); // A passthrough filter to remove spurious NaNs pass.setInputCloud(cloud); pass.filter(*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; // Downsample the dataset using a leaf size of 1cm vg.setInputCloud(cloud_filtered); vg.filter(*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; // Estimate point normals ne.setInputCloud(cloud_filtered); ne.compute(*cloud_normals); ROS_INFO("%lu normals estimated", cloud_normals->points.size()); }
Extractor() { tree.reset(new pcl::KdTreeFLANN<Point > ()); cloud.reset(new pcl::PointCloud<Point>); cloud_filtered.reset(new pcl::PointCloud<Point>); cloud_normals.reset(new pcl::PointCloud<pcl::Normal>); coefficients_plane_.reset(new pcl::ModelCoefficients); coefficients_cylinder_.reset(new pcl::ModelCoefficients); inliers_plane_.reset(new pcl::PointIndices); inliers_cylinder_.reset(new pcl::PointIndices); // Filter Pass pass.setFilterFieldName("z"); pass.setFilterLimits(-100, 100); // VoxelGrid for Downsampling LEAFSIZE = 0.01f; vg.setLeafSize(LEAFSIZE, LEAFSIZE, LEAFSIZE); // Any object < CUT_THRESHOLD will be abandoned. //CUT_THRESHOLD = (int) (LEAFSIZE * LEAFSIZE * 7000000); // 700 CUT_THRESHOLD = 700; //for nonfiltering // Clustering cluster_.setClusterTolerance(0.06 * UNIT); cluster_.setMinClusterSize(50.0); cluster_.setSearchMethod(clusters_tree_); // Normals ne.setSearchMethod(tree); ne.setKSearch(50); // 50 by default // plane SAC seg_plane.setOptimizeCoefficients(true); seg_plane.setModelType(pcl::SACMODEL_NORMAL_PLANE); seg_plane.setNormalDistanceWeight(0.1); // 0.1 seg_plane.setMethodType(pcl::SAC_RANSAC); seg_plane.setMaxIterations(1000); // 10000 seg_plane.setDistanceThreshold(0.05); // 0.03 // cylinder SAC seg_cylinder.setOptimizeCoefficients(true); seg_cylinder.setModelType(pcl::SACMODEL_CYLINDER); seg_cylinder.setMethodType(pcl::SAC_RANSAC); seg_cylinder.setNormalDistanceWeight(0.1); seg_cylinder.setMaxIterations(10000); seg_cylinder.setDistanceThreshold(0.02); // 0.05 seg_cylinder.setRadiusLimits(0.02, 0.07); // [0, 0.1] }
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) { double tempTime = ros::Time::now().toSec(); if(input->width < 30000) { ROS_ERROR_STREAM("Velodyne scan incomplete. Cloud points: "<< input->width); return; } //ROS_INFO("got the cloud"); //init the bin grid for(int i=0;i<NUMABINS*NUMLBINS;i++) { ptBins[i].clear(); } //clear all clouds ptBins.clear(); input_cloud->clear(); obs_cloud->clear(); drv_cloud->clear(); ground_cloud->clear(); cloud_temp->clear(); gCloudTemp->clear(); pcl::fromROSMsg (*input, *input_cloud); copyPointCloud(*input_cloud, *proc_cloud); compRollPitchCloud(proc_cloud,curRoll, curPitch); //roll/pitch compensation segment_ground(proc_cloud,obs_cloud,drv_cloud); //perform ground segmentation //ground cloud if(obs_cloud->size() == 0) { ROS_WARN_STREAM("No obsticale points!"); return; } sor.setLeafSize(VOXSIZE_XY,VOXSIZE_XY,VOXSIZE_Z); sor.setInputCloud(ground_cloud); sor.filter(*gCloudTemp); pcl::toROSMsg(*gCloudTemp,output_msg); output_msg.header.frame_id = "/nasa"; output_msg.header.stamp = input->header.stamp; pubGnd.publish(output_msg); //obsticale cloud sor.setInputCloud(obs_cloud); sor.filter(*gCloudTemp); pcl::toROSMsg(*gCloudTemp,output_msg); output_msg.header.frame_id = "/nasa"; output_msg.header.stamp = input->header.stamp; pub.publish (output_msg); //drv cloud sor.setLeafSize(0.1, 0.1, 0.1); sor.setInputCloud(drv_cloud); sor.filter(*gCloudTemp); pcl::toROSMsg(*gCloudTemp,output_msg); // pcl::toROSMsg(*drv_cloud,output_msg); output_msg.header.frame_id = "/nasa"; output_msg.header.stamp = input->header.stamp; pubDrv.publish (output_msg); timeOut << ros::Time::now().toSec() - tempTime << endl; }
void depthPointsCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { // Instantiate various clouds pcl::PCLPointCloud2* cloud_intermediate = new pcl::PCLPointCloud2; pcl::PCLPointCloud2ConstPtr cloudPtr(cloud_intermediate); pcl::PointCloud<pcl::PointXYZ> cloud; // Convert to PCL data type pcl_conversions::toPCL(*cloud_msg, *cloud_intermediate); // Apply Voxel Filter on PCLPointCloud2 vox.setInputCloud (cloudPtr); vox.setInputCloud (cloudPtr); vox.filter (*cloud_intermediate); // Convert PCL::PointCloud2 to PCL::PointCloud<PointXYZ> pcl::fromPCLPointCloud2(*cloud_intermediate, cloud); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p = cloud.makeShared(); // Apply Passthrough Filter pass.setFilterFieldName ("z"); pass.setFilterLimits (0.3, 1); pass.setInputCloud (cloud_p); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_p); // Apply Passthrough Filter pass.setFilterFieldName ("x"); pass.setFilterLimits (-0.2, 0.2); pass.setInputCloud (cloud_p); pass.setFilterFieldName ("z"); pass.setFilterLimits (0.0, 3.0); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_p); // Apply Statistical Noise Filter sor.setInputCloud (cloud_p); sor.filter (*cloud_p); // Planar segmentation: Remove large planes? Or extract floor? pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers (new pcl::PointIndices); int nr_points = (int) cloud_p->points.size (); Eigen::Vector3f lol (0, 1, 0); seg.setEpsAngle( 30.0f * (3.14f/180.0f) ); seg.setAxis(lol); //while(cloud_p->points.size () > 0.2 * nr_points) { sor.setInputCloud (cloud_p); sor.filter (*cloud_p); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.01); seg.setInputCloud (cloud_p); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { //break; } else { /*std::cout << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << "\n";*/ pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud_p); extract.setIndices(inliers); extract.setNegative(true); extract.filter(*cloud_p); } //} Eigen::Vector3f lol_p (0.5f, 0, 0.5f); seg.setAxis(lol_p); while(cloud_p->points.size () > 0.1 * nr_points) { seg.setInputCloud (cloud_p); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { break; } else { /*std::cout << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << "\n";*/ pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud_p); extract.setIndices(inliers); extract.setNegative(true); extract.filter(*cloud_p); } } // Apply Statistical Noise Filter sor.setInputCloud (cloud_p); sor.filter (*cloud_p); if(cloud_p->points.size() > 0) { std::vector<pcl::PointIndices> cluster_indices; tree->setInputCloud (cloud_p); ec.setInputCloud (cloud_p); ec.extract (cluster_indices); std::cout << "Clusters detected: " << cluster_indices.size() << "\n"; // Convert to ROS data type } // Convert to ROS data type sensor_msgs::PointCloud2 output; pcl::toROSMsg(*cloud_p, output); // Publish the data downsample_pub.publish(output); }
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); }
void laser_callback(const global_matching::GlocCloud& glocCloud) { //TODO: Map needs to be centered around with starting point at (0,0) #ifdef ONLY_ONCE if(glocCloud.id > 1) { stop_matching = true; return; } #endif if(glocCloud.id <= node_index) { ROS_DEBUG_STREAM("Skipping node: " << glocCloud.id); return; } ROS_DEBUG_STREAM("Processing node: " << glocCloud.id); bool match_found = false; pcl::fromROSMsg(glocCloud.cloud, *cloud_in); #ifdef VOXEL_FILTER sor.setInputCloud (cloud_in); sor.filter(*cloud_in_filt); #else cloud_in_filt = cloud_in; cloud_map_filt = cloud_map; #endif float cur_x = glocCloud.pose.pose.position.x; float cur_y = glocCloud.pose.pose.position.y; float cur_yaw = tf::getYaw(glocCloud.pose.pose.orientation); //LOOP FOR RANDOM GUESSES for(int i = 0; i < ITERATIONS; i++) { //ROS_DEBUG_STREAM("Itertation " << i); //Give random initial conditions float dx,dy,dth; if(i == 0) { //try current pose first dx = cur_x; dy = cur_y; dth = cur_yaw; } else { dx = (float)rand()/RAND_MAX*2*GUESS_DIST_RNG-GUESS_DIST_RNG + cur_x; dy = (float)rand()/RAND_MAX*2*GUESS_DIST_RNG-GUESS_DIST_RNG + cur_y; dth = (float)rand()/RAND_MAX*GUESS_ANGLE_RNG; } trans_init << cos(dth) , -sin(dth), 0, dx, sin(dth), cos(dth), 0, dy, 0, 0, 1, 0, 0, 0, 0, 1; pcl::transformPointCloud(*cloud_in_filt, *cloud_in_trans, trans_init); //Calculate gicp gicp.setInputCloud(cloud_in_trans); gicp.setInputTarget(cloud_map_filt); gicp.align(*cloud_final); float score = gicp.getFitnessScore(); float normScore = score / cloud_in_trans->size(); ROS_DEBUG_STREAM(" Score: " << score << " : " << normScore); transformation = gicp.getFinalTransformation(); trans_init = transformation*trans_init; pcl::transformPointCloud(*cloud_in, *cloud_final, trans_init); pcl::toROSMsg(*cloud_map_filt,ros_map); //pcl::toROSMsg(*cloud_in_trans,ros_guess); pcl::toROSMsg(*cloud_final,ros_match); ros_map.header.frame_id = "/nasa"; ros_guess.header.frame_id = "/nasa"; ros_match.header.frame_id = "/nasa"; pub_map.publish(ros_map); //pub_guess.publish(ros_guess); pub_match.publish(ros_match); if(normScore < MAX_SCORE) { ROS_INFO_STREAM("Global Match successful"); ROS_DEBUG_STREAM("initial: " << trans_init); //refine match gicp.setInputCloud(cloud_final); gicp.setInputTarget(cloud_map); gicp.align(*cloud_refined); transformation = gicp.getFinalTransformation(); trans_init = transformation*trans_init; ROS_DEBUG_STREAM("refined: " << trans_init); pcl::transformPointCloud(*cloud_in, *cloud_refined, trans_init); pcl::toROSMsg(*cloud_refined,ros_guess); pub_guess.publish(ros_guess); match_found = true; break; } }//END RANDOM GUESSES if(match_found) { //publish edge try_count = 0; double yaw,pitch,roll; graph_slam::Edge gEdge; gEdge.delta.x = trans_init(0,3); gEdge.delta.y = trans_init(1,3); tf::Matrix3x3 mat; mat[0][0] = trans_init(0,0); mat[0][1] = trans_init(0,1); mat[0][2] = trans_init(0,2); mat[1][0] = trans_init(1,0); mat[1][1] = trans_init(0,1); mat[1][2] = trans_init(1,2); mat[2][0] = trans_init(2,0); mat[2][1] = trans_init(0,1); mat[2][2] = trans_init(2,2); mat.getEulerYPR(yaw, pitch, roll,1); gEdge.delta.theta = yaw; gEdge.to = glocCloud.id; gEdge.from = 0; gEdge.covariance[0] = COV_XY; gEdge.covariance[1] = 0; gEdge.covariance[2] = 0; gEdge.covariance[3] = 0; gEdge.covariance[4] = COV_XY; gEdge.covariance[5] = 0; gEdge.covariance[6] = 0; gEdge.covariance[7] = 0; gEdge.covariance[8] = COV_YAW; edgePub.publish(gEdge); node_index = glocCloud.id; } else { try_count++; ROS_DEBUG_STREAM("No match found :("); } if(try_count >= MAX_TRIES) { //matching failed return initial guess ROS_WARN_STREAM(""); graph_slam::Edge gEdge; gEdge.delta.x = glocCloud.pose.pose.position.x; gEdge.delta.y = glocCloud.pose.pose.position.y; gEdge.delta.theta = tf::getYaw(glocCloud.pose.pose.orientation); gEdge.to = glocCloud.id; gEdge.from = 0; gEdge.covariance[0] = COV_XY; gEdge.covariance[1] = 0; gEdge.covariance[2] = 0; gEdge.covariance[3] = 0; gEdge.covariance[4] = COV_XY; gEdge.covariance[5] = 0; gEdge.covariance[6] = 0; gEdge.covariance[7] = 0; gEdge.covariance[8] = COV_YAW; edgePub.publish(gEdge); node_index = glocCloud.id; } }
void PbMapMaker::detectPlanesCloud( pcl::PointCloud<PointT>::Ptr &pointCloudPtr_arg, Eigen::Matrix4f &poseKF, double distThreshold, double angleThreshold, double minInliersF) { unsigned minInliers = minInliersF * pointCloudPtr_arg->size(); #ifdef _VERBOSE cout << "detectPlanes in a cloud with " << pointCloudPtr_arg->size() << " points " << minInliers << " minInliers\n"; #endif pcl::PointCloud<PointT>::Ptr pointCloudPtr_arg2(new pcl::PointCloud<PointT>); pcl::copyPointCloud(*pointCloudPtr_arg,*pointCloudPtr_arg2); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr alignedCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::transformPointCloud(*pointCloudPtr_arg,*alignedCloudPtr,poseKF); { mrpt::synch::CCriticalSectionLocker csl(&CS_visualize); *mPbMap.globalMapPtr += *alignedCloudPtr; } // End CS // Downsample voxel map's point cloud static pcl::VoxelGrid<pcl::PointXYZRGBA> grid; grid.setLeafSize(0.02,0.02,0.02); pcl::PointCloud<pcl::PointXYZRGBA> globalMap; grid.setInputCloud (mPbMap.globalMapPtr); grid.filter (globalMap); mPbMap.globalMapPtr->clear(); *mPbMap.globalMapPtr = globalMap; pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setMaxDepthChangeFactor (0.02f); // For VGA: 0.02f, 10.0f ne.setNormalSmoothingSize (5.0f); ne.setDepthDependentSmoothing (true); pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers (minInliers); mps.setAngularThreshold (angleThreshold); // (0.017453 * 2.0) // 3 degrees mps.setDistanceThreshold (distThreshold); //2cm pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>); ne.setInputCloud (pointCloudPtr_arg2); ne.compute (*normal_cloud); #ifdef _VERBOSE double plane_extract_start = pcl::getTime (); #endif mps.setInputNormals (normal_cloud); mps.setInputCloud (pointCloudPtr_arg2); std::vector<pcl::PlanarRegion<PointT>, aligned_allocator<pcl::PlanarRegion<PointT> > > regions; std::vector<pcl::ModelCoefficients> model_coefficients; std::vector<pcl::PointIndices> inlier_indices; pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>); std::vector<pcl::PointIndices> label_indices; std::vector<pcl::PointIndices> boundary_indices; mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices); #ifdef _VERBOSE double plane_extract_end = pcl::getTime(); std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << std::endl; // std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl; cout << regions.size() << " planes detected\n"; #endif // Create a vector with the planes detected in this keyframe, and calculate their parameters (normal, center, pointclouds, etc.) // in the global reference vector<Plane> detectedPlanes; for (size_t i = 0; i < regions.size (); i++) { Plane plane; Vector3f centroid = regions[i].getCentroid (); plane.v3center = compose(poseKF, centroid); plane.v3normal = poseKF.block(0,0,3,3) * Vector3f(model_coefficients[i].values[0], model_coefficients[i].values[1], model_coefficients[i].values[2]); // assert(plane.v3normal*plane.v3center.transpose() <= 0); // if(plane.v3normal*plane.v3center.transpose() <= 0) // plane.v3normal *= -1; // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZRGBA> extract; extract.setInputCloud (pointCloudPtr_arg2); extract.setIndices ( boost::make_shared<const pcl::PointIndices> (inlier_indices[i]) ); extract.setNegative (false); extract.filter (*plane.planePointCloudPtr); // Write the planar point cloud static pcl::VoxelGrid<pcl::PointXYZRGBA> plane_grid; plane_grid.setLeafSize(0.05,0.05,0.05); pcl::PointCloud<pcl::PointXYZRGBA> planeCloud; plane_grid.setInputCloud (plane.planePointCloudPtr); plane_grid.filter (planeCloud); plane.planePointCloudPtr->clear(); pcl::transformPointCloud(planeCloud,*plane.planePointCloudPtr,poseKF); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr contourPtr(new pcl::PointCloud<pcl::PointXYZRGBA>); contourPtr->points = regions[i].getContour(); // plane.contourPtr->points = regions[i].getContour(); // pcl::transformPointCloud(*plane.contourPtr,*plane.polygonContourPtr,poseKF); pcl::transformPointCloud(*contourPtr,*plane.polygonContourPtr,poseKF); plane_grid.setInputCloud (plane.polygonContourPtr); // plane_grid.filter (*plane.contourPtr); // plane.calcConvexHull(plane.contourPtr); plane_grid.filter (*contourPtr); plane.calcConvexHull(contourPtr); plane.computeMassCenterAndArea(); plane.areaVoxels= plane.planePointCloudPtr->size() * 0.0025; #ifdef _VERBOSE cout << "Area plane region " << plane.areaVoxels<< " of Chull " << plane.areaHull << " of polygon " << plane.compute2DPolygonalArea() << endl; #endif // Check whether this region correspond to the same plane as a previous one (this situation may happen when there exists a small discontinuity in the observation) bool isSamePlane = false; for (size_t j = 0; j < detectedPlanes.size(); j++) if( areSamePlane(detectedPlanes[j], plane, configPbMap.max_cos_normal, configPbMap.max_dist_center_plane, configPbMap.proximity_threshold) ) // The planes are merged if they are the same { isSamePlane = true; mergePlanes(detectedPlanes[j], plane); #ifdef _VERBOSE cout << "\tTwo regions support the same plane in the same KeyFrame\n"; #endif break; } if(!isSamePlane) detectedPlanes.push_back(plane); } #ifdef _VERBOSE cout << detectedPlanes.size () << " Planes detected\n"; #endif // Merge detected planes with previous ones if they are the same size_t numPrevPlanes = mPbMap.vPlanes.size(); // set<unsigned> observedPlanes; observedPlanes.clear(); for (size_t i = 0; i < detectedPlanes.size (); i++) { // Check similarity with previous planes detected bool isSamePlane = false; vector<Plane>::iterator itPlane = mPbMap.vPlanes.begin(); // if(frameQueue.size() != 12) for(size_t j = 0; j < numPrevPlanes; j++, itPlane++) // numPrevPlanes { if( areSamePlane(mPbMap.vPlanes[j], detectedPlanes[i], configPbMap.max_cos_normal, configPbMap.max_dist_center_plane, configPbMap.proximity_threshold) ) // The planes are merged if they are the same { // if (j==2 && frameQueue.size() == 12) // { // cout << "Same plane\n"; // //// ofstream pbm; //// pbm.open("comparePlanes.txt"); // { // cout << " ID " << mPbMap.vPlanes[j].id << " obs " << mPbMap.vPlanes[j].numObservations; // cout << " areaVoxels " << mPbMap.vPlanes[j].areaVoxels << " areaVoxels " << mPbMap.vPlanes[j].areaHull; // cout << " ratioXY " << mPbMap.vPlanes[j].elongation << " structure " << mPbMap.vPlanes[j].bFromStructure << " label " << mPbMap.vPlanes[j].label; // cout << "\n normal\n" << mPbMap.vPlanes[j].v3normal << "\n center\n" << mPbMap.vPlanes[j].v3center; // cout << "\n PpalComp\n" << mPbMap.vPlanes[j].v3PpalDir << "\n RGB\n" << mPbMap.vPlanes[j].v3colorNrgb; // cout << "\n Neighbors (" << mPbMap.vPlanes[j].neighborPlanes.size() << "): "; // for(map<unsigned,unsigned>::iterator it=mPbMap.vPlanes[j].neighborPlanes.begin(); it != mPbMap.vPlanes[j].neighborPlanes.end(); it++) // cout << it->first << " "; // cout << "\n CommonObservations: "; // for(map<unsigned,unsigned>::iterator it=mPbMap.vPlanes[j].neighborPlanes.begin(); it != mPbMap.vPlanes[j].neighborPlanes.end(); it++) // cout << it->second << " "; // cout << "\n ConvexHull (" << mPbMap.vPlanes[j].polygonContourPtr->size() << "): \n"; // for(unsigned jj=0; jj < mPbMap.vPlanes[j].polygonContourPtr->size(); jj++) // cout << "\t" << mPbMap.vPlanes[j].polygonContourPtr->points[jj].x << " " << mPbMap.vPlanes[j].polygonContourPtr->points[jj].y << " " << mPbMap.vPlanes[j].polygonContourPtr->points[jj].z << endl; // cout << endl; // } // { //// cout << " ID " << detectedPlanes[i].id << " obs " << detectedPlanes[i].numObservations; //// cout << " areaVoxels " << detectedPlanes[i].areaVoxels << " areaVoxels " << detectedPlanes[i].areaHull; //// cout << " ratioXY " << detectedPlanes[i].elongation << " structure " << detectedPlanes[i].bFromStructure << " label " << detectedPlanes[i].label; // cout << "\n normal\n" << detectedPlanes[i].v3normal << "\n center\n" << detectedPlanes[i].v3center; //// cout << "\n PpalComp\n" << detectedPlanes[i].v3PpalDir << "\n RGB\n" << detectedPlanes[i].v3colorNrgb; //// cout << "\n Neighbors (" << detectedPlanes[i].neighborPlanes.size() << "): "; //// for(map<unsigned,unsigned>::iterator it=detectedPlanes[i].neighborPlanes.begin(); it != detectedPlanes[i].neighborPlanes.end(); it++) //// cout << it->first << " "; //// cout << "\n CommonObservations: "; //// for(map<unsigned,unsigned>::iterator it=detectedPlanes[i].neighborPlanes.begin(); it != detectedPlanes[i].neighborPlanes.end(); it++) //// cout << it->second << " "; // cout << "\n ConvexHull (" << detectedPlanes[i].polygonContourPtr->size() << "): \n"; // for(unsigned jj=0; jj < detectedPlanes[i].polygonContourPtr->size(); jj++) // cout << "\t" << detectedPlanes[i].polygonContourPtr->points[jj].x << " " << detectedPlanes[i].polygonContourPtr->points[jj].y << " " << detectedPlanes[i].polygonContourPtr->points[jj].z << endl; // cout << endl; // } //// pbm.close(); // } isSamePlane = true; mergePlanes(mPbMap.vPlanes[j], detectedPlanes[i]); // Update proximity graph checkProximity(mPbMap.vPlanes[j], configPbMap.proximity_neighbor_planes); // Detect neighbors #ifdef _VERBOSE cout << "Previous plane " << mPbMap.vPlanes[j].id << " area " << mPbMap.vPlanes[j].areaVoxels<< " of polygon " << mPbMap.vPlanes[j].compute2DPolygonalArea() << endl; #endif if( observedPlanes.count(mPbMap.vPlanes[j].id) == 0 ) // If this plane has already been observed through a previous partial plane in this same keyframe, then we must not account twice in the observation count { mPbMap.vPlanes[j].numObservations++; // Update co-visibility graph for(set<unsigned>::iterator it = observedPlanes.begin(); it != observedPlanes.end(); it++) if(mPbMap.vPlanes[j].neighborPlanes.count(*it)) { mPbMap.vPlanes[j].neighborPlanes[*it]++; mPbMap.vPlanes[*it].neighborPlanes[mPbMap.vPlanes[j].id]++; // j = mPbMap.vPlanes[j] } else { mPbMap.vPlanes[j].neighborPlanes[*it] = 1; mPbMap.vPlanes[*it].neighborPlanes[mPbMap.vPlanes[j].id] = 1; } observedPlanes.insert(mPbMap.vPlanes[j].id); } #ifdef _VERBOSE cout << "Same plane\n"; #endif itPlane++; for(size_t k = j+1; k < numPrevPlanes; k++, itPlane++) // numPrevPlanes if( areSamePlane(mPbMap.vPlanes[j], mPbMap.vPlanes[k], configPbMap.max_cos_normal, configPbMap.max_dist_center_plane, configPbMap.proximity_threshold) ) // The planes are merged if they are the same { mergePlanes(mPbMap.vPlanes[j], mPbMap.vPlanes[k]); mPbMap.vPlanes[j].numObservations += mPbMap.vPlanes[k].numObservations; for(set<unsigned>::iterator it = mPbMap.vPlanes[k].nearbyPlanes.begin(); it != mPbMap.vPlanes[k].nearbyPlanes.end(); it++) mPbMap.vPlanes[*it].nearbyPlanes.erase(mPbMap.vPlanes[k].id); for(map<unsigned,unsigned>::iterator it = mPbMap.vPlanes[k].neighborPlanes.begin(); it != mPbMap.vPlanes[k].neighborPlanes.end(); it++) mPbMap.vPlanes[it->first].neighborPlanes.erase(mPbMap.vPlanes[k].id); // Update plane index for(size_t h = k+1; h < numPrevPlanes; h++) --mPbMap.vPlanes[h].id; for(size_t h = 0; h < numPrevPlanes; h++) { if(k==h) continue; for(set<unsigned>::iterator it = mPbMap.vPlanes[h].nearbyPlanes.begin(); it != mPbMap.vPlanes[h].nearbyPlanes.end(); it++) if(*it > mPbMap.vPlanes[k].id) { mPbMap.vPlanes[h].nearbyPlanes.insert(*it-1); mPbMap.vPlanes[h].nearbyPlanes.erase(*it); } for(map<unsigned,unsigned>::iterator it = mPbMap.vPlanes[h].neighborPlanes.begin(); it != mPbMap.vPlanes[h].neighborPlanes.end(); it++) if(it->first > mPbMap.vPlanes[k].id) { mPbMap.vPlanes[h].neighborPlanes[it->first-1] = it->second; mPbMap.vPlanes[h].neighborPlanes.erase(it); } } mPbMap.vPlanes.erase(itPlane); --numPrevPlanes; #ifdef _VERBOSE cout << "MERGE TWO PREVIOUS PLANES WHEREBY THE INCORPORATION OF A NEW REGION \n"; #endif } break; } } if(!isSamePlane) { detectedPlanes[i].id = mPbMap.vPlanes.size(); detectedPlanes[i].numObservations = 1; detectedPlanes[i].bFullExtent = false; detectedPlanes[i].nFramesAreaIsStable = 0; if(configPbMap.makeClusters) { detectedPlanes[i].semanticGroup = clusterize->currentSemanticGroup; clusterize->groups[clusterize->currentSemanticGroup].push_back(detectedPlanes[i].id); } #ifdef _VERBOSE cout << "New plane " << detectedPlanes[i].id << " area " << detectedPlanes[i].areaVoxels<< " of polygon " << detectedPlanes[i].areaHull << endl; #endif // Update proximity graph checkProximity(detectedPlanes[i], configPbMap.proximity_neighbor_planes); // Detect neighbors with max separation of 1.0 meters // Update co-visibility graph for(set<unsigned>::iterator it = observedPlanes.begin(); it != observedPlanes.end(); it++) { detectedPlanes[i].neighborPlanes[*it] = 1; mPbMap.vPlanes[*it].neighborPlanes[detectedPlanes[i].id] = 1; } observedPlanes.insert(detectedPlanes[i].id); mPbMap.vPlanes.push_back(detectedPlanes[i]); } } if(frameQueue.size() == 12) cout << "Same plane? " << areSamePlane(mPbMap.vPlanes[2], mPbMap.vPlanes[9], configPbMap.max_cos_normal, configPbMap.max_dist_center_plane, configPbMap.proximity_threshold) << endl; #ifdef _VERBOSE cout << "\n\tobservedPlanes: "; cout << observedPlanes.size () << " Planes observed\n"; for(set<unsigned>::iterator it = observedPlanes.begin(); it != observedPlanes.end(); it++) cout << *it << " "; cout << endl; #endif // For all observed planes for(set<unsigned>::iterator it = observedPlanes.begin(); it != observedPlanes.end(); it++) { Plane &observedPlane = mPbMap.vPlanes[*it]; // Calculate principal direction observedPlane.calcElongationAndPpalDir(); // Update color observedPlane.calcMainColor(); // cout << "Plane " << observedPlane.id << " color\n" << observedPlane.v3colorNrgb << endl; // Infer knowledge from the planes (e.g. do these planes represent the floor, walls, etc.) if(configPbMap.inferStructure) mpPlaneInferInfo->searchTheFloor(poseKF, observedPlane); } // End for obsevedPlanes // Search the floor plane if(mPbMap.FloorPlane != -1) // Verify that the observed planes centers are above the floor { #ifdef _VERBOSE cout << "Verify that the observed planes centers are above the floor\n"; #endif for(set<unsigned>::reverse_iterator it = observedPlanes.rbegin(); it != observedPlanes.rend(); it++) { if(static_cast<int>(*it) == mPbMap.FloorPlane) continue; if( mPbMap.vPlanes[mPbMap.FloorPlane].v3normal.dot(mPbMap.vPlanes[*it].v3center - mPbMap.vPlanes[mPbMap.FloorPlane].v3center) < -0.1 ) { if(mPbMap.vPlanes[mPbMap.FloorPlane].v3normal.dot(mPbMap.vPlanes[*it].v3normal) > 0.99) //(cos 8.1º = 0.99) { mPbMap.vPlanes[*it].label = "Floor"; mPbMap.vPlanes[mPbMap.FloorPlane].label = ""; mPbMap.FloorPlane = *it; } else { // assert(false); mPbMap.vPlanes[mPbMap.FloorPlane].label = ""; mPbMap.FloorPlane = -1; break; } } } } if(configPbMap.detect_loopClosure) for(set<unsigned>::iterator it = observedPlanes.begin(); it != observedPlanes.end(); it++) { if(mpPbMapLocaliser->vQueueObservedPlanes.size() < 10) mpPbMapLocaliser->vQueueObservedPlanes.push_back(*it); } #ifdef _VERBOSE cout << "DetectedPlanesCloud finished\n"; #endif }
void KeyframeLoopDetector::detectLoop(VisualFeatureMatcher_Generic& matcher, Visual3DRigidTransformationEstimator& transformationEstimator, ICPPoseRefiner& poseRefiner, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f > >& relativePoses, std::vector<Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6 > > >& informationMatrices, std::vector<int>& fromIndexes, int& toIndex) { static bool loopDetected;loopDetected=false; static int numberInliers; static int j;j=0; std::vector<cv::DMatch> matches; std::vector<char> matchesMask; std::vector<cv::Point2f> points1; std::vector<cv::Point2f> points2; relativePoses.resize(keyframes.size()-1); informationMatrices.resize(keyframes.size()-1); fromIndexes.resize(keyframes.size()-1); toIndex=keyframes.size()-1; for(int i=0;i<keyframes.size()-2;i++) { //If the last vertex pose is reasonably close to poses[i], check for loop closure if(!Miscellaneous::poseHasChanged(poses.back(),poses[i],0.4,30)) { //Match the current keyframe against previous keyframes //profiler.enter("Feature matching"); matches.clear(); matcher.match(descriptorsList[i],descriptorsList.back(),matches); //profiler.leave("Feature matching"); //Perform outlier rejection with the Fundamental or the Homography matrix matchesMask.clear(); #if ENABLE_HOMOGRAPHY_OUTLIER_REMOVAL profiler.enter("Homography matrix outlier removal"); numberInliers = matcher.outlierRemovalHomography(keypointsList[i],keypointsList.back(),matches,matchesMask,3.0); profiler.leave("Homography matrix outlier removal"); #else //profiler.enter("Fundamental matrix outlier removal"); numberInliers = matcher.outlierRemovalFundamentalMat(keypointsList[i],keypointsList.back(),matches,matchesMask,3.0); //profiler.leave("Fundamental matrix outlier removal"); #endif //If the number of inliers is greater than a certain threshold, then try estimate the rigid transformation if(numberInliers>numberInliersThreshold) { points1.clear(); points2.clear(); matcher.get2DMatchedPoints(keypointsList[i],keypointsList.back(),matches,points1,points2,numberInliers,matchesMask); fromIndexes[j]=i; relativePoses[j]=Eigen::Matrix4f::Identity (); //Estimate the rigid transformation //profiler.enter("Rigid transformation estimation"); int numberInliersAux=transformationEstimator.estimateVisual3DRigidTransformation(points1,points2,keyframes[i]->pointCloudPtr,keyframes.back()->pointCloudPtr,relativePoses[j]); //profiler.leave("Rigid transformation estimation"); if(numberInliersAux==-1) { //Not using outlier rejection } else { //Using outlier rejection numberInliers=numberInliersAux; } //If after the RANSAC rejection, there are still enough inliers //we consider that a loop has been detected if(numberInliers>numberInliersThreshold) { loopDetected=true; //Refine the rigid transformation with a ICP/GICP #if ENABLE_RIGID_TRANSFORMATION_REFINEMENT //profiler.enter("Rigid transformation refinement ICP/GICP"); poseRefiner.refinePose(*keyframes[i],*keyframes.back(),relativePoses[j]); //profiler.leave("Rigid transformation refinement ICP/GICP"); #endif //Set the loop relative pose Eigen::FullPivLU<Eigen::Matrix4f> lu(relativePoses[j]); relativePoses[j]=lu.inverse(); //SVD/ICP/GICP returns the inverse of the relative pose //Set the loop information matrix informationMatrices[j]=numberInliers*Eigen::Matrix<double,6,6>::Identity(); j++; } } } } relativePoses.resize(j); fromIndexes.resize(j); informationMatrices.resize(j); //Add the relative pose between previous keyframe and current keyframe relativePoses.push_back(relativePose); fromIndexes.push_back(toIndex-1); //Set the information matrix to the constraint with the previous keyframe //profiler.enter("Feature matching (previous keyframe)"); matches.clear(); matcher.match(descriptorsList[descriptorsList.size()-2],descriptorsList.back(),matches); //profiler.leave("Feature matching (previous keyframe)"); matchesMask.clear(); #if ENABLE_HOMOGRAPHY_OUTLIER_REMOVAL //profiler.enter("Homography matrix outlier removal (previous keyframe)"); numberInliers = matcher.outlierRemovalHomography(keypointsList[keypointsList.size()-2],keypointsList.back(),matches,matchesMask,3.0); //profiler.leave("Homography matrix outlier removal (previous keyframe)"); #else //profiler.enter("Fundamental matrix outlier removal (previous keyframe)"); numberInliers = matcher.outlierRemovalFundamentalMat(keypointsList[keypointsList.size()-2],keypointsList.back(),matches,matchesMask,3.0); //profiler.leave("Fundamental matrix outlier removal (previous keyframe)"); #endif if(numberInliers<=0){numberInliers=1;} informationMatrices.push_back(numberInliers*Eigen::Matrix<double,6,6>::Identity()); #if ENABLE_DEBUG_KEYFRAME_ALIGNMENT static pcl::VoxelGrid<pcl::PointXYZRGB> grid; grid.setLeafSize(0.025,0.025,0.025); //Debug: Save keyframe alignment for(int i=0;i<relativePoses.size();i++) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr alignedCloudPtr( new pcl::PointCloud<pcl::PointXYZRGB> ); Eigen::FullPivLU<Eigen::Matrix4f> lu(relativePoses[i]); Eigen::Matrix4f inverseRelativePose = lu.inverse(); pcl::transformPointCloud(*keyframes[fromIndexes[i]]->pointCloudPtr,*alignedCloudPtr,inverseRelativePose); alignedCloudPtr->header.frame_id = keyframes[toIndex]->pointCloudPtr->header.frame_id; for(int j=0;j<alignedCloudPtr->points.size();j++) { alignedCloudPtr->points[j].r=255; alignedCloudPtr->points[j].g=0; alignedCloudPtr->points[j].b=0; } *alignedCloudPtr += *keyframes[toIndex]->pointCloudPtr; pcl::PointCloud<pcl::PointXYZRGB> downsampledAlignedCloud; grid.setInputCloud (alignedCloudPtr); grid.filter (downsampledAlignedCloud); pcl::io::savePCDFile(mrpt::format("../results/pcd_files/keyframes_%03i_%03i.pcd",fromIndexes[i],toIndex),downsampledAlignedCloud); } #endif }