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;
}
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;
}
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();
}
}
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]
}
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 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 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 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
}
