void findOverlaps(const PCRGB::Ptr& target_pc, const PCRGB::Ptr& source_pc,
pcl::IndicesPtr& target_inds_list, pcl::IndicesPtr& source_inds_list,
double icp_radius=0.03, bool use_rgb=false, double color_weight=0.001)
{
KDTree::Ptr kd_tree(new pcl::KdTreeFLANN<PRGB> ());
kd_tree->setInputCloud(target_pc);
if(use_rgb){
boost::shared_ptr<pcl::DefaultPointRepresentation<PRGB> const>
pt_rep(new pcl::DefaultPointRepresentation<PRGB>(color_weight, color_weight, color_weight));
kd_tree->setPointRepresentation(pt_rep);
}
vector<bool> target_inds(target_pc->points.size(), false);
vector<bool> source_inds(source_pc->points.size(), false);
for(uint32_t i=0;i<source_pc->points.size();i++) {
vector<int> inds;
vector<float> dists;
kd_tree->radiusSearch(*source_pc, i, icp_radius, inds, dists);
for(uint32_t j=0;j<inds.size();j++)
target_inds[inds[j]] = true;
if(inds.size() > 0)
source_inds[i] = true;
}
for(uint32_t i=0;i<target_pc->points.size();i++)
if(target_inds[i])
target_inds_list->push_back(i);
for(uint32_t i=0;i<source_pc->points.size();i++)
if(source_inds[i])
source_inds_list->push_back(i);
}
pcl::IndicesPtr normalFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint)
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
pcl::NormalEstimation<PointT, pcl::Normal> ne;
ne.setInputCloud (cloud);
if(indices->size())
{
ne.setIndices(indices);
}
KdTreePtr tree (new KdTree(false));
if(indices->size())
{
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud(cloud);
}
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (radiusSearch);
if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
{
ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
}
ne.compute (*cloud_normals);
pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
int oi = 0; // output iterator
Eigen::Vector3f n(normal[0], normal[1], normal[2]);
for(unsigned int i=0; i<cloud_normals->size(); ++i)
{
Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
float angle = pcl::getAngle3D(normal, v);
if(angle < angleMax)
{
output->at(oi++) = indices->size()!=0?indices->at(i):i;
}
}
output->resize(oi);
return output;
}
void updateIndices(pcl::IndicesPtr indices, pcl::PointIndices::Ptr inliers)
{
int toRemove = inliers->indices.size();
for(int i=0;i<toRemove;i++)
{
int origSize = indices->size();
for(int j=0;j<origSize;j++)
{
if((*indices)[j]==inliers->indices[i])
{
indices->erase(indices->begin()+j);
break;
}
}
}
}
void FindFloorPlaneRANSAC()
{
double t = getTickCount();
cout << "RANSAC...";
/*
pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> (cloud));
*/
pcl::SampleConsensusModelNormalPlane<pcl::PointXYZRGB,pcl::Normal>::Ptr model_p(
new pcl::SampleConsensusModelNormalPlane<pcl::PointXYZRGB,pcl::Normal>(cloud));
// model_p->setInputCloud(cloud);
model_p->setInputNormals(mls_normals);
model_p->setNormalDistanceWeight(0.75);
inliers.reset(new vector<int>);
ransac.reset(new pcl::RandomSampleConsensus<pcl::PointXYZRGB>(model_p));
ransac->setDistanceThreshold (.1);
ransac->computeModel();
ransac->getInliers(*inliers);
t = ((double)getTickCount() - t)/getTickFrequency();
cout << "Done. (" << t <<"s)"<< endl;
ransac->getModelCoefficients(coeffs[0]);
model_p->optimizeModelCoefficients(*inliers,coeffs[0],coeffs[1]);
floorcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud,*inliers,*floorcloud);
}
void populateIndices(pcl::IndicesPtr indices, int size)
{
for(int i=0;i<size;i++)
{
indices->push_back(i);
}
}
pcl::IndicesPtr radiusFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
KdTreePtr tree (new KdTree(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud, indices);
for(unsigned int i=0; i<indices->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
}
output->resize(oi);
return output;
}
else
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud);
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = i;
}
}
output->resize(oi);
return output;
}
}
bool checkChange()
{
cv::Mat mask = cv::Mat::zeros(color.rows, color.cols, CV_8U);
cv::Mat img = cv::Mat::zeros(color.rows, color.cols, CV_32FC4);
size_t changedPixels = 0;
size_t size = indices->size();
changes.clear();
// Check pixel changes
for(size_t i = 0; i < indices->size(); ++i)
{
const int idx = indices->at(i);
cv::Vec4f v = invariantColor(color.at<cv::Vec3b>(idx), depth.at<uint16_t>(idx));
img.at<cv::Vec4f>(idx) = v;
mask.at<uint8_t>(idx) = 1;
if(checkChange(v, idx))
{
changes.push_back(idx);
++changedPixels;
}
}
// Check pixels that are masked out in current but not in last frame
for(size_t i = 0; i < lastIndices.size(); ++i)
{
const int idx = lastIndices.at(i);
if(!mask.at<uint8_t>(idx))
{
changes.push_back(idx);
++changedPixels;
++size;
}
}
lastImg = img;
lastMask = mask;
indices->swap(lastIndices);
const float diff = changedPixels / (float)size;
outInfo(changedPixels << " from " << size << " pixels changed (" << diff * 100 << "%)");
return diff > threshold;
}
void pcdCloud::extract(CloudPtr output, pcl::IndicesPtr inliers, pcl::IndicesPtr outliers, bool invert=false)
{
extract(output, inliers, invert);
outliers->assign(_extractor.getRemovedIndices()->begin(), _extractor.getRemovedIndices()->end());
// std::cout << "# removedIndices = " << outliers->size() << std::endl;
// if invert=true, meaning extracting the outliers, then removedIndices which is outliers = inliers
if(invert)
{
std::cout << "warning: param inliers, outliers are the same\n";
}
}
std::vector<pcl::IndicesPtr> extractClusters(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float clusterTolerance,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
KdTreePtr tree(new KdTree);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (clusterTolerance);
ec.setMinClusterSize (minClusterSize);
ec.setMaxClusterSize (maxClusterSize);
ec.setInputCloud (cloud);
if(indices->size())
{
ec.setIndices(indices);
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud(cloud);
}
ec.setSearchMethod (tree);
std::vector<pcl::PointIndices> cluster_indices;
ec.extract (cluster_indices);
int maxIndex=-1;
unsigned int maxSize = 0;
std::vector<pcl::IndicesPtr> output(cluster_indices.size());
for(unsigned int i=0; i<cluster_indices.size(); ++i)
{
output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
if(maxSize < cluster_indices[i].indices.size())
{
maxSize = cluster_indices[i].indices.size();
maxIndex = i;
}
}
if(biggestClusterIndex)
{
*biggestClusterIndex = maxIndex;
}
return output;
}
void filterRegion(const Region ®ion)
{
const float minX = -(region.width / 2) + border;
const float maxX = (region.width / 2) - border;
float minY = -(region.height / 2) + border;
const float maxY = (region.height / 2) - border;
const float minZ = -(region.depth / 2);
float maxZ = 0.5;
if (region.name == "drawer_sinkblock_upper_open")
{
maxZ = 0.08;
}
//needed because of crappy sem map
if(region.name == "kitchen_sink_block_counter_top")
{
minY += 1;//don't get points for the sink
}
else if(region.name == "kitchen_island_counter_top")
{
minY += 0.6; //same for the hot plate
}
tf::Transform transform;
transform = region.transform.inverse() * camToWorld;
Eigen::Affine3d eigenTransform;
tf::transformTFToEigen(transform, eigenTransform);
pcl::PointCloud<PointT>::Ptr transformed(new pcl::PointCloud<PointT>());
pcl::transformPointCloud<PointT>(*cloud, *transformed, eigenTransform);
for(size_t i = 0; i < transformed->points.size(); ++i)
{
const PointT &p = transformed->points[i];
if(p.x > minX && p.x < maxX && p.y > minY && p.y < maxY && p.z > minZ && p.z < maxZ)
{
indices->push_back(i);
}
}
}
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
pcl::IndicesPtr & ground,
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
float groundNormalAngle,
int minClusterSize)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
// Find the ground
pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
cloud,
groundNormalAngle,
Eigen::Vector4f(0,0,1,0),
normalRadiusSearch*2.0f,
Eigen::Vector4f(0,0,100,0));
int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
cloud,
flatSurfaces,
normalRadiusSearch*2.0f,
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max;
pcl::getMinMax3D<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{
if((int)i!=biggestFlatSurfaceIndex)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2] && centroid[2] <= max[2])
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
}
}
if(ground->size() != cloud->size())
{
// Remove ground
pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices<PointT>(cloud, ground);
//Cluster remaining stuff (obstacles)
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters<PointT>(
cloud,
otherStuffIndices,
normalRadiusSearch*2.0f,
minClusterSize);
// merge indices
obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
}
}
TyErrorId processWithLock(CAS &tcas, ResultSpecification const &res_spec)
{
MEASURE_TIME;
outInfo("process begins");
rs::SceneCas cas(tcas);
rs::Scene scene = cas.getScene();
cas.get(VIEW_CLOUD, *cloud);
cas.get(VIEW_COLOR_IMAGE, color);
cas.get(VIEW_DEPTH_IMAGE, depth);
cas.get(VIEW_CAMERA_INFO, cameraInfo);
indices->clear();
indices->reserve(cloud->points.size());
camToWorld.setIdentity();
if(scene.viewPoint.has())
{
rs::conversion::from(scene.viewPoint.get(), camToWorld);
}
else
{
outWarn("No camera to world transformation, no further processing!");
throw rs::FrameFilterException();
}
worldToCam = tf::StampedTransform(camToWorld.inverse(), camToWorld.stamp_, camToWorld.child_frame_id_, camToWorld.frame_id_);
computeFrustum();
//default place to look for objects is counter tops except if we got queried for some different place
//message comes from desigantor and is not the same as the entries from the semantic map so we need
//to transform them
rs::Query qs = rs::create<rs::Query>(tcas);
std::vector<std::string> regionsToLookAt;
regionsToLookAt.assign(defaultRegions.begin(),defaultRegions.end());
regions.clear();
if(cas.getFS("QUERY", qs))
{
outWarn("loaction set in query: " << qs.location());
if(std::find(defaultRegions.begin(), defaultRegions.end(), qs.location()) == std::end(defaultRegions) && qs.location()!="")
{
regionsToLookAt.clear();
regionsToLookAt.push_back(qs.location());
}
}
if(regions.empty())
{
std::vector<rs::SemanticMapObject> semanticRegions;
getSemanticMapEntries(cas, regionsToLookAt, semanticRegions);
regions.resize(semanticRegions.size());
for(size_t i = 0; i < semanticRegions.size(); ++i)
{
Region ®ion = regions[i];
region.width = semanticRegions[i].width();
region.depth = semanticRegions[i].depth();
region.height = semanticRegions[i].height();
region.name = semanticRegions[i].name();
rs::conversion::from(semanticRegions[i].transform(), region.transform);
}
}
for(size_t i = 0; i < regions.size(); ++i)
{
if(frustumCulling(regions[i]) || !frustumCulling_)
{
outInfo("region inside frustum: " << regions[i].name);
filterRegion(regions[i]);
}
else
{
outInfo("region outside frustum: " << regions[i].name);
}
}
pcl::ExtractIndices<PointT> ei;
ei.setKeepOrganized(true);
ei.setIndices(indices);
ei.filterDirectly(cloud);
cas.set(VIEW_CLOUD, *cloud);
if(changeDetection && !indices->empty())
{
++frames;
if(lastImg.empty())
{
lastMask = cv::Mat::ones(color.rows, color.cols, CV_8U);
lastImg = cv::Mat::zeros(color.rows, color.cols, CV_32FC4);
}
uint32_t secondsPassed = camToWorld.stamp_.sec - lastTime.sec;
bool change = checkChange() || cas.has("QUERY") || secondsPassed > timeout;
if(!change)
{
++filtered;
}
else
{
lastTime = camToWorld.stamp_;
}
outInfo("filtered frames: " << filtered << " / " << frames << "(" << (filtered / (float)frames) * 100 << "%)");
if(!change)
{
outWarn("no changes in frame detected, no further processing!");
throw rs::FrameFilterException();
}
}
return UIMA_ERR_NONE;
}
