Registration::Registration(Freenect2Device::IrCameraParams depth_p, Freenect2Device::ColorCameraParams rgb_p):
depth(depth_p), color(rgb_p), filter_width_half(2), filter_height_half(1), filter_tolerance(0.01f)
{
float mx, my;
int ix, iy, index;
float rx, ry;
int *map_dist = distort_map;
float *map_x = depth_to_color_map_x;
float *map_y = depth_to_color_map_y;
int *map_yi = depth_to_color_map_yi;
for (int y = 0; y < 424; y++) {
for (int x = 0; x < 512; x++) {
// compute the dirstored coordinate for current pixel
distort(x,y,mx,my);
// rounding the values and check if the pixel is inside the image
ix = (int)(mx + 0.5f);
iy = (int)(my + 0.5f);
if(ix < 0 || ix >= 512 || iy < 0 || iy >= 424)
index = -1;
else
// computing the index from the coordianted for faster access to the data
index = iy * 512 + ix;
*map_dist++ = index;
// compute the depth to color mapping entries for the current pixel
depth_to_color(x,y,rx,ry);
*map_x++ = rx;
*map_y++ = ry;
// compute the y offset to minimize later computations
*map_yi++ = (int)(ry + 0.5f);
}
}
}
void GpuRegistration::setup(ofProtonect2* protonect, float scale) {
ldepth_index = 0;
depth = protonect->getIrCameraParams();
color = protonect->getColorCameraParams();
int w = scale * 512;
int h = scale * 424;
depth_to_color_map.allocate(w, h, 3);
{
ofFbo::Settings settings;
settings.width = w;
settings.height = h;
settings.minFilter = GL_NEAREST;
settings.maxFilter = GL_NEAREST;
settings.colorFormats.push_back(GL_RGB);
settings.colorFormats.push_back(GL_RGB32F_ARB);
settings.depthStencilAsTexture = true;
settings.useDepth = true;
settings.useStencil = true;
regist_fbo.allocate(settings);
}
masked_regist_fbo.allocate(w, h, GL_RGB);
const float mask_scale = 3.0;
ldepth_work_fbo.allocate(1920 / mask_scale, 1080 / mask_scale, GL_RGB32F_ARB);
ldepth_work_fbo.getTextureReference().setTextureMinMagFilter(GL_NEAREST, GL_NEAREST);
for (ofFbo& ldepth_fbo : ldepth_fbos) {
ldepth_fbo.allocate(1920 / mask_scale, 1080 / mask_scale, GL_RGB32F_ARB);
ldepth_fbo.getTextureReference().setTextureMinMagFilter(GL_NEAREST, GL_NEAREST);
}
float mx, my;
float rx, ry;
int ptr = 0;
float inv_sc = 1.0f / scale;
for (int y = 0; y < h; y++)
for (int x = 0; x < w; x++, ptr+=3) {
undistort_depth(inv_sc*x,inv_sc*y,mx,my);
depth_to_color(mx,my,rx,ry);
depth_to_color_map[ptr] = rx;
depth_to_color_map[ptr+1] = ry;
depth_to_color_map[ptr+2] = 0;
m.addVertex(ofVec3f(x, y, 0));
m.addTexCoord(ofVec2f(x,y));
}
m.setMode(OF_PRIMITIVE_POINTS);
m.enableTextures();
depth_to_color_map_tex.loadData(depth_to_color_map);
static string cshader_vert_str =
STRINGIFY(
uniform sampler2DRect tex; // cmap
uniform float inv_mask_scale;
void main()
{
vec4 cmap = texture2DRect(tex, gl_MultiTexCoord0.xy);
float dz = cmap.z;
if (dz == 0.0) {
dz = 65535.0;
}
vec4 worldpos = vec4(cmap.x * inv_mask_scale,
cmap.y * inv_mask_scale, -dz / 65535.0, 1.0);
gl_Position = gl_ModelViewProjectionMatrix * worldpos;
gl_FrontColor = cmap;
gl_TexCoord[0] = gl_MultiTexCoord0;
}
