songsenand
/
cgal
mirror of https://github.com/CGAL/cgal
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
cgal/Point_set_shape_detection_3/include/CGAL/Efficient_ransac.h

541 lines
21 KiB
C++
Raw Blame History

// Copyright (c) 2013 INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
// You can redistribute it and/or modify it under the terms of the GNU
// General Public License as published by the Free Software Foundation,
// either version 3 of the License, or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
//
//
// Author(s) : Yannick Verdié, Clément Jamin
//
//******************************************************************************
// File Description :
//
//
//******************************************************************************
#ifndef CGAL_EFFICIENT_RANSAC_H
#define CGAL_EFFICIENT_RANSAC_H
#include "Types.h"
#include "Octree.h"
#include "Primitive.h"
#include "Cylinder.h"
#include "Plane.h"
#include "Sphere.h"
//for octree ------------------------------
#include <CGAL/Kd_tree.h>
#include <CGAL/Fuzzy_iso_box.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/basic.h>
#include <CGAL/Search_traits_adapter.h>
#include <boost/iterator/zip_iterator.hpp>
#include <CGAL/bounding_box.h> //----------
#include <utility> // defines std::pair
#include <set> // defines std::set
#include <list>
#include <vector>
#include <fstream>
#include <sstream>
#include <random>
#define _USE_MATH_DEFINES
#include <math.h>
//boost --------------
#include <boost/iterator/counting_iterator.hpp>
#include <boost/bind/make_adaptable.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <functional>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
//---------------------
//#define D1
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
namespace CGAL {
namespace Efficient_ransac {
#ifdef DEBUG
#define printd(...) printf(__VA_ARGS__)
#else
#define printd(...) printf(__VA_ARGS__)
#endif
#ifdef D1
#define printd1(...) printf(__VA_ARGS__)
#else
#define printd1(...)
#endif
//m for members
//l for local
//c for constants/readonlys
//p for pointer (and pp for pointer to pointer)
//v for volatile
//s for static
//i for indexes and iterators
//e for eventstemplate <typename Kernel, class inputDataType>
template <typename Kernel, class inputDataType>
class Efficient_ransac
{
public:
typedef typename std::vector<inputDataType>::iterator inputIterator;
//--------------------------------------------typedef
typedef typename Kernel::FT FT;
// basic geometric types
typedef typename Kernel::Point_3 Point;
typedef typename Kernel::Vector_3 Vector;
typedef typename Kernel::Line_3 Line;
typedef typename std::pair<Point, Vector> Pwn;
//more advance container for geometric types
typedef typename std::vector<Point>::const_iterator Point_const_iterator;
typedef Primitive_ab<Kernel, inputDataType> Primitive;
typedef Octree<Kernel, DirectPointAccessor<inputDataType>, inputDataType> DirectOctree;
typedef Octree<Kernel, IndexedPointAccessor<inputDataType>, inputDataType> IndexedOctree;
//--------------------------------------------typedef
public:
Efficient_ransac() {};
~Efficient_ransac() {/*if (m_global_tree) delete m_global_tree;*/}
Efficient_ransac(inputIterator first, inputIterator beyond);
void /*std::pair<std::vector<Primitive *>, std::vector<int> >*/ run(); //the primitive and the index of the point sorted
void addPrimitives(Primitive* p) {m_list_sought_primitives.insert(p->type());delete p;}
const std::vector<Primitive *> &getExtractedPrimitives() {return m_extractedPrimitives;}
int unassignedPoints() {return m_numAvailablePoints;}
void setOptions(Option_RANSAC _m) {m_options = _m;}
private:
int getLevelOctree();
Primitive* getBestCandidate(std::vector<Primitive* >& l_list_candidates, const int _SizeP);
inline FT getRadiusSphere_from_level(int l_level_octree) {return m_max_radiusSphere_Octree/powf(2, l_level_octree);}
bool improveBound(Primitive *candidate, const int _SizeP, unsigned int max_subset, unsigned int min_points);
void rescoreCandidates(std::vector<Primitive *> &candidates);
inline FT StopProbability(FT _sizeC, FT _np, FT _dC, FT _l) const
{
//printf("stop without thr %f\n", std::pow(1.f - _sizeC/ (_np * _l * 4), _dC));
return std::min<float>(std::pow(1.f - _sizeC / (_np * _l * 4), _dC), 1.); //4 is (1 << (m_reqSamples - 1))) with m_reqSamples=3 (min number of points to create a candidate)
}
static bool candComp(const Primitive* a, const Primitive* b) {
return a->ExpectedValue() < b->ExpectedValue();
}
//--------------------------------------------Functions
//--------------------------------------------Variables
public:
static const unsigned int scm_max_depth_octree = 4;
private:
Option_RANSAC m_options;
DirectOctree **m_direct_octrees;
IndexedOctree *m_global_octree;
std::vector<int> m_availableOctreeSizes;
unsigned int m_num_subsets;
std::vector<int> m_shapeIndex; // maps index into points to assigned extracted primitive
unsigned int m_numAvailablePoints;
std::vector<int> m_index_subsets; //give the index of the subset of point i
std::vector<Primitive *> m_extractedPrimitives;
std::set<int> m_list_sought_primitives;
inputIterator m_it_Point_Normal, m_last_it_Point_Normal; //copy iterator of input points
FT m_max_radiusSphere_Octree;
std::vector<FT> m_level_weighting; //sum must be 1
//--------------------------------------------Variables
};
template <typename Kernel, class inputDataType>
Efficient_ransac<Kernel, inputDataType>::Efficient_ransac(inputIterator first, inputIterator beyond)
{
srand ( time(NULL) );
m_global_octree = new IndexedOctree(first, beyond, 0);
m_numAvailablePoints = beyond - first;
m_it_Point_Normal = first;
m_last_it_Point_Normal = beyond;
//create subsets ------------------------------------------------
//how many subsets ?
m_num_subsets = std::max( (int) std::floor(std::logf(m_numAvailablePoints)/std::logf(2) )-9, 2);
printd("number of subtree: %d\n", m_num_subsets);
// SUBSET GENERATION ->
// approach with increasing subset sizes -> replace with octree later on
std::random_device rd;
std::mt19937 gen(rd());
std::uniform_int_distribution<> dis(0, 1);
inputIterator end = beyond - 1;
int remainingPoints = m_numAvailablePoints;
m_availableOctreeSizes.resize(m_num_subsets);
m_direct_octrees = new DirectOctree *[m_num_subsets];
std::cout << "subSetSizes: ";
for (int s = m_num_subsets - 1;s>=0;--s) {
int subsetSize = remainingPoints;
std::vector<unsigned int> indices(subsetSize);
if (s) {
subsetSize >>= 1;
for (unsigned int i = 0;i<subsetSize;i++) {
int index = dis(gen);
index = index + (i<<1);
index = (index >= remainingPoints) ? remainingPoints - 1 : index;
indices[i] = index;
}
// move points to the end of the point vector
for (int i = subsetSize - 1;i>=0;i--) {
inputDataType tmp = (*end);
*end = first[indices[i]];
first[indices[i]] = tmp;
end--;
}
m_direct_octrees[s] = new DirectOctree(end + 1, end + subsetSize + 1, remainingPoints - subsetSize);
}
else
m_direct_octrees[0] = new DirectOctree(first, first + (subsetSize), 0);
std::cout << subsetSize << " ";
m_availableOctreeSizes[s] = subsetSize;
m_direct_octrees[s]->createTree();
remainingPoints -= subsetSize;
}
m_global_octree = new IndexedOctree(first, beyond);
m_global_octree->createTree();
//m_global_octree->verify();
printd("init Ransac done\n");
};
template <typename Kernel, class inputDataType>
int Efficient_ransac<Kernel, inputDataType>::getLevelOctree()
{
return abs(getRandomInt()) % (m_global_octree->maxLevel() + 1);
};
template <typename Kernel, class inputDataType>
Primitive_ab<Kernel, inputDataType>* Efficient_ransac<Kernel, inputDataType>::getBestCandidate(std::vector<Primitive* >& l_list_candidates, const int _SizeP)
{
if (l_list_candidates.size() == 1) return l_list_candidates.back();
int index_worse_candidate = 0;
bool improved = true;
while (index_worse_candidate < l_list_candidates.size()-1 && improved) //quit if find best candidate or no more improvement possible
{
improved = false;
std::sort(l_list_candidates.begin()+index_worse_candidate, l_list_candidates.end(), [](Primitive* a, Primitive* b)
{
return a->maxBound() < b->maxBound();
});
//refine the best one
improveBound(l_list_candidates.back(), _SizeP, m_num_subsets, m_options.m_minNbPoints);
int position_stop;
//Take all those intersecting the best one
for (position_stop= l_list_candidates.size()-2;position_stop>=index_worse_candidate;position_stop--)
{
if (l_list_candidates.back()->minBound() > l_list_candidates.at(position_stop)->maxBound() ) break;//the intervals do not overlaps anymore
if (l_list_candidates.at(position_stop)->maxBound() <= m_options.m_minNbPoints) break; //the following candidate doesnt have enough points !
//if we reach this point, there is an overlaps between best one and position_stop
//so request refining bound on position_stop
improved |= improveBound(l_list_candidates.at(position_stop), _SizeP, m_num_subsets, m_options.m_minNbPoints);//this include the next subset for computing bounds, -> the score is then returned by ExpectedValue()
//test again after refined
if (l_list_candidates.back()->minBound() > l_list_candidates.at(position_stop)->maxBound() ) break;//the intervals do not overlaps anymore
}
index_worse_candidate = position_stop;
}
if (index_worse_candidate == l_list_candidates.size()-1 && improved) printf("delete everything, should never happen (%d) !!!!!!\n", index_worse_candidate);
return l_list_candidates.back();
};
template <typename Kernel, class inputDataType>
bool Efficient_ransac<Kernel, inputDataType>::improveBound(Primitive *candidate, const int _SizeP, unsigned int max_subset, unsigned int min_points) {
candidate->m_nb_subset_used = (candidate->m_nb_subset_used >= m_num_subsets) ? m_num_subsets - 1 : candidate->m_nb_subset_used;
//what it does is add another subset and recompute lower and upper bound
//the next subset to include is provided by m_nb_subset_used
//1. sum nb points of previous subset, and score
int l_sum_score = candidate->m_score;
int l_nb_total_points_subsets = 0;
for (int i=0;i<candidate->m_nb_subset_used;i++)
l_nb_total_points_subsets += m_availableOctreeSizes[i]; //no some points are forbidden now
//2. need score of new subset as well as sum of the score of the previous considered subset
int l_new_score = 0;
do
{
//l_new_score = candidate->score(_t.first, candidate->m_query_ball, _it_Points, _it_Normal, is_available);
l_new_score = m_direct_octrees[candidate->m_nb_subset_used]->score(candidate, m_it_Point_Normal, m_shapeIndex, m_options.m_epsilon, m_options.m_normalThresh);
//candidate->m_Indices.clear();
//l_new_score3 = m_direct_octrees[candidate->m_nb_subset_used]->fullScore(candidate, m_shapeIndex, m_options.m_epsilon, m_options.m_normalThresh);
candidate->m_score += l_new_score;
//int tmp = score(octree)
//printf("subset %d exp value %f\n", m_nb_subset_used, l_new_score);
//candidate->m_score_by_subset.push_back(l_new_score); //add to vector of score
l_nb_total_points_subsets += m_availableOctreeSizes[candidate->m_nb_subset_used]; //add point new subset
l_sum_score += l_new_score;
candidate->m_nb_subset_used++;
}
while (l_sum_score < min_points && candidate->m_nb_subset_used < m_num_subsets);
// if (l_new_score == 0) {
// return false;
// };
candidate->computeBound(l_nb_total_points_subsets, _SizeP);//estimate the bound
return true;
}
template <typename Kernel, class inputDataType>
void Efficient_ransac<Kernel, inputDataType>::run()
{
//no primitives added, exit
if (m_list_sought_primitives.size() == 0) return;
// initializing the shape index
m_shapeIndex.resize(m_numAvailablePoints, -1);
std::vector<Primitive* > l_result; // Y <- 0 (no result)
std::vector<Primitive* > l_list_candidates; // C <- 0 (no candidate)
int requiredSamples = 4;
int l_index_point_p1;
int l_level_octree ; //level 0 is full size, i.e
FT l_radius_Sphere;
FT bestExp = 0;
int numInvalid = 0; // number of points that have been assigned to a shape
int nbNewCandidates = 0;
int nbFailedCandidates = 0;
bool forceExit = false;
printf("Starting...\n");
do //main loop
{
bestExp = 0;
//TODO: do this in batch of n candidate, and thus can be parallelized in n threads
do //generate candidate
{
//1. pick a point p1 randomly among available points
std::set<int> indices;
bool done = false;
do {
l_index_point_p1 = getRandomInt() % m_numAvailablePoints;
//done = m_global_octree->getPointsInCellContainingPoint((m_it_Point_Normal + l_index_point_p1)->first, getLevelOctree(), indices, m_shapeIndex);
done = m_global_octree->drawSamplesFromCellContainingPoint((m_it_Point_Normal + l_index_point_p1)->first, getLevelOctree(), indices, m_shapeIndex, requiredSamples);
} while (m_shapeIndex[l_index_point_p1] != -1 || !done);
//add candidate for each type of primitives
for(std::set<int>::iterator it = m_list_sought_primitives.begin(); it != m_list_sought_primitives.end(); it++) //1 type
{
Primitive *p = Primitive::create(*it, m_options.m_epsilon, m_options.m_normalThresh);
p->compute( indices, m_it_Point_Normal); //compute the primitive and says if the candidate is valid
if (p->isValid()) {
improveBound(p, m_numAvailablePoints - numInvalid, 1, 500);//this include the next subset for computing bounds, -> the score is then returned by ExpectedValue()
//evaluate the candidate
if(p->maxBound() >= m_options.m_minNbPoints) {
if (bestExp < p->ExpectedValue()) bestExp = p->ExpectedValue();
l_list_candidates.push_back(p);
nbNewCandidates++;
}
else {
nbFailedCandidates++;
delete p;
}
}
else {
nbFailedCandidates++;
delete p;
}
}
if (nbFailedCandidates >= 100000)
forceExit = true;
} while( !forceExit
&& StopProbability(bestExp, m_numAvailablePoints - numInvalid, nbNewCandidates /*l_list_candidates.size()*/, scm_max_depth_octree) > m_options.m_probability
&& StopProbability(m_options.m_minNbPoints, m_numAvailablePoints - numInvalid, nbNewCandidates /*l_list_candidates.size()*/, scm_max_depth_octree) > m_options.m_probability);
// end of generate candidate
if (forceExit) break;
//now get the best candidate in the current set of all candidates
//Note that the function sort the candidates: the best candidate is always the last element of the vector
Primitive *best_Candidate = getBestCandidate(l_list_candidates, m_numAvailablePoints - numInvalid);
best_Candidate->m_indices.clear();
int s = m_global_octree->score(best_Candidate, m_it_Point_Normal, m_shapeIndex, 3 * m_options.m_epsilon, m_options.m_normalThresh);
int cc = best_Candidate->connectedComponent(m_it_Point_Normal, m_options.m_bitmapEpsilon, m_global_octree->m_center, m_global_octree->m_width);
//if the bestCandidate is good enough (proba of overlook lower than m_options.m_probability)
if (StopProbability(best_Candidate->ExpectedValue(), (m_numAvailablePoints - numInvalid),
nbNewCandidates/*l_list_candidates.size()*/, scm_max_depth_octree) <= m_options.m_probability)
{
//we keep it
if (best_Candidate->getPointsIndices()->size() >= m_options.m_minNbPoints)
{
printd1(best_Candidate->info().c_str());
l_list_candidates.back() = NULL; //put null like that when we delete the vector, the object is not lost (pointer saved in bestCandidate)
//1. add best candidate to final result.
l_result.push_back(best_Candidate);
//2. remove the points
//2.1 update boolean
std::vector<int> *indices_points_best_candidate = best_Candidate->getPointsIndices();
for (int i=0;i<indices_points_best_candidate->size();i++)
{
if (m_shapeIndex[indices_points_best_candidate->at(i)] != -1) {
std::cout << "shapeIndex already assigned!" << std::endl;
}
m_shapeIndex[indices_points_best_candidate->at(i)] = l_result.size() - 1;
numInvalid++;
bool exactlyOnce = true;
for (unsigned int j = 0;j<m_num_subsets;j++) {
if (m_direct_octrees[j] && m_direct_octrees[j]->m_root) {
int offset = m_direct_octrees[j]->offset();
if (offset <= indices_points_best_candidate->at(i) && (indices_points_best_candidate->at(i) - offset) < m_direct_octrees[j]->size()) {
if (!exactlyOnce) {
std::cout << "adjusting available octree sizes failed! (twice)" << std::endl;
}
exactlyOnce = false;
m_availableOctreeSizes[j]--;
}
}
}
if (exactlyOnce) {
std::cout << "adjusting available octree sizes failed! (never)" << std::endl;
}
}
//2.3 block also the points for the subtrees
nbNewCandidates--;
nbFailedCandidates = 0;
bestExp = 0;
//nbNewCandidates = l_result.size();
//3. refit (not implemented yet)
//best_Candidate->LSfit();
//4. save primitive
if (1) {
std::cout << "extracted primitive: " << best_Candidate->info().c_str() << std::endl;
std::stringstream ss;
ss << "ours_" << best_Candidate->type_str() << "_" << l_result.size() << ".ply";
best_Candidate->save(ss.str().c_str(), m_it_Point_Normal);
}
m_extractedPrimitives.push_back(best_Candidate);
}
std::vector<int> subsetSizes(m_num_subsets);
subsetSizes[0] = m_availableOctreeSizes[0];
for (unsigned int i = 1;i<m_num_subsets;i++) {
subsetSizes[i] = subsetSizes[i-1] + m_availableOctreeSizes[i];
}
//5. Remove points from candidates common with extracted primitive
#pragma omp parallel for
for (int i=0;i< l_list_candidates.size()-1;i++) //-1 because we keep BestCandidate
{
if (l_list_candidates[i]) {
l_list_candidates[i]->updatePoints(m_shapeIndex);
if (l_list_candidates[i]->score() < m_options.m_minNbPoints) {
delete l_list_candidates[i];
l_list_candidates[i] = NULL;
}
else l_list_candidates[i]->computeBound(subsetSizes[l_list_candidates[i]->m_nb_subset_used - 1], m_numAvailablePoints - numInvalid);
}
}
int start = 0, end = l_list_candidates.size() - 1;
while (start < end) {
while (l_list_candidates[start] && start < end) start++;
while (!l_list_candidates[end] && start < end) end--;
if (!l_list_candidates[start] && l_list_candidates[end] && start < end) {
l_list_candidates[start] = l_list_candidates[end];
l_list_candidates[end] = NULL;
start++;
end--;
}
}
l_list_candidates.resize(end);
}
}
while(
StopProbability(m_options.m_minNbPoints, m_numAvailablePoints - numInvalid, nbNewCandidates /*l_list_candidates.size()*/, scm_max_depth_octree) > m_options.m_probability
&& FT(m_numAvailablePoints - numInvalid) >= m_options.m_minNbPoints
);
std::cout << "run Ransac done" << std::endl;
m_numAvailablePoints -= numInvalid;
};
}
}
#endif
Reference in New Issue View Git Blame Copy Permalink