void
HoughTransform::findMaxima (Float32 threshold, std::vector <Float32>& maxima_values, std::vector <Indices>& maxima_voter_ids)
{
Float32 value;
Indices voter_ids;
Accumulator::ptr accumulator = m_accumulator->copy ();
ExtractIndices extractor;
PointCloud3D inliers, old_cloud, new_cloud;
// Accumulator::ptr accumulator_backup = m_accumulator;
// PointCloud3D::const_ptr cloud_backup = m_cloud;
HoughTransform::ptr hough = this->copy ();
maxima_values.clear ();
maxima_voter_ids.clear ();
UInt8 i = 0;
old_cloud = *m_cloud;
while (true) {
Indices inliers_indices;
Indices max_indices = accumulator->getMaxValue ();
PRINT (max_indices[0]);
PRINT (max_indices[1]);
getInliers (old_cloud, max_indices, inliers_indices);
extractor.setInputCloud (old_cloud);
extractor.setExtractIndices (inliers_indices);
extractor.setNegative (false);
extractor.compute (inliers);
extractor.setNegative (true);
extractor.compute (new_cloud);
PRINT (old_cloud.size ());
old_cloud = new_cloud;
PRINT (inliers.size ());
PRINT (new_cloud.size ());
hough->m_cloud = &inliers;
hough->m_accumulator->reset ();
hough->run ();
*accumulator -= *hough->m_accumulator;
if (++i == 3)
break;
}
*m_accumulator = *accumulator;
}
