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);
}
