__global__ void Kernel_integrate(
bool flipVertical4render,
unsigned int xSize, unsigned int ySize, unsigned int zSize,
ComputeT xMin, ComputeT yMin, ComputeT zMin,
ComputeT unit, ComputeT margin,
unsigned int width, unsigned int height,
const float* depth, const ComputeT* pose, const ComputeT* intrinsics, StorageT *tsdf, uint8_t *weight) {
unsigned int x = blockIdx.x;
unsigned int y = threadIdx.x;
ComputeT xWorld = xMin + x * unit;
ComputeT yWorld = yMin + y * unit;
ComputeT zWorld = zMin;
ComputeT xCamera = pose[0] * xWorld + pose[1] * yWorld + pose[2] *zWorld + pose[3];
ComputeT yCamera = pose[4] * xWorld + pose[5] * yWorld + pose[6] *zWorld + pose[7];
ComputeT zCamera = pose[8] * xWorld + pose[9] * yWorld + pose[10] *zWorld + pose[11];
ComputeT xDelta = pose[2] * unit;
ComputeT yDelta = pose[6] * unit;
ComputeT zDelta = pose[10] * unit;
unsigned int idx_offset = (x * ySize + y) * zSize;
for (unsigned int z = 0; z < zSize; ++z, xCamera += xDelta, yCamera += yDelta, zCamera += zDelta){
ComputeT xOzCamera = xCamera / zCamera;
ComputeT yOzCamera = yCamera / zCamera;
int px = roundf(intrinsics[0] * xOzCamera + intrinsics[2]);
int py = roundf(intrinsics[4] * yOzCamera + intrinsics[5]);
if (px < 0 || px >= width || py < 0 || py >= height) continue;
float p_depth = *(depth + (flipVertical4render? (height-1 - py): py) * width + px);
if (p_depth == 0.0) continue;
ComputeT diff = ((ComputeT)p_depth - zCamera) * sqrtf(1.0 + xOzCamera * xOzCamera + yOzCamera * yOzCamera);
if(diff > -margin){
ComputeT v_new = fminf(1.0, diff/margin); //tsdf
v_new = 1.0 - fabs(v_new); // 1-tdf // comment this out if you want to use tsdf
unsigned int idx = idx_offset + z;
uint8_t w = weight[idx];
ComputeT v = GPUStorage2ComputeT(tsdf[idx]);
tsdf[idx] = GPUCompute2StorageT(fmin(fmax((ComputeT(w)*v + v_new)/(ComputeT(w + 1)), -1.f), 1.f));
weight[idx] = min(w+1,254);
}
}
}
// Helper function which clips the segment against the edge indicated by the flag argument
static inline void IntersectSide( ClipVert *v1, ClipVert *v2, ClipVert *v, BOOL color, BOOL light, BOOL tex, UInt32 flag )
{
float x, y, z, t;
float dx, dy, dz;
// Compute the parametric location of the intersection of the edge and the clip plane
dx = TheStateStack.ClipInfoPool[v2->xyz].csX - TheStateStack.ClipInfoPool[v1->xyz].csX;
dy = TheStateStack.ClipInfoPool[v2->xyz].csY - TheStateStack.ClipInfoPool[v1->xyz].csY;
dz = TheStateStack.ClipInfoPool[v2->xyz].csZ - TheStateStack.ClipInfoPool[v1->xyz].csZ;
t = ComputeT(TheStateStack.ClipInfoPool[v1->xyz].csX,
TheStateStack.ClipInfoPool[v1->xyz].csY,
TheStateStack.ClipInfoPool[v1->xyz].csZ,
dx,
dy,
dz,
flag );
ShiAssert( (t >= -0.002f) && (t <= 1.002f) );
// Compute the camera space intersection point
TheStateStack.ClipInfoPool[v->xyz].csZ = z = TheStateStack.ClipInfoPool[v1->xyz].csZ + t * (dz);
TheStateStack.ClipInfoPool[v->xyz].csX = x = TheStateStack.ClipInfoPool[v1->xyz].csX + t * (dx); // Note: either dx or dy is used only once, so could
TheStateStack.ClipInfoPool[v->xyz].csY = y = TheStateStack.ClipInfoPool[v1->xyz].csY + t * (dy); // be avoided, but this way, the code is more standardized...
// Now interpolate any other vertex parameters required
if (color) {
TheColorBank.ColorPool[v->rgba].r = TheColorBank.ColorPool[v1->rgba].r +
t * (TheColorBank.ColorPool[v2->rgba].r - TheColorBank.ColorPool[v1->rgba].r);
TheColorBank.ColorPool[v->rgba].g = TheColorBank.ColorPool[v1->rgba].g +
t * (TheColorBank.ColorPool[v2->rgba].g - TheColorBank.ColorPool[v1->rgba].g);
TheColorBank.ColorPool[v->rgba].b = TheColorBank.ColorPool[v1->rgba].b +
t * (TheColorBank.ColorPool[v2->rgba].b - TheColorBank.ColorPool[v1->rgba].b);
TheColorBank.ColorPool[v->rgba].a = TheColorBank.ColorPool[v1->rgba].a +
t * (TheColorBank.ColorPool[v2->rgba].a - TheColorBank.ColorPool[v1->rgba].a);
}
if (light) {
TheStateStack.IntensityPool[v->I] = TheStateStack.IntensityPool[v1->I] +
t * (TheStateStack.IntensityPool[v2->I] - TheStateStack.IntensityPool[v1->I]);
}
if (tex) {
v->uv.u = v1->uv.u + t * (v2->uv.u - v1->uv.u);
v->uv.v = v1->uv.v + t * (v2->uv.v - v1->uv.v);
}
// Now determine if the point is out to the sides
if (flag & (CLIP_TOP | CLIP_BOTTOM)) {
TheStateStack.ClipInfoPool[v->xyz].clipFlag = GetHorizontalClipFlags( x, z );
} else {
TheStateStack.ClipInfoPool[v->xyz].clipFlag = ON_SCREEN;
}
// Compute the screen space coordinates of the new point
register float OneOverZ = 1.0f / z;
TheStateStack.XformedPosPool[v->xyz].z = z;
TheStateStack.XformedPosPool[v->xyz].x = TheStateStack.XtoPixel( x * OneOverZ );
TheStateStack.XformedPosPool[v->xyz].y = TheStateStack.YtoPixel( y * OneOverZ );
}
auto Deg2Rad(Scalar deg) {
using ComputeT = typename std::conditional<std::is_floating_point<Scalar>::value, Scalar, long double>::type;
return deg / ComputeT(180) * Constants<ComputeT>::Pi;
}
auto Rad2Deg(Scalar rad) {
using ComputeT = typename std::conditional<std::is_floating_point<Scalar>::value, Scalar, long double>::type;
return rad / Constants<ComputeT>::Pi * ComputeT(180);
}
