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