void PointKDTree::rangeQuery(AxisAlignedBox3 const & query, std::vector<Point *> & points_in_range) const { points_in_range.clear(); // Write a helper function rangeQuery(node, query, points_in_range): // - If node->bbox does not intersect query, return // - If node->lo && node->lo->bbox intersects query, rangeQuery(node->lo, query, points_in_range) // - If node->hi && node->hi->bbox intersects query, rangeQuery(node->hi, query, points_in_range) if(!(root->bbox).intersects(query)) return; if((root->points).size()!=0) { for(int i=0;i<(int)(root->points).size();i++) { Vector3 rootPoint = root->points[i]->getPosition(); if(query.contains(rootPoint)) points_in_range.push_back(root->points[i]); } return; } if((root->lo->bbox).intersects(query)) rangeQuery(root->lo, query, points_in_range); if((root->hi->bbox).intersects(query)) rangeQuery(root->hi, query, points_in_range); }
Node * rangeQuery(Node *v, double distance, double *pt, int D){ Node *current=NULL, *tmp; int j; if ( ( (v->pt[v->orientation] - distance) <= pt[v->orientation]) && ( (v->pt[v->orientation] + distance) >= pt[v->orientation]) ){ if (calcdistance(pt,v->pt,D) <= distance){ current=(Node*) malloc(sizeof(Node)); current->pt=(double*) malloc(sizeof(double)*D); current->right=NULL; for (j=0; j < D; j++) current->pt[j]=v->pt[j]; current->index=v->index; } } if ( (!(v->left)) && (!(v->right)) ) return current; tmp=current; while (tmp && tmp->right) tmp=tmp->right; if (v->left){ if ( v->pt[v->orientation] >= (pt[v->orientation] - distance ) ){ if (tmp) tmp->right=rangeQuery(v->left, distance, pt,D); else current=rangeQuery(v->left, distance, pt, D); } } tmp=current; while (tmp &&tmp->right) tmp=tmp->right; if (v->right){ if ( v->pt[v->orientation] <= (pt[v->orientation] + distance ) ){ if (tmp) tmp->right=rangeQuery(v->right, distance, pt, D); else current= rangeQuery(v->right, distance, pt, D); } } return current; }
vector<vector<pair<T_ID, uint32> > > InvertedIndexMulDim::rangeQueries(vector<vector<int> >& query, int range, uint32 threshold){ vector<vector<pair<T_ID, uint32> > > res; for(uint32 i=0;i<query.size();i++){ res.push_back(rangeQuery(query[i],range, threshold)); } return res; }
void PointKDTree::rangeQuery(Node *node, AxisAlignedBox3 const & query, std::vector<Point *> & points_in_range) const { if(!(node->bbox).intersects(query)) return; if(node->lo == NULL && node->hi == NULL && (node->points).size()!=0) { for(int i=0;i<(int)(node->points).size();i++) { Vector3 nodePoint = node->points[i]->getPosition(); if(query.contains(nodePoint)) points_in_range.push_back(node->points[i]); } return; } if((node->lo->bbox).intersects(query) && node->lo !=NULL) rangeQuery(node->lo, query, points_in_range); if((node->hi->bbox).intersects(query) && node->hi !=NULL) rangeQuery(node->hi, query, points_in_range); }
vector<pair<T_ID, uint32> > InvertedIndexMulDim::rangeQuery(vector<int>& query, int range, uint32 threshold){ vector<int> queryLow(query.size()); vector<int> queryHigh(query.size()); for(uint i=0;i<query.size();i++){ queryLow[i] = query[i] - range; queryHigh[i] = query[i] +range; } return rangeQuery(queryLow, queryHigh, threshold); }
// // The top-level function for running a range search on the k-D tree. // void run_range_search( Node *pVertex, double *model, int M, int D, double distlim, double **pts_in_range, unsigned int *L, unsigned int **indices){ int i,j, count; double min_distance; double *pt; Node *LL, *cur, *leaf, *tmp; pt= (double*)malloc(sizeof(double)*D); for (i=0; i < M; i++) { #ifdef DEBUG_RUN_RANGE_SEARCH mexPrintf("Running Search (%d/%d) (value: ", i+1, M); for (j=0; j < D ; j++) mexPrintf(" %f", model[ M*j+i]); mexPrintf(" )\n"); #endif for (j=0; j < D ; j++) pt[j]=model[M*j+i]; LL=rangeQuery(pVertex,distlim,pt,D); if (!LL) { *L=0; (*pts_in_range)=NULL; #ifdef DEBUG_RUN_RANGE_SEARCH mexPrintf("Null LL\n"); #endif } else { cur=LL; count=0; while (cur){ cur=cur->right; count++; } #ifdef DEBUG_RUN_RANGE_SEARCH mexPrintf("Found %d points\n",count); #endif *L=count; (*indices) = (unsigned int *) malloc (sizeof(unsigned int)*count); (*pts_in_range) = (double *) malloc (sizeof(double) *count*D); cur=LL; count=0; while(cur){ (*indices)[count] = cur->index; for (j=0; j < D; j++) { (*pts_in_range)[j*(*L)+count] = cur->pt[j]; } tmp=cur; cur=cur->right; free(tmp->pt); free(tmp); count++; } } } free(pt); }
// // The top-level function for running a query on the k-D tree. // void run_queries( Node *pVertex, double *model, int M, int D, double *closest_pt, double *distance, short ReturnType) { int i,j; double min_distance, *pt; Node *LL, *cur, *leaf, *tmp; pt= (double*)malloc(sizeof(double)*D); for (i=0; i < M; i++) { #ifdef DEBUG_RUN_QUERIES mexPrintf("Running Query (%d/%d) (value: ", i+1, M); for (j=0; j < D ; j++) mexPrintf(" %f", model[ M*j+i]); mexPrintf(" )\n"); #endif for (j=0; j < D; j++) pt[j]=model[M*j+i]; leaf=pointLocation(pVertex,pt,D); min_distance=calcdistance(leaf->pt, pt,D )+0.001; LL=rangeQuery(pVertex,min_distance,pt,D); if (!LL) { if (ReturnType == RETURN_INDEX) closest_pt[i] = -1; else{ for (j=0; j< D; j++) closest_pt[j*M+i]=-1; } mexPrintf("Null LL\n"); } else { distance[i]=calcdistance(LL->pt, pt,D); if (ReturnType == RETURN_INDEX) closest_pt[i] = LL->index; else { for (j=0; j < D; j++) closest_pt[j*M+i] = LL->pt[j]; } cur=LL; while (cur){ if ( calcdistance(cur->pt, pt,D) <= distance[i] ) { if (ReturnType == RETURN_INDEX) closest_pt[i] = cur->index; else { for (j=0; j < D; j++) closest_pt[j*M+i] = cur->pt[j]; } distance[i]=calcdistance(cur->pt, pt,D); } tmp=cur; cur=cur->right; free(tmp->pt); free(tmp); } } #ifdef DEBUG_RUN_QUERIES mexPrintf("Distance to closest point is %f\n",distance[i]); #endif } free(pt); }
MainApplication::MainApplication(QWidget *parent) : QWidget(parent) { const int textSize = 12; const int toolBoxWidth = 160; const int toolBoxWidgetsWidth = 140; const int toolBoxSubWidgetsWidth = 120; QSize textEditSize = QSize(40, 30); int windowHeight = 500; int windowWidth = 800; /*---- Buttons ----*/ QPushButton *rangeQueryButton = new QPushButton(tr("Range")); rangeQueryButton->setFont(QFont("Times", textSize, QFont::AnyStyle)); QPushButton *radiusQueryButton = new QPushButton(tr("Radius")); radiusQueryButton->setFont(QFont("Times", textSize, QFont::AnyStyle)); QPushButton *loadButton = new QPushButton(tr("Load")); loadButton->setFont(QFont("Times", textSize, QFont::AnyStyle)); QPushButton *nnQueryButton = new QPushButton(tr("NN-Query")); nnQueryButton->setFont(QFont("Times", textSize, QFont::AnyStyle)); QPushButton *smoothingButton = new QPushButton(tr("Smooth")); smoothingButton->setFont(QFont("Times", textSize, QFont::AnyStyle)); QPushButton *distanceColorMapButton = new QPushButton(tr("ColorbyDist")); distanceColorMapButton->setFont(QFont("Times", textSize, QFont::AnyStyle)); QPushButton *thinningButton = new QPushButton(tr("Thinning")); thinningButton->setFont(QFont("Times", textSize, QFont::AnyStyle)); QPushButton *lineFittingButton = new QPushButton(tr("fit Line")); lineFittingButton->setFont(QFont("Times", textSize, QFont::AnyStyle)); QPushButton *planeFittingButton = new QPushButton(tr("fit Plane")); planeFittingButton->setFont(QFont("Times", textSize, QFont::AnyStyle)); QPushButton *sphereFittingButton = new QPushButton(tr("fit Sphere")); sphereFittingButton->setFont(QFont("Times", textSize, QFont::AnyStyle)); connect(loadButton, SIGNAL(clicked()), this, SLOT(loadPoints())); connect(rangeQueryButton, SIGNAL(clicked()), this, SLOT(rangeQuery())); connect(radiusQueryButton, SIGNAL(clicked()), this, SLOT(radiusQuery())); connect(smoothingButton, SIGNAL(clicked()), this, SLOT(smoothPointCloud())); connect(nnQueryButton, SIGNAL(clicked()), this, SLOT(nnQuery())); connect(distanceColorMapButton, SIGNAL(clicked()), this, SLOT(colorPointsByDistance())); connect(thinningButton, SIGNAL(clicked()), this, SLOT(applyThinning())); connect(lineFittingButton, SIGNAL(clicked()), this, SLOT(fitLine())); connect(planeFittingButton, SIGNAL(clicked()), this, SLOT(fitPlane())); connect(sphereFittingButton, SIGNAL(clicked()), this, SLOT(fitSphere())); /*---- Labels ----*/ labelCloudBounds = new QLabel("---", this); labelCloudBounds->setMaximumHeight(60); labelPoints = new QLabel("---", this); labelPoints->setMaximumHeight(60); labelTime = new QLabel("---", this); labelTime->setMaximumHeight(60); labelFitting = new QLabel("p: dir:", this); labelFitting->setMaximumHeight(120); /*---- Text Edits ----*/ QDoubleValidator *validDouble = new QDoubleValidator(); minXRange = new QLineEdit(); minXRange->setMaximumSize(textEditSize); minXRange->setValidator(validDouble); maxXRange = new QLineEdit(); maxXRange->setMaximumSize(textEditSize); maxXRange->setValidator(validDouble); minYRange = new QLineEdit(); minYRange->setMaximumSize(textEditSize); minYRange->setValidator(validDouble); maxYRange = new QLineEdit(); maxYRange->setMaximumSize(textEditSize); maxYRange->setValidator(validDouble); minZRange = new QLineEdit(); minZRange->setMaximumSize(textEditSize); minZRange->setValidator(validDouble); maxZRange = new QLineEdit(); maxZRange->setMaximumSize(textEditSize); maxZRange->setValidator(validDouble); xRadius = new QLineEdit(); xRadius->setMaximumSize(textEditSize); xRadius->setValidator(validDouble); yRadius = new QLineEdit(); yRadius->setMaximumSize(textEditSize); yRadius->setValidator(validDouble); zRadius = new QLineEdit(); zRadius->setMaximumSize(textEditSize); zRadius->setValidator(validDouble); rRadius = new QLineEdit(); rRadius->setMaximumSize(textEditSize); rRadius->setValidator(validDouble); xNeighbour = new QLineEdit(); xNeighbour->setMaximumSize(textEditSize); xNeighbour->setValidator(validDouble); yNeighbour = new QLineEdit(); yNeighbour->setMaximumSize(textEditSize); yNeighbour->setValidator(validDouble); zNeighbour = new QLineEdit(); zNeighbour->setMaximumSize(textEditSize); zNeighbour->setValidator(validDouble); rSmoothing = new QLineEdit(); rSmoothing->setMaximumSize(textEditSize); rSmoothing->setMaximumWidth(toolBoxSubWidgetsWidth); rSmoothing->setValidator(validDouble); rThinning = new QLineEdit(); rThinning->setMaximumSize(textEditSize); rThinning->setMaximumWidth(toolBoxSubWidgetsWidth); rThinning->setValidator(validDouble); /*---- Tool Box and Tool Box Widgets ----*/ QToolBox *toolBox = new QToolBox(); //Load QVBoxLayout *layoutLoad = new QVBoxLayout(); layoutLoad->addWidget(loadButton); QWidget* LoadWidget = new QWidget(); LoadWidget->setLayout(layoutLoad); LoadWidget->setFixedWidth(toolBoxWidgetsWidth); toolBox->addItem(LoadWidget, "Load Data"); // Range Query QGridLayout *layoutRangeTextEdits = new QGridLayout(); layoutRangeTextEdits->addWidget(minXRange,0,0,0); layoutRangeTextEdits->addWidget(maxXRange,0,1,0); layoutRangeTextEdits->addWidget(minYRange,1,0,0); layoutRangeTextEdits->addWidget(maxYRange,1,1,0); layoutRangeTextEdits->addWidget(minZRange,2,0,0); layoutRangeTextEdits->addWidget(maxZRange,2,1,0); QWidget* RangeTextEditsWidget = new QWidget(); RangeTextEditsWidget->setLayout(layoutRangeTextEdits); RangeTextEditsWidget->setFixedWidth(toolBoxSubWidgetsWidth); QVBoxLayout *layoutRange = new QVBoxLayout(); layoutRange->addWidget(RangeTextEditsWidget); layoutRange->addWidget(rangeQueryButton); QWidget* RangeWidget = new QWidget(); RangeWidget->setLayout(layoutRange); RangeWidget->setFixedWidth(toolBoxWidgetsWidth); toolBox->addItem(RangeWidget, "Range Query"); // Radius Query QGridLayout *layoutRadiusTextEdits = new QGridLayout(); layoutRadiusTextEdits->addWidget(xRadius, 0, 0, 0); layoutRadiusTextEdits->addWidget(yRadius, 0, 1, 0); layoutRadiusTextEdits->addWidget(zRadius, 0, 3, 0); layoutRadiusTextEdits->addWidget(rRadius, 1, 1, 0); QWidget* RadiusTextEditsWidget = new QWidget(); RadiusTextEditsWidget->setLayout(layoutRadiusTextEdits); RadiusTextEditsWidget->setFixedWidth(toolBoxSubWidgetsWidth); QVBoxLayout *layoutRadius = new QVBoxLayout(); layoutRadius->addWidget(RadiusTextEditsWidget); layoutRadius->addWidget(radiusQueryButton); QWidget* RadiusWidget = new QWidget(); RadiusWidget->setLayout(layoutRadius); RadiusWidget->setFixedWidth(toolBoxWidgetsWidth); toolBox->addItem(RadiusWidget, "Radius Query"); // NN Query QGridLayout *layoutNNTextEdits = new QGridLayout(); layoutNNTextEdits->addWidget(xNeighbour, 0, 0, 0); layoutNNTextEdits->addWidget(yNeighbour, 0, 1, 0); layoutNNTextEdits->addWidget(zNeighbour, 0, 3, 0); QWidget* NNTextEditsWidget = new QWidget(); NNTextEditsWidget->setLayout(layoutNNTextEdits); NNTextEditsWidget->setFixedWidth(toolBoxSubWidgetsWidth); QVBoxLayout *layoutNN = new QVBoxLayout(); layoutNN->addWidget(NNTextEditsWidget); layoutNN->addWidget(nnQueryButton); QWidget* NNWidget = new QWidget(); NNWidget->setLayout(layoutNN); NNWidget->setFixedWidth(toolBoxWidgetsWidth); toolBox->addItem(NNWidget, "Nearest Neighbour"); // Thinning QVBoxLayout *layoutThinning = new QVBoxLayout(); layoutThinning->addWidget(rThinning); layoutThinning->addWidget(thinningButton); QWidget* ThinningWidget = new QWidget(); ThinningWidget->setLayout(layoutThinning); ThinningWidget->setFixedWidth(toolBoxWidgetsWidth); toolBox->addItem(ThinningWidget, "Thinning"); // Smoothing QVBoxLayout *layoutSmoothing = new QVBoxLayout(); layoutSmoothing->addWidget(rSmoothing); layoutSmoothing->addWidget(smoothingButton); QWidget* SmoothingWidget = new QWidget(); SmoothingWidget->setLayout(layoutSmoothing); SmoothingWidget->setFixedWidth(toolBoxWidgetsWidth); toolBox->addItem(SmoothingWidget, "Smoothing"); // Fitting QVBoxLayout *layoutFitting = new QVBoxLayout(); layoutFitting->addWidget(labelFitting); layoutFitting->addWidget(planeFittingButton); layoutFitting->addWidget(lineFittingButton); layoutFitting->addWidget(sphereFittingButton); QWidget* FittingWidget = new QWidget(); FittingWidget->setLayout(layoutFitting); FittingWidget->setFixedWidth(toolBoxWidgetsWidth); toolBox->addItem(FittingWidget, "Fitting"); // Color QVBoxLayout *layoutColorByDist = new QVBoxLayout(); layoutColorByDist->addWidget(distanceColorMapButton); QWidget* ColorByDistWidget = new QWidget(); ColorByDistWidget->setLayout(layoutColorByDist); ColorByDistWidget->setFixedWidth(toolBoxWidgetsWidth); toolBox->addItem(ColorByDistWidget, "Color by Distance"); /*---- Data Group Box ----*/ QGroupBox *dataBox = new QGroupBox(tr("Data")); QVBoxLayout *layoutDataBox = new QVBoxLayout; layoutDataBox->addWidget(labelPoints); layoutDataBox->addWidget(labelCloudBounds); dataBox->setLayout(layoutDataBox); /*---- Side Bar ----*/ QVBoxLayout *layoutSideBar = new QVBoxLayout(); layoutSideBar->addWidget(dataBox); layoutSideBar->addWidget(toolBox); layoutSideBar->addWidget(labelTime); QWidget* sideBarWidget = new QWidget(); sideBarWidget->setLayout(layoutSideBar); sideBarWidget->setFixedWidth(toolBoxWidth); /*---- Main Widget ----*/ glWidget = new MainGLWidget(); glWidget->resize(windowWidth, windowHeight); glWidget->setMinimumWidth(windowWidth); glWidget->setMinimumHeight(windowHeight); QHBoxLayout *layoutMain = new QHBoxLayout(); layoutMain->addWidget(glWidget); layoutMain->addWidget(sideBarWidget); setLayout(layoutMain); }