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;
}
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];
}
Segment ObjectClusterer::ScalePlaneToIncludePoint(const Segment& srcPlane, const cl_float4& point)
{
Segment dstPlane = srcPlane;
const float normalDistBorder = fabsf(clDot(point, srcPlane.normal));
const float normalDistPlane = fabsf(clDot(srcPlane.center, srcPlane.normal));
dstPlane.center = srcPlane.center/normalDistPlane*normalDistBorder;
return dstPlane;
}
float ObjectClusterer::AngleBetweenVectorsDegree(const cl_float4& v1, const cl_float4& v2)
{
cl_float4 eye = (cl_float4){1,0,0,0};
bool concave = (clDot(eye,v1) + clDot(eye,v2) > 0);
float angleDegree = RAD2DEG(acosf(clDot(v1,v2)));
if(concave)
return 360.f - angleDegree;
else
return angleDegree;
}
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);
}
}
}
}
}
cl_float4 ObjectClusterer::PlaneDirectionFromBorder(const cl_float4& thisPlaneNormal, const cl_float4& borderDirection, const cl_float4& roughDirection)
{
cl_float4 planeDirection = clCross(thisPlaneNormal, borderDirection);
if(clDot(planeDirection, roughDirection) < 0.f)
planeDirection = planeDirection*(-1.f);
return clNormalize(planeDirection);
}
void DescriptorMakerCpu::SetRightVector(cl_float4 ctpoint, cl_float4 ctnormal
, const cl_float4* pointCloud, const cl_int* neighborIndices, int offset, int num_pts, float* L)
{
int nbidx;
cl_float4 diff;
cl_float4 fx[NUM_VAR];
cl_float8 rowOfF;
for(int i=0; i<NUM_VAR; i++)
fx[i] = (cl_float4){0,0,0,0};
for(int i=offset; i<offset+num_pts; i++)
{
nbidx = neighborIndices[i];
diff = pointCloud[nbidx] - ctpoint;
diff = diff*EQUATION_SCALE;
// set each row of F
rowOfF.s[0] = diff.x*diff.x;
rowOfF.s[1] = diff.y*diff.y;
rowOfF.s[2] = diff.z*diff.z;
rowOfF.s[3] = 2.f*diff.x*diff.y;
rowOfF.s[4] = 2.f*diff.y*diff.z;
rowOfF.s[5] = 2.f*diff.z*diff.x;
for(int r=0; r<NUM_VAR; r++)
for(int c=0; c<PT_DIM; c++)
fx[r].s[c] += rowOfF.s[r] * diff.s[c];
}
for(int i=0; i<NUM_VAR; i++)
L[L_INDEX(i,L_DIM)] = clDot(fx[i], ctnormal);
}
//除法计算
void divid(const char a[],const char b[],char result[])
{
bool isNegative = false;
char *op1,*pa,*pb,*pr;
int up,alen,blen,adotp,bdotp,i,k,dotp,t,t1,j,quo_size;
/////////////判定符号///////////////
//如果为异号
if((a[0] == '-'||b[0] == '-')&&a[0] != b[0])
result[0] = '-',isNegative = true;
//去除负号
if(a[0] == '-')a++;
if(b[0] == '-')b++;
///////////////////////////////////
alen = strlen(a)-1; //减去一位小数点
blen = strlen(b)-1;
///////获取被除数小数点移位后的位置//////////
adotp = strchr(a,'.')-a;
bdotp = strchr(b,'.')-b;
//计算商小数点位置
dotp = adotp+blen-bdotp;
if(isNegative)dotp++;
//////////准备数据/////////////
op1 = (char *)calloc(alen+blen+1,sizeof(char));
pa = (char *)calloc(alen+blen+1,sizeof(char));
pb = (char *)calloc(blen+1,sizeof(char));
pr = (char *)calloc(alen+blen+1,sizeof(char));
for(i = 0,t=0; i<=alen; i++)
{
if(a[i]!='.')
pa[t++] = a[i];
}
for(;t<dotp-1;t++)
{
pa[t] = '0';
}
for(;t<blen;t++)
{
pa[t] = '0';
}
pa[t] = '\0';
for(i = 0,t=0; i<=blen; i++)
{
if(b[i]!='.')
pb[t++] = b[i];
}
pb[t] = '\0';
clz(pa);
clz(pb);
////////取得被除数的高位数op1,且op1大于被除数b//////////
strncpy(op1,pa,strlen(pb));
if(strcmp(op1,pb)<0)
{
strncpy(op1,pa,strlen(pb)+1);
}
/////计算//////
j = k = strlen(op1);
t1=0;
quo_size = strlen(pa)+1-k; //获取商的长度
while(t1<quo_size)
{
up = 0;
t = cmp(op1,pb);
while(t>=0)
{
dsub(op1,pb);
t = cmp(op1,pb);
up++;
}
pr[t1++] = up+'0';
op1[strlen(op1)]=pa[j++];
clz(op1);
}
quo_size+=50;//加50精度
while(t1<quo_size&&(cmp(op1,(char *)"0")>0))
{
up = 0;
op1[strlen(op1)]='0';
t = cmp(op1,pb);
while(t>=0)
{
dsub(op1,pb);
t = cmp(op1,pb);
up++;
}
pr[t1++] = up+'0';
}
//////////////////////////////
if(isNegative)t=1;
else t=0;
for(i=0;i<=t1;i++)//复制结果并给商加上小数点
{
if(t==dotp) result[t++] = '.';
result[t++]=pr[i];
}
clz(result);
clDot(result);
free(op1);
free(pa);
free(pb);
}
