test.jl

@time using PCLCommon,PCLVisualization,PCLIO,PCLFilters,Cxx
@time using CVCore, CVHighGUI, CVImgProc

# IR camera parameters of Kinect v2
fx = 365.1177
fy = 365.1177
cx = 256.0793 + 74
cy = 204.4635 + 100

function getDepthFromPointCloud!(depth, cloud)
    # @assert size(depth) == (512,424)
    fill!(depth, 0.0)
    w, h = size(depth)
    depthp = pointer(depth)
    icxx"""
        const float fx = $fx;
        const float fy = $fy;
        const float cx = $cx;
        const float cy = $cy;
        for (size_t i = 0; i < $(cloud.handle)->points.size(); ++i) {
            const auto p = $(cloud.handle)->points[i];
            if (std::isnan(p.z) || p.z <= 0) {
                continue;
            }
            int r, c;
            float depth;
            c = fx * p.x / p.z + cx;
            r = fy * p.y / p.z + cy;
            depth = p.z * 1000.0;
            if (r >= 0 && r < $h && c >= 0 && c < $w) {
                $(depthp)[$w * r + c] = depth;
            }
        }
    """
    depth
end

pcd_file = "2016-12-15-23-45-38-884_concat.pcd"
cloud = PointCloud{PointXYZ}(pcd_file)

depth = zeros(Float64, 512, 424)
getDepthFromPointCloud!(depth, cloud)

if isdefined(:vis) && vis
    viewer = PCLVisualizer("pcl visualizer")
    addCoordinateSystem(viewer, 1.2, 0,0,0)
    addPointCloud(viewer, cloud, PointCloudColorHandlerCustom(cloud, 255, 0, 0), id="cloud")
    spin(viewer)
    close(viewer); viewer = 0; gc();gc()
end

if isdefined(:highgui) && highgui
    mat = Mat(depth)
    flip!(mat, mat, -1)
    imshow("test", mat)
    waitKey(0)
    destroyAllWindows()
end