先说下他的工作原理
把kinect每帧能得到一张深度图， 然后这张深度图呢一个像素对应camera view 中的一个点，如果知道camera的变换矩阵， 这些点就可以变成全局坐标. 我们把kinnect照出的点想成一个手电筒打出一堆点， 以第一帧的手电筒的位置和方向做为世界坐标系， 然后处理第二帧时， 得到一堆点，这堆点是在第二个手电筒下的坐标值， 这时我们把第二个手电筒放在第一个手电筒时， 这些点是没有对齐的， 这时我们用ICP, 让这些点对齐，对齐的同时， 第二个手电筒也动， 则当收敛时，第二个手电筒的位置和方向就确认了。这样后面每一帧手电筒的位置都可以通过前一帧来得到。
注意 ICP不是找最近, 而是找前后两帧Image view的对应点, 然后转到世界坐标系中来算camera pose.
然后就是voxel里面tsdf值的问题， 体素是事先建好的， 从第一帧开始，对于每个体素来说，先算体素中心点（注意中心点是世界坐标）到手电筒（camera）距离d1，然后该中心点通过手电筒的变换矩阵的逆转到camera view中， 然后用perspective project投影，将坐标标准化， 最后用这个标准化坐标去索引第一帧的深度图(注意不是整数的坐标会插值得到深度)， 得到该中心点的深度， 也就是手电筒到中心点的方向上， 手电筒到surface的距离d2，这样d1-d2就是surface到该体素中心的距离， 后面也照这个方向去算， 只不过算出来的值会与以前的值进行权值平均， 权值怎么算， 怎么平均请看paper.
最后就是通过raycast从体素中提取点的问题了，对于最新一帧手电筒得到深度图， 每个像素，执行[u,0]和[u,1]的反投影，得到对应在camera view的坐标， 结合camera的变换矩阵， 得到对应的世界坐标的值， 然后得到对应的体素， 用他们的体素坐标来算，然后沿着射线的方向，来依次搜索tsdf为0的点，如果搜索到提取出其世界坐标， 具体怎么提取看paper， 然后算该点（网格坐标下）的tsdf的梯度， 作为该点的法向量。 注意论文中global mesh vertex是其他已知的mesh， 先按照camera的方向渲染出一个mesh map, 如果没有找到0截面就用mesh map对应的值， 或者secondary ray.
1) Depth map conversion
vi(u)=Di(u)K−1[u,1]
v
i
(
u
)
=
D
i
(
u
)
K
−
1
[
u
,
1
]
可以找出深度图中每 个像素对应的camera view中的坐标
2) Camera tracking
这里论文中的算法写得不太清楚
直接对照着源码来看吧
pcl/gpu/kinfu/src/cuda/estimate_combined.cu
__device__ __forceinline__ bool
search (int x, int y, float3& n, float3& d, float3& s) const
{
float3 ncurr;
ncurr.x = nmap_curr.ptr (y)[x];
if (isnan (ncurr.x))
return (false);
float3 vcurr;//当前camera view中的坐标
vcurr.x = vmap_curr.ptr (y )[x];
vcurr.y = vmap_curr.ptr (y + rows)[x];
vcurr.z = vmap_curr.ptr (y + 2 * rows)[x];
float3 vcurr_g = Rcurr * vcurr + tcurr;//用上一步的R和t作为这帧的R,t的估计,然后得到这帧中点的全局坐标
float3 vcurr_cp = Rprev_inv * (vcurr_g - tprev); // prev camera coo space 通过这个公式将该点称回到上一帧的camera view的坐标系中
int2 ukr; //projection
ukr.x = __float2int_rn (vcurr_cp.x * intr.fx / vcurr_cp.z + intr.cx); //4
ukr.y = __float2int_rn (vcurr_cp.y * intr.fy / vcurr_cp.z + intr.cy); //4
//获取该点在上一帧中image space的坐标
if (ukr.x < 0 || ukr.y < 0 || ukr.x >= cols || ukr.y >= rows || vcurr_cp.z < 0)//如果在上一帧中的视图中,继续;否则结束
return (false);
float3 nprev_g;//全局法向量通过前一帧的全局nmap来找的
nprev_g.x = nmap_g_prev.ptr (ukr.y)[ukr.x];
if (isnan (nprev_g.x))
return (false);
float3 vprev_g;//全局点坐标通过前一帧的全局vmap来找的
vprev_g.x = vmap_g_prev.ptr (ukr.y )[ukr.x];
vprev_g.y = vmap_g_prev.ptr (ukr.y + rows)[ukr.x];
vprev_g.z = vmap_g_prev.ptr (ukr.y + 2 * rows)[ukr.x];
float dist = norm (vprev_g - vcurr_g); //前一帧点的全局坐标和当前帧点的全局坐标差
if (dist > distThres)
return (false);
ncurr.y = nmap_curr.ptr (y + rows)[x];
ncurr.z = nmap_curr.ptr (y + 2 * rows)[x];
float3 ncurr_g = Rcurr * ncurr;//当前帧的全局法向量
nprev_g.y = nmap_g_prev.ptr (ukr.y + rows)[ukr.x];
nprev_g.z = nmap_g_prev.ptr (ukr.y + 2 * rows)[ukr.x];
float sine = norm (cross (ncurr_g, nprev_g));
if (sine >= angleThres)//如果当前帧的全局法向量和前一帧的全局法向量超过一定anlge则是不匹配
return (false);
n = nprev_g;
d = vprev_g;
s = vcurr_g;
return (true);
}注意这里要先找correspondence, 再求Optimal transforamtion
论文的算法太confusing了, 直接跳过
3) volumetric representation
看下listing 2的算法
1 对于每个体素点(方块中间那个点)
2 对每层的体素
3 先转成世界坐标
4 转成camera view坐标
5 转成Image space 坐标
6 如果在image space中
7
∥ti−vg∥
‖
t
i
−
v
g
‖
是表示体素的global position 到摄像机的距离,
Di(p)
D
i
(
p
)
表示摄像机到surface的距离,
|ti−vg∥−Di(p)
|
t
i
−
v
g
‖
−
D
i
(
p
)
表示体素到surface的距离
后面就是将体素到surface的距离截取到[-1,1]范围内,然后一直更新前后所算得的距离
4) raycasting for rendering and tracking 看算法
__device__ __forceinline__ void
operator () () const
{
int x = threadIdx.x + blockIdx.x * CTA_SIZE_X;
int y = threadIdx.y + blockIdx.y * CTA_SIZE_Y;
if (x >= cols || y >= rows)
return;
vmap.ptr (y)[x] = numeric_limits<float>::quiet_NaN ();
nmap.ptr (y)[x] = numeric_limits<float>::quiet_NaN ();
float3 ray_start = tcurr;//摄像机的全局位置
float3 ray_next = Rcurr * get_ray_next (x, y) + tcurr;//摄像机远平面对应像素x,y的点的全局坐标
float3 ray_dir = normalized (ray_next - ray_start);
//ensure that it isn't a degenerate case
ray_dir.x = (ray_dir.x == 0.f) ? 1e-15 : ray_dir.x;
ray_dir.y = (ray_dir.y == 0.f) ? 1e-15 : ray_dir.y;
ray_dir.z = (ray_dir.z == 0.f) ? 1e-15 : ray_dir.z;
// computer time when entry and exit volume
float time_start_volume = getMinTime (volume_size, ray_start, ray_dir);
float time_exit_volume = getMaxTime (volume_size, ray_start, ray_dir);
const float min_dist = 0.f; //in meters
time_start_volume = fmax (time_start_volume, min_dist);
if (time_start_volume >= time_exit_volume)
return;
float time_curr = time_start_volume;
int3 g = getVoxel (ray_start + ray_dir * time_curr);
g.x = max (0, min (g.x, VOLUME_X - 1));
g.y = max (0, min (g.y, VOLUME_Y - 1));
g.z = max (0, min (g.z, VOLUME_Z - 1));
float tsdf = readTsdf (g.x, g.y, g.z);
//infinite loop guard
const float max_time = 3 * (volume_size.x + volume_size.y + volume_size.z);
for (; time_curr < max_time; time_curr += time_step)
{
float tsdf_prev = tsdf;
int3 g = getVoxel ( ray_start + ray_dir * (time_curr + time_step) );
if (!checkInds (g))
break;
tsdf = readTsdf (g.x, g.y, g.z);
if (tsdf_prev < 0.f && tsdf > 0.f)
break;
if (tsdf_prev > 0.f && tsdf < 0.f) //zero crossing
{
float Ftdt = interpolateTrilineary (ray_start, ray_dir, time_curr + time_step);
if (isnan (Ftdt))
break;
float Ft = interpolateTrilineary (ray_start, ray_dir, time_curr);//current time时该
if (isnan (Ft))
break;
//float Ts = time_curr - time_step * Ft/(Ftdt - Ft);
float Ts = time_curr - time_step * Ft / (Ftdt - Ft);//减少时间回去到tsdf为0的点
float3 vetex_found = ray_start + ray_dir * Ts;//tsdf为0的点
vmap.ptr (y )[x] = vetex_found.x;
vmap.ptr (y + rows)[x] = vetex_found.y;
vmap.ptr (y + 2 * rows)[x] = vetex_found.z;
int3 g = getVoxel ( ray_start + ray_dir * time_curr );
if (g.x > 1 && g.y > 1 && g.z > 1 && g.x < VOLUME_X - 2 && g.y < VOLUME_Y - 2 && g.z < VOLUME_Z - 2)
{
float3 t;
float3 n;
//开始求法向量
t = vetex_found;
t.x += cell_size.x;
float Fx1 = interpolateTrilineary (t);
t = vetex_found;
t.x -= cell_size.x;
float Fx2 = interpolateTrilineary (t);
n.x = (Fx1 - Fx2);
t = vetex_found;
t.y += cell_size.y;
float Fy1 = interpolateTrilineary (t);
t = vetex_found;
t.y -= cell_size.y;
float Fy2 = interpolateTrilineary (t);
n.y = (Fy1 - Fy2);
t = vetex_found;
t.z += cell_size.z;
float Fz1 = interpolateTrilineary (t);
t = vetex_found;
t.z -= cell_size.z;
float Fz2 = interpolateTrilineary (t);
n.z = (Fz1 - Fz2);
n = normalized (n);
nmap.ptr (y )[x] = n.x;
nmap.ptr (y + rows)[x] = n.y;
nmap.ptr (y + 2 * rows)[x] = n.z;
}
break;
}
} /* for(;;) */
}
};
__global__ void
rayCastKernel (const RayCaster rc) {
rc ();
}
}
}__device__ __forceinline__ float
interpolateTrilineary (const float3& point) const
{
int3 g = getVoxel (point);
if (g.x <= 0 || g.x >= VOLUME_X - 1)//为了保证顺利进行插值, 把边界的体素去掉,
return numeric_limits<float>::quiet_NaN ();
if (g.y <= 0 || g.y >= VOLUME_Y - 1)
return numeric_limits<float>::quiet_NaN ();
if (g.z <= 0 || g.z >= VOLUME_Z - 1)
return numeric_limits<float>::quiet_NaN ();
float vx = (g.x + 0.5f) * cell_size.x;
float vy = (g.y + 0.5f) * cell_size.y;
float vz = (g.z + 0.5f) * cell_size.z;
//算落在体素的哪个卦限, 根据卦限算出由voxel center组成的cube的最左下角的voxel center所在的voxel的索引
g.x = (point.x < vx) ? (g.x - 1) : g.x;
g.y = (point.y < vy) ? (g.y - 1) : g.y;
g.z = (point.z < vz) ? (g.z - 1) : g.z;
//在voxel center组成的cube中, 由当前位置减去该cube的左下角的点(体素center点)求出每维上的权值
float a = (point.x - (g.x + 0.5f) * cell_size.x) / cell_size.x;
float b = (point.y - (g.y + 0.5f) * cell_size.y) / cell_size.y;
float c = (point.z - (g.z + 0.5f) * cell_size.z) / cell_size.z;
这里是算该点到由八个voxel中心点围成的cube在每个维度上的权值
float res = readTsdf (g.x + 0, g.y + 0, g.z + 0) * (1 - a) * (1 - b) * (1 - c) +
readTsdf (g.x + 0, g.y + 0, g.z + 1) * (1 - a) * (1 - b) * c +
readTsdf (g.x + 0, g.y + 1, g.z + 0) * (1 - a) * b * (1 - c) +
readTsdf (g.x + 0, g.y + 1, g.z + 1) * (1 - a) * b * c +
readTsdf (g.x + 1, g.y + 0, g.z + 0) * a * (1 - b) * (1 - c) +
readTsdf (g.x + 1, g.y + 0, g.z + 1) * a * (1 - b) * c +
readTsdf (g.x + 1, g.y + 1, g.z + 0) * a * b * (1 - c) +
readTsdf (g.x + 1, g.y + 1, g.z + 1) * a * b * c;
return res;
}
扫一扫
1540
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
