cl_int2 PointSmoother::SearchLinearDirection(cl_float4* pointCloud, const cl_int2& pixel, cl_float4& normal)
{
// direc.x, y <= MG
static const cl_int2 direc[] = {{2,0}, {2,2}, {0,2}, {-2,2}};
const int szdir = 4;
const cl_float4& herept = pointCloud[PIXIDX(pixel)];
const float distUppLimit = clDot(normal, herept)*0.002f;
float flatness, minDist = distUppLimit;
int minIndex = -1;
// search flattest direction
for(int i=0; i<szdir; i++)
{
const cl_float4& leftpt = pointCloud[IMGIDX(pixel.y-direc[i].y, pixel.x-direc[i].x)];
const cl_float4& righpt = pointCloud[IMGIDX(pixel.y+direc[i].y, pixel.x+direc[i].x)];
if(clIsNull(leftpt) || clIsNull(righpt))
continue;
flatness = fabs(clDot(normal, (leftpt - righpt)));
if(flatness < minDist)
{
minDist = flatness;
minIndex = i;
}
}
if(minIndex < 0)
return (cl_int2) {
0,0
};
else
return direc[minIndex];
}
void PointSmoother::SmoothePointCloud(cl_float4* pointCloud, cl_float4* normalCloud)
{
static cl_float4* pointBuffer = new cl_float4[IMAGE_WIDTH*IMAGE_HEIGHT];
for(int y=0+MG; y<IMAGE_HEIGHT-MG; y++)
for(int x=0+MG; x<IMAGE_WIDTH-MG; x++)
pointBuffer[IMGIDX(y,x)] = SmoothePoint(pointCloud, (cl_int2) {
x, y
}, normalCloud[IMGIDX(y,x)]);
memcpy(pointCloud, pointBuffer, IMAGE_WIDTH*IMAGE_HEIGHT*sizeof(cl_float4));
}
cl_float4 PointSmoother::SmoothePointByMeanDepth(cl_float4* pointCloud, const cl_int2& pixel, const cl_int2& linearDir)
{
const cl_float4& herept = pointCloud[PIXIDX(pixel)];
const cl_float4& righpt = pointCloud[IMGIDX(pixel.y+linearDir.y, pixel.x+linearDir.x)];
const cl_float4& leftpt = pointCloud[IMGIDX(pixel.y-linearDir.y, pixel.x-linearDir.x)];
float meanDepth = (DEPTH(herept) + DEPTH(leftpt) + DEPTH(righpt)) / 3.f;
cl_float4 smoothedpt = herept / DEPTH(herept) * meanDepth;
// Test:: normal distances of herept righpt leftpt smoothedpt
return smoothedpt;
}
void MainWindow::CheckPixel(QPoint pixel)
{
int viewOption = GetViewOptions();
QImage depthGrayMarked;
if(viewOption & ViewOpt::Segment)
depthGrayMarked = DrawUtils::GetColorMap().copy();
else
ImageConverter::ConvertToGrayImage(depthImg, depthGrayMarked);
QImage colorImgMarked = sharedData.ConstColorImage().copy();
pcworker->MarkNeighborsOnImage(colorImgMarked, pixel);
pcworker->MarkNeighborsOnImage(depthGrayMarked, pixel);
pcworker->DrawOnlyNeighbors(sharedData, pixel);
DrawUtils::MarkPoint3D(&sharedData, pixel);
gvm::AddCartesianAxes();
gvm::ShowAddedVertices();
depthScene->addPixmap(QPixmap::fromImage(depthGrayMarked));
colorScene->addPixmap(QPixmap::fromImage(colorImgMarked));
const int ptidx = IMGIDX(pixel.y(),pixel.x());
const cl_int* segmap = sharedData.ConstObjectMap();
QRgb color = DrawUtils::colorMap.pixel(pixel);
qDebug() << "picked pixel" << pixel << sharedData.ConstPointCloud()[ptidx]
<< "descriptor" << sharedData.ConstDescriptors()[ptidx]
<< "color" << qRed(color) << qGreen(color) << qBlue(color)
<< "object" << segmap[ptidx];
}
void PlaneExtractor::ExtractPlanes(cl_float4* normalCloud)
{
int x,y,n;
int countPixel=0;
int planeID = 0;//initial planeID
smalls_num=0;
for(n=0;n<IMAGE_WIDTH*IMAGE_HEIGHT;n++)
{
planemap[n]=-1;
normalCloud[n] = clNormalize(normalCloud[n]);
}
for(y=0; y<IMAGE_HEIGHT; y++)
{
for(x=0; x<IMAGE_WIDTH; x++)
{
if(planemap[IMGIDX(y,x)]==-1)
{
countPixel = 0;
CompareNormal(x, y, normalCloud, planeID, &countPixel);
if(countPixel < 30){
smalls[smalls_num]=planeID;
}
planeID++;
}
}
}
for(y=0; y<IMAGE_HEIGHT; y++)
{
for(x=0; x<IMAGE_WIDTH; x++)
{
for(n=0; n<smalls_num; n++)
{
if(planemap[IMGIDX(y,x)]==smalls[n])
{
planemap[IMGIDX(y,x)]=NotPlane;
}
}
}
}
qDebug() << "planeID is" <<planeID;
planeNum = planeID;
}
void PclDescriptors::ComputeObjectDescriptors(SharedData* shdDat, const float descriptorRadius)
{
qDebug() << "---------- PCL Object Descriptors ----------";
const cl_uchar* nullityMap = shdDat->ConstNullityMap();
const int indicSizeUpto = 1000;
int pxitv = 1;
do {
pxitv++;
indicesptr->clear();
for(int y=0; y<IMAGE_HEIGHT; y+=pxitv)
for(int x=0; x<IMAGE_WIDTH; x+=pxitv)
if(nullityMap[IMGIDX(y,x)] == NullID::NoneNull)
indicesptr->push_back(IMGIDX(y,x));
qDebug() << "object indices size" << indicesptr->size() << indicesptr->at(0) << pxitv;
} while (indicesptr->size() > indicSizeUpto);
ComputeIndexedDescriptors(shdDat, false, descriptorRadius, indicesptr);
}
void PclDescriptors::ComputeTrackingDescriptors(SharedData* shdDat, const std::vector<TrackPoint>* trackPoints, const float descriptorRadius)
{
qDebug() << "---------- PCL Tracking Descriptors ----------";
indicesptr->clear();
for(size_t i=0; i<trackPoints->size(); i++)
{
if(trackPoints->at(i).frameIndex == g_frameIdx)
{
int idx = IMGIDX(trackPoints->at(i).pixel.y, trackPoints->at(i).pixel.x);
indicesptr->push_back(idx);
}
}
ComputeIndexedDescriptors(shdDat, false, descriptorRadius, indicesptr);
}
void PlaneExtractor::CompareNormal(int x, int y, cl_float4* normalCloud, int planeID, int* countPixel){
//float Threshold = pointCloud[IMGIDX(y,x)].x;
static const cl_int2 direction[] = {{-1,0},{1,0},{0,1},{0,-1}};//left, right, up, dwon
if(planemap[IMGIDX(y,x)]==-1)
{
if(isnanf(normalCloud[y*IMAGE_WIDTH+x].x) || isnanf(normalCloud[y*IMAGE_WIDTH+x].y))
{
planemap[IMGIDX(y,x)]=NotPlane;
}
else{
*countPixel = *countPixel + 1;
planemap[IMGIDX(y,x)]=planeID;
bool move[4] = {1,1,1,1};
bool move_twice[4] = {1,1,1,1};
if(x==0) move[0]=0;
else if(x==1) move_twice[0]=0;
if(x==IMAGE_WIDTH-1) move[1]=0;
else if(x==IMAGE_WIDTH-2) move_twice[1]=0;
if(y==IMAGE_HEIGHT-1) move[2]=0;
else if(y==IMAGE_HEIGHT-2) move_twice[2]=0;
if(y==0) move[3]=0;
else if(y==1) move_twice[3]=0;
for(int i=0; i<4; i++){
int x_move1 = x+direction[i].x;
int y_move1 = y+direction[i].y;
int x_move2 = x+direction[i].x*2;
int y_move2 = y+direction[i].y*2;
if(move[i] && clDot(normalCloud[IMGIDX(y,x)],normalCloud[IMGIDX(y_move1,x_move1)])>Threshold)
{
CompareNormal(x_move1, y_move1, normalCloud, planeID, countPixel);
}
else if(move_twice[i] && clDot(normalCloud[IMGIDX(y,x)],normalCloud[IMGIDX(y_move2,x_move2)])>Threshold)
{
CompareNormal(x_move1, y_move1, normalCloud, planeID, countPixel);
CompareNormal(x_move2, y_move2, normalCloud, planeID, countPixel);
}
}
}
}
}
