std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> MovingObjectsIdentificator::extractClusters() {
if(verbose) std::cout << "Extracting clusters ... ";
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree (new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(workingCloud);
std::vector<pcl::PointIndices> indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ece;
ece.setClusterTolerance(clusterTolerance);
ece.setMinClusterSize(minClusterSize);
ece.setSearchMethod(kdTree);
ece.setInputCloud(workingCloud);
ece.extract(indices);
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > clusters;
for(std::vector<pcl::PointIndices>::iterator it = indices.begin(); it != indices.end(); it++) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(workingCloud);
pcl::PointIndices::Ptr pi(new pcl::PointIndices);
pi->indices=it->indices;
extract.setIndices(pi);
extract.setNegative(false);
extract.filter(*cluster);
clusters.push_back(cluster);
}
if(verbose) std::cout << "done!" << std::endl;
return clusters;
}
bool SIFTDetector::createSiftDetector() {
TRACE(logger, "createSiftDetector: Starting");
decltype(siftDetector) detector( new pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointWithScale>() );
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kdTree( new pcl::search::KdTree<pcl::PointXYZRGB>() );
detector->setSearchMethod(kdTree);
detector->setScales(minScale, octaves, scalesPerOctave);
detector->setMinimumContrast(minContrast);
siftDetector = std::move( detector );
TRACE(logger, "createSiftDetector: Finished");
return true;
}
void NormalEstimator::createNormalEstimator() {
TRACE(logger, "createNormalEstimator: Starting");
std::unique_ptr<pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal>> estimator(new pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal>() );
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kdTree( new pcl::search::KdTree<pcl::PointXYZRGB>() );
estimator->setSearchMethod( kdTree );
float radius = getParam<float>( normalRadius );
if(radius > 0)
estimator->setRadiusSearch( radius );
else {
int k = getParam<int>( kNN );
assert(k > 0);
estimator->setKSearch(k);
}
normalEstimator = std::move( estimator );
TRACE(logger, "createNormalEstimator: Finished");
}
_Use_decl_annotations_
KdTreeCompiler2* KdTreeCompiler2::CreateFromTriangles(const StaticGeometryVertex* vertices, uint32_t numVertices, const uint32_t* indices, uint32_t numIndices)
{
UNREFERENCED_PARAMETER(vertices);
UNREFERENCED_PARAMETER(numVertices);
UNREFERENCED_PARAMETER(indices);
UNREFERENCED_PARAMETER(numIndices);
std::unique_ptr<KdTreeCompiler2> kdTree(new KdTreeCompiler2());
uint32_t numTriangles = numIndices / 3;
kdTree->_vertices.resize(numVertices);
for (uint32_t i = 0; i < numVertices; ++i)
{
kdTree->_vertices[i] = vertices[i];
}
float min[3], max[3];
uint32_t tri = kdTree->AllocCompilerTriangle(vertices, indices[0], indices[1], indices[2]);
memcpy_s(min, sizeof(min), kdTree->_compilerTriangles[0].Min, sizeof(min));
memcpy_s(max, sizeof(max), kdTree->_compilerTriangles[0].Max, sizeof(max));
for (uint32_t i = 1; i < numTriangles; ++i)
{
uint32_t triNext = kdTree->AllocCompilerTriangle(vertices, indices[i * 3], indices[i * 3 + 1], indices[i * 3 + 2]);
KdCompilerTriangle* t = &kdTree->_compilerTriangles[triNext];
for (uint32_t axis = 0; axis < 3; ++axis)
{
if (t->Min[axis] < min[axis]) min[axis] = t->Min[axis];
if (t->Max[axis] > max[axis]) max[axis] = t->Max[axis];
}
kdTree->_compilerTriangles[tri].Next = triNext;
tri = triNext;
}
kdTree->_root = kdTree->AllocNode();
kdTree->Process(0, 0, numTriangles, min, max);
return kdTree.release();
}
int getDifferencesCount(const PCPtr& src,
const PCPtr& tgt,
float distanceThreshold)
{
PCPtr small, big;
if(src->size() < tgt->size())
{
small = src;
big = tgt;
}
else
{
small = tgt;
big = src;
}
//Seteo el kdtree con la segunda nube
pcl::ExtractIndices<PCXYZ> extract;
pcl::PointIndices::Ptr repeatedPoints (new pcl::PointIndices ());
pcl::KdTreeFLANN<PCXYZ>::Ptr kdTree (new pcl::KdTreeFLANN<PCXYZ> (false));
std::vector<int> kIndices (1);
std::vector<float> kDistances (1);
std::vector<int> indicesToRemove;
int foundedPoints = 0;
kdTree->setInputCloud(big);
for (size_t i = 0; i < small->points.size (); ++i){
foundedPoints = kdTree->nearestKSearch(small->at(i),1,kIndices,kDistances);
if(foundedPoints > 0)
{
if(kDistances.at(0) < distanceThreshold)
indicesToRemove.push_back(kIndices.at(0));
}
}
return big->size() - indicesToRemove.size();
}
static void dominantTransforms(const cv::Mat &img, std::vector <cv::Point2i> &transforms,
const int nTransform, const int psize)
{
const int zeroThresh = 2*psize;
const int leafNum = 64;
/** Walsh-Hadamard Transformation **/
std::vector <cv::Mat> channels;
cv::split(img, channels);
int cncase = std::max(img.channels() - 2, 0);
const int np[] = {cncase == 0 ? 12 : (cncase == 1 ? 16 : 10),
cncase == 0 ? 12 : (cncase == 1 ? 04 : 02),
cncase == 0 ? 00 : (cncase == 1 ? 04 : 02),
cncase == 0 ? 00 : (cncase == 1 ? 00 : 10)};
for (int i = 0; i < img.channels(); ++i)
rgb2whs(channels[i], channels[i], np[i], psize);
cv::Mat whs; // Walsh-Hadamard series
cv::merge(channels, whs);
KDTree <float, 24> kdTree(whs, leafNum, zeroThresh);
std::vector <int> annf( whs.total(), 0 );
/** Propagation-assisted kd-tree search **/
for (int i = 0; i < whs.rows; ++i)
for (int j = 0; j < whs.cols; ++j)
{
double dist = std::numeric_limits <double>::max();
int current = i*whs.cols + j;
int dy[] = {0, 1, 0}, dx[] = {0, 0, 1};
for (int k = 0; k < int( sizeof(dy)/sizeof(int) ); ++k)
if ( i - dy[k] >= 0 && j - dx[k] >= 0 )
{
int neighbor = (i - dy[k])*whs.cols + (j - dx[k]);
int leafIdx = (dx[k] == 0 && dy[k] == 0)
? neighbor : annf[neighbor] + dy[k]*whs.cols + dx[k];
kdTree.updateDist(leafIdx, current,
annf[i*whs.cols + j], dist);
}
}
/** Local maxima extraction **/
cv::Mat_<double> annfHist(2*whs.rows - 1, 2*whs.cols - 1, 0.0),
_annfHist(2*whs.rows - 1, 2*whs.cols - 1, 0.0);
for (size_t i = 0; i < annf.size(); ++i)
++annfHist( annf[i]/whs.cols - int(i)/whs.cols + whs.rows - 1,
annf[i]%whs.cols - int(i)%whs.cols + whs.cols - 1 );
cv::GaussianBlur( annfHist, annfHist,
cv::Size(0, 0), std::sqrt(2.0), 0.0, cv::BORDER_CONSTANT);
cv::dilate( annfHist, _annfHist,
cv::Matx<uchar, 9, 9>::ones() );
std::vector < std::pair<double, int> > amount;
std::vector <cv::Point2i> shiftM;
for (int i = 0, t = 0; i < annfHist.rows; ++i)
{
double *pAnnfHist = annfHist.ptr<double>(i);
double *_pAnnfHist = _annfHist.ptr<double>(i);
for (int j = 0; j < annfHist.cols; ++j)
if ( pAnnfHist[j] != 0 && pAnnfHist[j] == _pAnnfHist[j] )
{
amount.push_back( std::make_pair(pAnnfHist[j], t++) );
shiftM.push_back( cv::Point2i(j - whs.cols + 1,
i - whs.rows + 1) );
}
}
std::partial_sort( amount.begin(), amount.begin() + nTransform,
amount.end(), std::greater< std::pair<double, int> >() );
transforms.resize(nTransform);
for (int i = 0; i < nTransform; ++i)
{
int idx = amount[i].second;
transforms[i] = cv::Point2i( shiftM[idx].x, shiftM[idx].y );
}
}
