cl_int2 ObjectClusterer::SearchValidPixelOnLine(const ImLine& border, const cl_int2& centerPixel, const int firstID, const int secondID)
{
int majorAxis=0, minorAxis=1;
if(border.IsXAxisMajor()==false)
{
majorAxis=1;
minorAxis=0;
}
cl_int2 linePixel = centerPixel;
int pxidx, move=1;
int rangeMin = smin(border.endPixels[0].s[majorAxis], border.endPixels[1].s[majorAxis]);
int rangeMax = smax(border.endPixels[0].s[majorAxis], border.endPixels[1].s[majorAxis]);
cl_float4 coef = (cl_float4){border.a, border.b, border.c, 0.f};
while(linePixel.s[majorAxis] >= rangeMin && linePixel.s[majorAxis] <= rangeMax)
{
linePixel.s[majorAxis] = centerPixel.s[majorAxis]-move;
linePixel.s[minorAxis] = (int)(-coef.s[majorAxis]/coef.s[minorAxis]*linePixel.s[majorAxis] + coef.s[2]/coef.s[minorAxis]);
pxidx = PIXIDX(linePixel);
if(nullityMap[pxidx] < NullID::PointNull && (objectMap[pxidx]==firstID || objectMap[pxidx]==secondID))
return linePixel;
linePixel.s[majorAxis] = centerPixel.s[majorAxis]+move;
linePixel.s[minorAxis] = (int)(-coef.s[majorAxis]/coef.s[minorAxis]*linePixel.s[majorAxis] + coef.s[2]/coef.s[minorAxis]);
pxidx = PIXIDX(linePixel);
if(nullityMap[pxidx] < NullID::PointNull && (objectMap[pxidx]==firstID || objectMap[pxidx]==secondID))
return linePixel;
move++;
}
throw 1;
return centerPixel;
}
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];
}
cl_float4 ObjectClusterer::VirtualPointOnPlaneAroundBorder(const Segment& plane, const ImLine& border, const cl_int2& borderCenter, bool upperPlane)
{
const int pixelDist = 10;
cl_int2 vpixel1st, vpixel2nd, virtualPixel;
ImLine orthLine;
orthLine.OrthogonalTo(border, borderCenter);
if(orthLine.IsXAxisMajor())
{
vpixel1st.x = borderCenter.x + pixelDist;
vpixel1st.y = orthLine.GetY(vpixel1st.x);
vpixel2nd.x = borderCenter.x - pixelDist;
vpixel2nd.y = orthLine.GetY(vpixel2nd.x);
}
else
{
vpixel1st.y = borderCenter.y + pixelDist;
vpixel1st.x = orthLine.GetX(vpixel1st.y);
vpixel2nd.y = borderCenter.y - pixelDist;
vpixel2nd.x = orthLine.GetX(vpixel2nd.y);
}
if(upperPlane)
virtualPixel = (border.IsAboveLine(vpixel1st)) ? vpixel1st : vpixel2nd;
else
virtualPixel = (border.IsAboveLine(vpixel1st)) ? vpixel2nd : vpixel1st;
#ifdef DEBUG_ObjectClusterBase
virutalPixels.push_back(virtualPixel);
#endif
const float normalDistBorder = fabsf(clDot(pointCloud[PIXIDX(borderCenter)], plane.normal));
const cl_float4 rayDir = ImageConverter::ConvertPixelToPoint(virtualPixel.x, virtualPixel.y, 1.f);
cl_float4 pointOnPlane = rayDir/fabsf(clDot(rayDir, plane.normal))*normalDistBorder;
assert(fabsf(normalDistBorder - fabsf(clDot(pointOnPlane, plane.normal))) < 0.001f);
return pointOnPlane;
}
float ObjectClusterer::InnerAngleBetweenPlanes(const Segment& firstPlane, const Segment& secondPlane, const vecPairOfPixels& connPixels)
{
ImLine border = FitLine2D(connPixels);
bool firstPlaneUp = IsFirstUpperPlane(border, connPixels);
cl_int2 borderCenter = PickBorderCenter(border, firstPlane.id, secondPlane.id);
const cl_float4& pointOnBorder = pointCloud[PIXIDX(borderCenter)];
cl_float4 borderDirection = clCross(firstPlane.normal, secondPlane.normal);
Segment scFirstPlane = ScalePlaneToIncludePoint(firstPlane, pointOnBorder);
Segment scSecondPlane = ScalePlaneToIncludePoint(secondPlane, pointOnBorder);
cl_float4 pointOnFirst = VirtualPointOnPlaneAroundBorder(scFirstPlane, border, borderCenter, firstPlaneUp);
cl_float4 pointOnSecond = VirtualPointOnPlaneAroundBorder(scSecondPlane, border, borderCenter, !firstPlaneUp);
cl_float4 firstDirFromBorder = PlaneDirectionFromBorder(scFirstPlane.normal, borderDirection, pointOnFirst - pointOnBorder);
cl_float4 secondDirFromBorder = PlaneDirectionFromBorder(scSecondPlane.normal, borderDirection, pointOnSecond - pointOnBorder);
float angleDegree = AngleBetweenVectorsDegree(firstDirFromBorder, secondDirFromBorder);
#ifdef DEBUG_ObjectClusterBase
IdPairs.emplace_back(firstPlane.id, secondPlane.id);
pointPairs.emplace_back(pointOnFirst, pointOnSecond);
borderPoints.push_back(pointOnBorder);
borderLines.push_back(border);
betweenAngles.push_back(angleDegree);
heights.emplace_back(0,0);
#endif
return angleDegree;
}
cl_int2 ObjectClusterer::PickBorderCenter(const ImLine& border, const int firstID, const int secondID)
{
const cl_int2 centerPixel = (border.endPixels[0] + border.endPixels[1])/2;
const int pxidx = PIXIDX(centerPixel);
if(nullityMap[pxidx] < NullID::PointNull && (objectMap[pxidx]==firstID || objectMap[pxidx]==secondID))
return centerPixel;
else
return SearchValidPixelOnLine(border, centerPixel, firstID, secondID);
}
cl_float4 PointSmoother::SmoothePoint(cl_float4* pointCloud, const cl_int2& pixel, cl_float4& normal)
{
// find only linear direction
const cl_int2 linearDir = SearchLinearDirection(pointCloud, pixel, normal);
if(linearDir.x==0 && linearDir.y==0)
return pointCloud[PIXIDX(pixel)];
// smoothe and debug
return SmoothePointByMeanDepth(pointCloud, pixel, linearDir);
}
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;
}
