ICP fail

[QinZiwen]

When run demo, The ICP aways fail.

bool ok = icp_->estimateTransform(affine, p.intr, curr_.points_pyr, curr_.normals_pyr, prev_.points_pyr, prev_.normals_pyr);

I read codes in "projective_icp.cpp", but all of them seem normal.

[drmateo]

@QinZiwen same problem, but if you see the output of raycaster volume_->raycast(poses_.back(), p.intr, prev_.points_pyr[0], prev_.normals_pyr[0]); You'll see that is rendering a black image from the volume, so it's normal that ICP fails. I was exploring a little bit more in this problem and it seem that all comes from the volume integration, actually isn't integrating any image. Some clue to solve this?

[RNG65536]

It's because the new pack/unpack TSDF implementation uses half but not half_raw when converting to ushort, the same goes for the depth-to-dist kernel.

[tarukosu]

I've got the same problem. @RNG65536 Could you give me more information?

[RNG65536]

I've got the same problem. @RNG65536 Could you give me more information?

With cuda 9.0+, half type cannot be directly casted into int16 type, and must be first casted as __half_raw.

In device.hpp

__kf_device__ kfusion::device::TsdfVolume::elem_type kfusion::device::pack_tsdf(
    float tsdf, int weight)
{
    return make_ushort2(static_cast<__half_raw>(__float2half_rn(tsdf)).x, weight);
}

__kf_device__ float kfusion::device::unpack_tsdf(TsdfVolume::elem_type value,
                                                 int&                  weight)
{
    weight = value.y;
    return __half2float(static_cast<__half>(__half_raw{ value.x }));
}

__kf_device__ float kfusion::device::unpack_tsdf(TsdfVolume::elem_type value)
{
    return __half2float(static_cast<__half>(__half_raw{ value.x }));
}

In imgproc.cu

__global__ void compute_dists_kernel(const PtrStepSz<ushort> depth, Dists dists, float2 finv, float2 c)
{
    int x = threadIdx.x + blockIdx.x * blockDim.x;
    int y = threadIdx.y + blockIdx.y * blockDim.y;

    if (x < depth.cols || y < depth.rows)
    {
        float xl = (x - c.x) * finv.x;
        float yl = (y - c.y) * finv.y;
        float lambda = sqrtf (xl * xl + yl * yl + 1);
        dists(y, x) = static_cast<__half_raw>(__float2half_rn(depth(y, x) * lambda * 0.001f)).x; //meters
    }
}