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