double estimateMI_ANN( const RV_1<P_V,P_M>& xRV,
const RV_2<P_V,P_M>& yRV,
const unsigned int xDimSel[], unsigned int dimX,
const unsigned int yDimSel[], unsigned int dimY,
unsigned int k, unsigned int N, double eps )
{
ANNpointArray dataXY;
double MI_est;
unsigned int dimXY = dimX + dimY;
// Allocate memory
dataXY = annAllocPts(N,dimXY);
// Copy samples in ANN data structure
P_V smpRV_x( xRV.imageSet().vectorSpace().zeroVector() );
P_V smpRV_y( yRV.imageSet().vectorSpace().zeroVector() );
for( unsigned int i = 0; i < N; i++ ) {
// get a sample from the distribution
xRV.realizer().realization( smpRV_x );
yRV.realizer().realization( smpRV_y );
// copy the vector values in the ANN data structure
for( unsigned int j = 0; j < dimX; j++ ) {
dataXY[ i ][ j ] = smpRV_x[ xDimSel[j] ];
}
for( unsigned int j = 0; j < dimY; j++ ) {
dataXY[ i ][ dimX + j ] = smpRV_y[ yDimSel[j] ];
}
// annPrintPt( dataXY[i], dimXY, std::cout ); std::cout << std::endl;
}
MI_est = computeMI_ANN( dataXY,
dimX, dimY,
k, N, eps );
// Deallocate memory
annDeallocPts( dataXY );
return MI_est;
}
void Slave::operator()() {
ANNpointArray child_centers = annAllocPts(Globals::pow2dim, Globals::dim);
int it;
for (int i=0; i<Globals::pow2dim; i++) {
it = 1;
for (int j=0; j<Globals::dim; j++) {
if ((i&it) != 0) {
child_centers[i][j] = 0.75;
} else {
child_centers[i][j] = 0.25;
}
it = it << 1;
}
if (i%Globals::threads_num == id) {
//TODO make this dynamically allocate pieces to threads
std::cout << "Thread " << id << " gets branch " << i << std::endl;
fil_in_reprs(Globals::qb->children[0]+i,&child_centers[i],0.5);
}
}
annDeallocPts(child_centers);
}
//process all that shit in vectors.... very gay
void fill_vectors(QNode* qb, ANNpoint* center, double side) {
if (qb->children == 0) {
Globals::qt_nodes.push_back(qb);
Globals::qt_centers.push_back(center);
Globals::qt_sides.push_back(side);
//std::cout << side << std::endl;
} else {
int it;
ANNpointArray child_centers = annAllocPts(Globals::pow2dim, Globals::dim);
for (int i=0; i<Globals::pow2dim; i++) {
it = 1;
for (int j=0; j<Globals::dim; j++) {
if ((i&it) != 0) {
child_centers[i][j] = (*center)[j]+(side/4.0);
} else {
child_centers[i][j] = (*center)[j]-(side/4.0);
}
it = it << 1;
}
fill_vectors(qb->children[0]+i,&child_centers[i],side/2.0);
}
annDeallocPts(child_centers);
}
}
arlCore::ICP::ICP( arlCore::PointList::csptr model, arlCore::PointList::csptr cloud, bool justVisible ):
m_point2PointMode(true),
m_modelMesh(),
m_cloud(),
m_initialization(false),
m_justVisible(justVisible),
m_modelSize(0),
m_cloudSize(0),
m_modelPoints(0),
m_cloudPoints(0),
m_Pk(0),
m_Yk(0),
m_Pi(0),
m_ANNtree(0),
m_nn_idx(0),
m_squaredDists(0),
m_maxIterations(50),
m_nbIterations(0),
m_startError(-1),
m_endError(-1)
{
m_solution.setIdentity();
#ifdef ANN
const unsigned int m_dimension = model->getDimension();
assert(m_dimension==cloud->getDimension());
m_modelGravity.set_size(m_dimension);
m_cloudGravity.set_size(m_dimension);
unsigned int i, j, n;
if(justVisible) m_modelSize = model->visibleSize();
else m_modelSize = model->size();
if(m_modelSize<1) return;
m_modelPoints = annAllocPts( m_modelSize, m_dimension );
for( i=0, n=0 ; i<m_modelSize ; ++i )
if((*model)[i])
if(!justVisible || (justVisible && (*model)[i]->isVisible()))
{
for( j=0 ; j<m_dimension ; ++j )
m_modelPoints[n][j]=(*model)[i]->get(j);
++n;
}
m_modelSize = n;
if(justVisible) m_cloudSize = cloud->visibleSize();
else m_cloudSize = cloud->size();
if(m_modelSize<1 || m_cloudSize<1)
{
annDeallocPts( m_modelPoints );
m_modelPoints = 0;
return;
}
m_cloudPoints = annAllocPts( m_cloudSize, m_dimension );
for( i=0, n=0 ; i<m_cloudSize ; ++i )
if((*cloud)[i])
if(!justVisible || (justVisible && (*cloud)[i]->isVisible()))
{
for( j=0 ; j<m_dimension ; ++j )
m_cloudPoints[n][j]=(*cloud)[i]->get(j);
++n;
}
m_cloudSize = n;
if(m_cloudSize<1)
{
annDeallocPts( m_modelPoints );
annDeallocPts( m_cloudPoints );
m_modelPoints = 0;
m_cloudPoints = 0;
return;
}
m_Pk = annAllocPts( m_cloudSize, m_dimension );
m_Yk = annAllocPts( m_cloudSize, m_dimension );
m_Pi = annAllocPts( m_cloudSize, m_dimension );
const int BucketSize = 1;
m_ANNtree = new ANNkd_tree( m_modelPoints, m_modelSize, m_dimension, BucketSize, ANN_KD_SL_MIDPT );
m_nbNN = 1;
m_nn_idx = new ANNidx[m_nbNN];
m_squaredDists = new ANNdist[m_nbNN];
#endif // ANN
}
void ANNCluster::doCluster(mat_f& data, grpEle_vect& grpVect, float radius) {
//
int ptCnt = data.size().height;
int dim = data.size().width;
//this is ANN implementation, but it is changed to FLANN now, please refer the updated block below
float *ptWeight = new float[ptCnt];
std::vector<bool> mFlag(ptCnt,false);
#ifdef __USE_ANN__
ANNpointArray ptArray = annAllocPts(ptCnt, dim);
//assign point values
for (int i=0; i<ptCnt; i++) {
ANNpoint ptPtr = ptArray[i];
for(int j = 0; j < dim; j++){
float tmp = data(i,j);
ptPtr[j] = data(i , j);
}
}
///
ANNkd_tree kdt(ptArray, ptCnt, dim);
ANNpoint queryPt = annAllocPt(dim);
ANNidxArray nnIdx = new ANNidx[ptCnt];
ANNdistArray nnDist = new ANNdist[ptCnt];
int grpCounter = 0;
for (int i=0; i<ptCnt; i++) {
if(mFlag[i])
continue; //skip matched point
for(int j = 0; j < dim; j++){
queryPt[j] = data(i,j);
}
int resultCnt = kdt.annkFRSearch(queryPt, radius, ptCnt, nnIdx, nnDist);
std::vector<cv::Vec3f> nnPt;
GrpEle ge;
ge.m_gID = grpCounter++ ;
for (int j=0; j < resultCnt; j++) {
ANNidx idx = nnIdx[j];
mFlag[idx] = true;
ANNpoint tmpNNPt = ptArray[idx];
cv::Vec3f cvPt(tmpNNPt[0], tmpNNPt[1], tmpNNPt[2]);
ptWeight[j] = exp(-nnDist[j]/radius);
ge.m_eMember.push_back(Element(idx,ptWeight[j]));
}
grpVect.push_back(ge);
}
annDeallocPt(queryPt);
annDeallocPts(ptArray);
delete [] nnDist;
delete [] nnIdx;
delete [] ptWeight;
#else
cv::flann::KDTreeIndexParams idxParams(4);
cv::flann::SearchParams scParams(32);
cv::flann::Index indexer(data,idxParams);
std::vector<float> nnDists(dim);
std::vector<int> nnInds(dim);
std::vector<float> queryPt(dim);
for(int i = 0; i < ptCnt; i++){
if(mFlag[i])
continue;
for(int j = 0; j < dim; j++)
queryPt[j] = data(i,j);
int nnCnt = indexer.radiusSearch(queryPt,nnInds,nnDists,radius,scParams);
GrpEle ge;
for(int k = 0; k < nnCnt; k++){
int tmpIdx = nnInds[k];
float tmpDist = nnDists[k];
float tmpWeight = exp(-tmpDist/radius);
ge.m_eMember.push_back(Element(tmpIdx,tmpWeight));
}
grpVect.push_back(ge);
}
#endif
}
void carveHole(GRegion *gr, int num, double distance, std::vector<int> &surfaces)
{
Msg::Info("Carving hole %d from surface %d at distance %g", num, surfaces[0], distance);
GModel *m = gr->model();
// add all points from carving surfaces into kdtree
int numnodes = 0;
for(unsigned int i = 0; i < surfaces.size(); i++){
GFace *gf = m->getFaceByTag(surfaces[i]);
if(!gf){
Msg::Error("Unknown carving surface %d", surfaces[i]);
return;
}
numnodes += gf->mesh_vertices.size();
}
ANNpointArray kdnodes = annAllocPts(numnodes, 3);
int k = 0;
for(unsigned int i = 0; i < surfaces.size(); i++){
GFace *gf = m->getFaceByTag(surfaces[i]);
for(unsigned int j = 0; j < gf->mesh_vertices.size(); j++){
kdnodes[k][0] = gf->mesh_vertices[j]->x();
kdnodes[k][1] = gf->mesh_vertices[j]->y();
kdnodes[k][2] = gf->mesh_vertices[j]->z();
k++;
}
}
ANNkd_tree *kdtree = new ANNkd_tree(kdnodes, numnodes, 3);
// remove the volume elements that are within 'distance' of the
// carved surface
carveHole(gr->tetrahedra, distance, kdtree);
carveHole(gr->hexahedra, distance, kdtree);
carveHole(gr->prisms, distance, kdtree);
carveHole(gr->pyramids, distance, kdtree);
delete kdtree;
annDeallocPts(kdnodes);
// TODO: remove any interior elements left inside the carved surface
// (could shoot a line from each element's barycenter and count
// intersections o see who's inside)
// generate discrete boundary mesh of the carved hole
GFace *gf = m->getFaceByTag(num);
if(!gf) return;
std::set<MFace, Less_Face> faces;
std::list<GFace*> f = gr->faces();
for(std::list<GFace*>::iterator it = f.begin(); it != f.end(); it++){
addFaces((*it)->triangles, faces);
addFaces((*it)->quadrangles, faces);
}
addFaces(gr->tetrahedra, faces);
addFaces(gr->hexahedra, faces);
addFaces(gr->prisms, faces);
addFaces(gr->pyramids, faces);
std::set<MVertex*> verts;
for(std::set<MFace, Less_Face>::iterator it = faces.begin(); it != faces.end(); it++){
for(int i = 0; i < it->getNumVertices(); i++){
it->getVertex(i)->setEntity(gf);
verts.insert(it->getVertex(i));
}
if(it->getNumVertices() == 3)
gf->triangles.push_back(new MTriangle(it->getVertex(0), it->getVertex(1),
it->getVertex(2)));
else if(it->getNumVertices() == 4)
gf->quadrangles.push_back(new MQuadrangle(it->getVertex(0), it->getVertex(1),
it->getVertex(2), it->getVertex(3)));
}
}
// cd L:\ann_mwrapper; Untitled2
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
mwSize dims[3];
char command[1024];
int strlen = mxGetN(prhs[0]);
strlen = strlen < mxGetM(prhs[0]) ? mxGetN(prhs[0]) : strlen;
strlen += 1;
mxGetString (prhs[0], command, strlen);
// Initialize tree
if (nrhs > 0 && !strcmp(command, "createKdTree")) {
const mxArray *mxRefPts = prhs[1];
const mxArray *mxOpts = prhs[2];
// parse options
AnnBuildOptions opts_build;
parse_tree_building_options(mxOpts, opts_build);
// construct the tree
int d = 0;
int n = 0;
ANNpointArray *pts = createPointArray(mxRefPts, d, n);
ANNkd_tree *kd_tree = createKdTree(d, n, pts[0], opts_build);
//mexPrintf("%lld\n", kd_tree);
//mexPrintf("dim = %d\n", kd_tree->theDim());
dims[0] = 2;
dims[1] = 1;
plhs[0] = mxCreateNumericArray(2,dims,mxUINT64_CLASS,mxREAL);
((unsigned long long *)(mxGetData( plhs[0])))[0] = (unsigned long long)kd_tree;
((unsigned long long *)(mxGetData( plhs[0])))[1] = (unsigned long long)pts;
// mexPrintf("Creating:\n");
// mexPrintf("Pts: ");
// for (int i =0;i<MEX_DEBUG_WND;++i){
// mexPrintf("%x",pts[i]);
// }
// mexPrintf("\n");
// mexPrintf("kdTree: ");
// for (int i =0;i<10;++i){
// mexPrintf("%x",((char *)kd_tree)[i]);
// }
// mexPrintf("\n");
}
// Search
else if (nrhs > 0 && !strcmp(command, "performAnnkSearch")) {
// take inputs
const mxArray *mxQuery = prhs[2];
const mxArray *mxOpts = prhs[3];
// parse options
AnnSearchOptions opts_search;
parse_search_options(mxOpts, opts_search);
// get pointer to tree
ANNkd_tree *kd_tree = (ANNkd_tree *)(((unsigned long long *)mxGetData( prhs[1]))[0]);
// mexPrintf("%lld\n", kd_tree);
// mexPrintf("dim = %d\n", kd_tree->theDim());
// perform the search
mxArray *mxInds = NULL;
mxArray *mxDists = NULL;
// mexPrintf("Pts: ");
// for (int i =0;i<MEX_DEBUG_WND;++i){
// mexPrintf("%x",pts[i]);
// }
// mexPrintf("\n");
// mexPrintf("kdTree: ");
// for (int i =0;i<10;++i){
// mexPrintf("%x",((char *)kd_tree)[i]);
// }
// mexPrintf("\n");
//
performAnnkSearch(kd_tree, mxQuery, opts_search, mxInds, mxDists);
// set outputs
plhs[0] = mxInds;
plhs[1] = mxDists;
}
// Deinitialize tree
else if (nrhs > 0 && !strcmp(command, "deleteKdTree")) {
ANNkd_tree *kd_tree = (ANNkd_tree *)(((unsigned long long *)mxGetData( prhs[1]))[0]);
ANNpointArray *pts = (ANNpointArray *)(((unsigned long long *)mxGetData( prhs[1]))[1]);
// mexPrintf("Deleting:\n");
// mexPrintf("Pts: ");
// for (int i =0;i<MEX_DEBUG_WND;++i){
// mexPrintf("%x",pts[i]);
// }
// mexPrintf("\n");
// mexPrintf("kdTree: ");
// for (int i =0;i<10;++i){
// mexPrintf("%x",((char *)kd_tree)[i]);
// }
// mexPrintf("\n");
// release the kd-tree
delete kd_tree;
annDeallocPts(pts[0]);
delete pts;
}
// Close
else if (nrhs > 0 && !strcmp(command, "annClose")) {
annClose();
}
else {
mexPrintf("INVALID COMMAND %s\n", command);
return;
}
}
double estimateCE_ANN( RV_1<P_V,P_M>& xRV,
RV_2<P_V,P_M>& yRV,
unsigned int xDimSel[], unsigned int dimX,
unsigned int yDimSel[], unsigned int dimY,
unsigned int xN, unsigned int yN,
unsigned int k, double eps )
{
ANNpointArray dataX;
ANNpointArray dataY;
double* distsXY;
double CE_est;
ANNkd_tree* kdTree;
// sanity check
if( dimX != dimY ) {
queso_error_msg("Error-CE: the dimensions should agree");
}
// Allocate memory
dataX = annAllocPts( xN, dimX );
dataY = annAllocPts( yN, dimY );
distsXY = new double[xN];
kdTree = new ANNkd_tree( dataY, yN, dimY );
// Copy X samples in ANN data structure
P_V xSmpRV( xRV.imageSet().vectorSpace().zeroVector() );
for( unsigned int i = 0; i < xN; i++ ) {
// get a sample from the distribution
xRV.realizer().realization( xSmpRV );
// copy the vector values in the ANN data structure
for( unsigned int j = 0; j < dimX; j++ ) {
dataX[ i ][ j ] = xSmpRV[ xDimSel[j] ];
}
}
// Copy Y samples in ANN data structure
P_V ySmpRV( yRV.imageSet().vectorSpace().zeroVector() );
for( unsigned int i = 0; i < yN; i++ ) {
// get a sample from the distribution
yRV.realizer().realization( ySmpRV );
// copy the vector values in the ANN data structure
for( unsigned int j = 0; j < dimY; j++ ) {
dataY[ i ][ j ] = ySmpRV[ yDimSel[j] ];
}
}
// Get distance to knn for each point
distANN_XY( dataX, dataY, distsXY, dimX, dimY, xN, yN, k, eps );
kdTree = new ANNkd_tree( dataY, yN, dimY );
// Compute cross entropy estimate
double sum_log = 0.0;
for( unsigned int i = 0; i < xN; i++ )
{
sum_log += log( distsXY[i] );
}
CE_est = (double)dimX/(double)xN * sum_log + log( (double)yN ) - gsl_sf_psi_int( k );
// Deallocate memory
annDeallocPts( dataX );
annDeallocPts( dataY );
delete [] distsXY;
return CE_est;
}
double estimateKL_ANN( RV_1<P_V,P_M>& xRV,
RV_2<P_V,P_M>& yRV,
unsigned int xDimSel[], unsigned int dimX,
unsigned int yDimSel[], unsigned int dimY,
unsigned int xN, unsigned int yN,
unsigned int k, double eps )
{
ANNpointArray dataX;
ANNpointArray dataY;
double* distsX;
double* distsXY;
double KL_est;
// sanity check
if( dimX != dimY ) {
queso_error_msg("Error-KL: the dimensions should agree");
}
// Allocate memory
dataX = annAllocPts( xN, dimX );
dataY = annAllocPts( yN, dimY );
distsX = new double[xN];
distsXY = new double[xN];
// Copy X samples in ANN data structure
P_V xSmpRV( xRV.imageSet().vectorSpace().zeroVector() );
for( unsigned int i = 0; i < xN; i++ ) {
// get a sample from the distribution
xRV.realizer().realization( xSmpRV );
// copy the vector values in the ANN data structure
for( unsigned int j = 0; j < dimX; j++ ) {
dataX[ i ][ j ] = xSmpRV[ xDimSel[j] ];
}
}
// Copy Y samples in ANN data structure
P_V ySmpRV( yRV.imageSet().vectorSpace().zeroVector() );
for( unsigned int i = 0; i < yN; i++ ) {
// get a sample from the distribution
yRV.realizer().realization( ySmpRV );
// copy the vector values in the ANN data structure
for( unsigned int j = 0; j < dimY; j++ ) {
dataY[ i ][ j ] = ySmpRV[ yDimSel[j] ];
}
}
// Get distance to knn for each point
distANN_XY( dataX, dataX, distsX, dimX, dimX, xN, xN, k+1, eps ); // k+1 because the 1st nn is itself
distANN_XY( dataX, dataY, distsXY, dimX, dimY, xN, yN, k, eps );
// Compute KL-divergence estimate
double sum_log_ratio = 0.0;
for( unsigned int i = 0; i < xN; i++ )
{
sum_log_ratio += log( distsXY[i] / distsX[i] );
}
KL_est = (double)dimX/(double)xN * sum_log_ratio + log( (double)yN / ((double)xN-1.0 ) );
// Deallocate memory
annDeallocPts( dataX );
annDeallocPts( dataY );
delete [] distsX;
delete [] distsXY;
return KL_est;
}
KNNClassifier::~KNNClassifier()
{
annDeallocPts(dataPts);
delete kdTree;
annClose();
}
// release the ANN point array allocated
void ReleaseANNPointArray(ANNpointArray pts)
{
annDeallocPts(pts);
}
myset* Slave::fil_in_reprs(QNode* qb, ANNpoint* center, double side) {
if ((qb->depth == 0) && (qb->children == 0)) {
//time to fill in the reprs my dear!
//if (qb->representatives != 0) {
//}
myset rs;
rs.insert(kdtree->ann1Search(*center,asdf));
// {boost::mutex::scoped_lock lock(thread_mutex);
if (Globals::final_set.count(rs) == 0) {
qb->representatives = new myset(rs);
Globals::final_set[rs] = qb->representatives;
} else {
qb->representatives = Globals::final_set[rs];
}
// }
return qb->representatives;
} else {
if (qb->depth != 0) {
qb->bear_children();
for (int i=0; i<Globals::pow2dim; i++) {
qb->children[0][i].depth = qb->depth-1;
}
qb->depth=0;
}
myset union_reprs;
int it;
//A local container to consume the result of our recursive calls
//std::vector<Globals::QLeaf*> new_children;
myset* new_children[Globals::pow2dim];
//recurse to children
ANNpointArray child_centers = annAllocPts(Globals::pow2dim, Globals::dim);
for (int i=0; i<Globals::pow2dim; i++) {
it = 1;
for (int j=0; j<Globals::dim; j++) {
if ((i&it) != 0) {
child_centers[i][j] = (*center)[j]+(side/4.0);
} else {
child_centers[i][j] = (*center)[j]-(side/4.0);
}
it = it << 1;
}
new_children[i] = fil_in_reprs(qb->children[0]+i,&child_centers[i],side/2.0);
}
annDeallocPts(child_centers);
leaves_count = 0;
//check every child and insert union all reprs
for (int i=0; i<Globals::pow2dim; i++) {
if (new_children[i] != 0) {
leaves_count++;
//std::set doesn't have a [] operator. Instead we use the iterators
for (myset::iterator it = (*new_children[i]).begin();
it != (*new_children[i]).end();
++it) {
union_reprs.insert(*it);
}
}
}
if ((leaves_count == Globals::pow2dim) && (union_reprs.size() <= Globals::reprs_num)) {
//Create the leaf and transfer the new set of representatives
//{boost::mutex::scoped_lock lock(thread_mutex);
if (Globals::final_set.count(union_reprs) == 0) {
//implies there are no reprs in this node.
Globals::final_set[union_reprs] = new myset(union_reprs);
}
qb->representatives = Globals::final_set[union_reprs];
//}
qb->remove_children();
return qb->representatives;
} else {
return 0;
}
}
}
//Should be a method inside Worker
myset* find_all_reprs(QNode* qb, ANNpoint* center, double side, ANNkd_tree* kdtree, ANNmin_k* asdf, int id) {
if (qb->children == 0) {
/* If the current node has no children, it's a leaf. Therefore we need
* to calculate a number of representatives and return a QLeaf_tmp
*/
if (qb->representatives == 0) {
//std::cout << "UNOREPRS" << std::endl;
//COPY
myset rs;
rs.insert(kdtree->ann1Search(*center,asdf));
if (Globals::sets[id].count(rs) == 0) {
qb->representatives = new myset(rs);
Globals::sets[id][rs] = qb->representatives;
} else {
qb->representatives = Globals::sets[id][rs];
}
//exit(0);
}
return qb->representatives;
} else {
/* This is an inner node. First we recurse to each leaf.
* Notice that we need to compute the coordinates of the centers
* of each child, since that information is not stored in QNode
*/
myset union_reprs;
int it;
//A local container to consume the result of our recursive calls
//std::vector<Globals::QLeaf*> new_children;
myset* new_children[Globals::pow2dim];
//Just a fancy array of arrays. We have pow2dim children and each one
//needs dim coordinates to express its center
ANNpointArray child_centers = annAllocPts(Globals::pow2dim, Globals::dim);
for (int i=0; i<Globals::pow2dim; i++) {
it = 1;
for (int j=0; j<Globals::dim; j++) {
if ((i&it) != 0) {
child_centers[i][j] = (*center)[j]+(side/4.0);
} else {
child_centers[i][j] = (*center)[j]-(side/4.0);
}
it = it << 1;
}
new_children[i] = find_all_reprs(qb->children[0]+i,&child_centers[i],side/2.0, kdtree, asdf, id);
}
annDeallocPts(child_centers);
//We now have a vector of size pow2dim, full of QNode and QLeaf_tmp
//Count the leaves and at the same time take the union of their
//representatives
leaves_count = 0;
//union_reprs.clear();
//check every child and insert union all reprs
for (int i=0; i<Globals::pow2dim; i++) {
if (new_children[i] != 0) {
leaves_count++;
//std::set doesn't have a [] operator. Instead we use the iterators
for (myset::iterator it = (*new_children[i]).begin();
it != (*new_children[i]).end();
++it) {
union_reprs.insert(*it);
}
}
}
//if all children are leaves and the union of all representatives is not
//bigger than max, I can merge all leaves into one and return that
if ((leaves_count == Globals::pow2dim) && (union_reprs.size() <= Globals::reprs_num)) {
//Create the leaf and transfer the new set of representatives
// if (qb->representatives !=0) {
// std::cout << "halp" << std::endl;
// }
if (Globals::sets[id].count(union_reprs) == 0) {
//implies there are no reprs in this node.
Globals::sets[id][union_reprs] = new myset(union_reprs);
}
qb->representatives = Globals::sets[id][union_reprs];
qb->remove_children();
return qb->representatives;
} else {
return 0;
}
}
}
void SymmetryDetection::detect_reflectional_symmetry(
const Eigen::MatrixXd &_sparse_points,
const Eigen::MatrixXd &_dense_points,
const double &_inlier_dist, const double &_inlier_ratio,
std::list<ReflectionPlane> &_reflection_planes)
{
const unsigned int dimension = 3;
assert(_sparse_points.rows() == dimension);
assert(_dense_points.rows() == dimension);
assert(_inlier_ratio > 0.0);
assert(_inlier_ratio < 1.0);
_reflection_planes.clear();
const unsigned int num_sparse_points = _sparse_points.cols();
const unsigned int num_dense_points = _dense_points.cols();
ANNpointArray sparse_ann_points;
ANNkd_tree* sparse_kd_tree = ICP::create_kd_tree(_sparse_points, sparse_ann_points);
const unsigned int num_iteration = 1000;
static SimpleRandomCong_t rng_cong;
// seed = num_iteration.
simplerandom_cong_seed(&rng_cong, num_iteration);
for (unsigned int iteration = 0; iteration < num_iteration; ++iteration)
{
double sparse_index_1 = simplerandom_cong_next(&rng_cong) % num_sparse_points;
double sparse_index_2 = simplerandom_cong_next(&rng_cong) % num_sparse_points;
Eigen::VectorXd sparse_point_1 = _sparse_points.col(sparse_index_1);
Eigen::VectorXd sparse_point_2 = _sparse_points.col(sparse_index_2);
ReflectionPlane reflection_plane;
reflection_plane.normal_ = sparse_point_2 - sparse_point_1;
reflection_plane.normal_.normalize();
reflection_plane.point_ = 0.5 * (sparse_point_1 + sparse_point_2);
Eigen::MatrixXd symmetric_sparse_points = get_symmetric_point(
reflection_plane.normal_, reflection_plane.point_, _sparse_points);
Eigen::VectorXd sparse_distances;
ICP::get_closest_points(sparse_kd_tree, symmetric_sparse_points, sparse_distances);
unsigned int num_sparse_inliers = (sparse_distances.array() <= _inlier_dist).count();
if (static_cast<double>(num_sparse_inliers) / num_sparse_points >= _inlier_ratio)
{
reflection_plane.inlier_indices_.clear();
for (unsigned int i = 0; i < num_sparse_points; ++i)
if (sparse_distances[i] < _inlier_dist)
reflection_plane.inlier_indices_.push_back(i);
_reflection_planes.push_back(reflection_plane);
std::cout << "Reflection plane detected [" << _reflection_planes.size() << "]:" << std::endl;
std::cout << " - # of inliers = (" << num_sparse_inliers << " / " << num_sparse_points << ")" << std::endl;
std::cout << " - Normal = (" << reflection_plane.normal_.transpose() << ")" << std::endl;
std::cout << " - Point = (" << reflection_plane.point_.transpose() << ")" << std::endl;
std::cout << std::endl;
}
}
annDeallocPts(sparse_ann_points);
delete sparse_kd_tree;
}
KMeans::~KMeans() {
if (kd_tree_)
delete kd_tree_;
if (ann_points_)
annDeallocPts( ann_points_ );
}
FeatureSearch::~FeatureSearch()
{
annDeallocPts( m_pointArray );
delete m_tree;
}
void NormalEstimator::fitNormal(){
//
//compute surface normal
int ptCnt = m_pt.size();
if (ptCnt <= 0)
{
std::cout << "no point set provided" << std::endl;
return;
}
ANNpointArray ptArray = annAllocPts(ptCnt, 3);
//assign point values
for (int i=0; i<ptCnt; i++) {
cv::Vec3f pt = m_pt[i];
ANNpoint ptPtr = ptArray[i];
ptPtr[0] = pt[0];
ptPtr[1] = pt[1];
ptPtr[2] = pt[2];
}
///
ANNkd_tree kdt(ptArray, ptCnt, 3);
ANNpoint queryPt = annAllocPt(3);
int nnCnt = 100;
ANNidxArray nnIdx = new ANNidx[nnCnt];
ANNdistArray nnDist = new ANNdist[nnCnt];
float sigma = -1;
float evalRatio = 0.05;
//estimate sigma
for (int i=0; i < ptCnt; i++) {
cv::Vec3f pt = m_pt[i];
queryPt[0] = pt[0];
queryPt[1] = pt[1];
queryPt[2] = pt[2];
//kdt.annkSearch(queryPt,nnCnt, nnIdx, nnDist);
kdt.annkSearch(queryPt,50, nnIdx, nnDist);
if (nnDist[49] < sigma ||sigma == -1 )
{
sigma = nnDist[49];
}
}
sigma = 0.001;
std::cout << "search radius:" << sigma << std::endl;
std::cout << "estimating normals for point set by PCA, be patient..." << std::endl;
for (int i=0; i < ptCnt; i++) {
cv::Vec3f pt = m_pt[i];
queryPt[0] = pt[0];
queryPt[1] = pt[1];
queryPt[2] = pt[2];
//kdt.annkSearch(queryPt,nnCnt, nnIdx, nnDist);
kdt.annkFRSearch(queryPt, sigma, nnCnt, nnIdx, nnDist);
int validCnt = 0;
for (int j = 0; j < nnCnt; j++)
{
if (nnIdx[j] == ANN_NULL_IDX)
{
break;
}
validCnt++;
}
//std::cout << validCnt << std::endl;
if (validCnt < 3)
{
continue;
}
cv::Mat pcaVec(validCnt,3,CV_64FC1);
cv::Mat pcaMean(1,3,CV_64FC1);
for (int j = 0; j < validCnt; j++)
{
pcaVec.at<double>(j,0) = m_pt[nnIdx[j]][0];
pcaVec.at<double>(j,1) = m_pt[nnIdx[j]][1];
pcaVec.at<double>(j,2) = m_pt[nnIdx[j]][2];
}
cv::PCA pca(pcaVec,cv::Mat(),CV_PCA_DATA_AS_ROW);
if (pca.eigenvalues.at<double>(2,0) / pca.eigenvalues.at<double>(1,0) > evalRatio)
{
continue;
}
m_ptNorm[i] = cv::Vec3f(pca.eigenvectors.at<double>(2,0),pca.eigenvectors.at<double>(2,1),pca.eigenvectors.at<double>(2,2));
float nr = cv::norm(m_ptNorm[i]);
m_ptNorm[i][0] /= nr;
m_ptNorm[i][1] /= nr;
m_ptNorm[i][2] /= nr;
//std::cout << m_ptNorm[i][0] << " " << m_ptNorm[i][1] << " " << m_ptNorm[i][2] << std::endl;
m_ptNormFlag[i] = true;
}
//
std::cout << "done..." << std::endl;
//////////////////////////////////////////////////////////////////////////
//std::cout << "correct normal direction..." << std::endl;
//sigma *= 1; //
//nnCnt *= 1; //
////reallocate the space for nn idx and nn dist array
//delete [] nnDist;
//delete [] nnIdx;
//nnIdx = new ANNidx[nnCnt];
//nnDist = new ANNdist[nnCnt];
//int invertCnt = 0;
//for (int i = 0; i < ptCnt; i++)
//{
// if (!m_ptNormFlag[i])
// {
// continue;
// }
//
// cv::Vec3f pt = m_pt[i];
// queryPt[0] = pt[0];
// queryPt[1] = pt[1];
// queryPt[2] = pt[2];
// kdt.annkFRSearch(queryPt, sigma, nnCnt, nnIdx, nnDist);
// int validCnt = 0, normConsCnt = 0, distConsCnt = 0;
// for (int j = 0; j < nnCnt; j++)
// {
// if (nnIdx[j] == ANN_NULL_IDX)
// {
// break;
// }
// else{
// //check the direction
// cv::Vec3f v1 = m_ptNorm[i];
// cv::Vec3f v2 = m_ptNorm[nnIdx[j]];
//
// if (!m_ptNormFlag[nnIdx[j]])
// {
// continue;
// }else{
// //
// validCnt++;
// if( v2.ddot(v1) > 0 )
// normConsCnt++;
// }
// }
// }
// //inconsistency detected, invert the direction
// if (normConsCnt / validCnt < 0.9)
// {
// //std::cout << "invert" << std::endl;
// invertCnt++;
// m_ptNorm[i] = cv::Vec3f(0,0,0) - m_ptNorm[i];
// }
//}
//std::cout << "# of inverted vertex:" << invertCnt << std::endl;
////////////////////////////////////////////////////////////////////////////
annDeallocPt(queryPt);
annDeallocPts(ptArray);
delete [] nnDist;
delete [] nnIdx;
}
void Worker::operator()() {
for (private_index=get_index(); private_index!=-1; private_index=get_index()) {
{boost::mutex::scoped_lock lock(io_mutex);
std::cout << "THREAD " << id << " GOT " << private_index+1 << "/" << Globals::wspd_centers.size() << std::endl;}
//Find the side length, radius, radius squared and center of circle/ring for
//each one of the circle/rings that I'm going to draw
for (int j=0; j<=Globals::wspd_circs_num[private_index]; j++) {
//1 as argument because I insert parents of leaves
//This means that when I do find a square with this side, I'm going to call
//bear_children method again. This essentially reduces processing time by 3/4
sides.push_back(side_discr(pow(2,j)*Globals::wspd_distances[private_index]/denominator,3));//1
// sides.push_back(side_discr(pow(2,j)*Globals::wspd_distances[private_index]/denominator,0));
//make radius an exact multiple of side
radiuses.push_back(radius_discr(pow(2,j)*Globals::wspd_distances[private_index], sides[j]));
radiuses_sqr.push_back(radiuses[j]*radiuses[j]);
//Center of circle should fall neatly on the center of a grid square
ANNpoint center = annAllocPt(Globals::dim);
for (int k=0; k<Globals::dim; k++) {
center[k] = center_discr(Globals::wspd_centers[private_index][k], sides[j], 0.5);
}
centers.push_back(center);
}
//first iteration is for the core, the inner circle which is completely filled
//the rest of the iteration are rings, essentially 2 concentric cirles where I fill
//the difference
core=true;
for (int circ=0; circ<=Globals::wspd_circs_num[private_index]; circ++) {
//Generate all the possible values of a coordinate, based on side and radius
//for example if radius is 8 and side is 2 this is going to be 0,2,4,6,8
for (ANNcoord v=0.0; v<=radiuses[circ]; v+=sides[circ]) {
all_values.push_back(v);
}
//cart_prod_counter=0;
cart_product(all_values, result, circ);
if (Globals::qbs[id]->children!=0)
depth_all_reprs(Globals::qbs[id]);
// find_all_reprs(Globals::qbs[id], &Globals::hypercube_center, 1.0, kdtree, asdf, id);
//CLEANUP
all_values.clear();
core=false;
}
//CLEANUP
radiuses.clear();
radiuses_sqr.clear();
sides.clear();
for (unsigned int d=0; d<centers.size(); d++)
annDeallocPt(centers[d]);
centers.clear();
}
std::cout << "THREAD " << id << " DIES" << std::endl;
annDeallocPt(qb_center);
annDeallocPts(leaf_centers);
delete[] leaf_reprs;
pos_neg_result.clear();
pos_neg_final.clear();
vd1.clear();
vd2.clear();
delete asdf;
delete kdtree;
// delete qb;
return;
}
