Example #1
0
void Camera::get_frame(std::vector<unsigned char>& buffer_rgb, std::vector<float>& point_cloud)
{
    buffer_rgb.resize(3 * width * height);
    point_cloud.resize(3 * width * height);
    std::vector<float> buffer_depth(width * height);
    glBindFramebuffer(GL_FRAMEBUFFER, fbo[0]);
    glReadPixels(0, 0, width, height, GL_RGB, GL_UNSIGNED_BYTE, &buffer_rgb[0]);
    glReadPixels(0, 0, width, height, GL_DEPTH_COMPONENT, GL_FLOAT, &buffer_depth[0]);
    glBindFramebuffer(GL_FRAMEBUFFER, 0);
    for (int i = 0; i < width * height / 2; ++i) {
	const div_t coord = std::div(i, width);
	const int x = coord.rem, y = coord.quot, y_inv = height - 1 - y;
	std::swap(buffer_depth[i], buffer_depth[y_inv * width + x]);
	for (int j = 0; j < 3; ++j)
	    std::swap(buffer_rgb[3 * i + j], buffer_rgb[3 * (y_inv * width + x) + j]);
    }
    int viewport[4] = {0, 0, width, height};
    Eigen::Matrix4d id = Eigen::Matrix4d::Identity();
    for (unsigned i = 0; i < buffer_depth.size(); ++i) {
	const std::div_t coords = std::div(i, width);
	double out[3];
	gluUnProject(coords.rem, coords.quot, buffer_depth[i], id.data(), projection_matrix.data(), viewport, out + 0, out + 1, out + 2);
	for (int j = 0; j < 3; ++j)
	    point_cloud[i * 3 + j] = out[j];
	point_cloud[i * 3 + 2] *= -1.0f;
    }
    glBindVertexArray(vao[0]);
    glBindBuffer(GL_ARRAY_BUFFER, vbo[0]);
    glBufferData(GL_ARRAY_BUFFER, point_cloud.size() * sizeof(float), &point_cloud[0], GL_STATIC_DRAW);
    glVertexPointer(3, GL_FLOAT, 0, 0);
    glEnableClientState(GL_VERTEX_ARRAY);
    glBindVertexArray(0);
    n_points = point_cloud.size() / 3;
}
Example #2
0
void Camera::get_frame(std::vector<unsigned char>& buffer_rgb, std::vector<float>& point_cloud)
{
    buffer_rgb.resize(3 * width * height);
    point_cloud.resize(3 * width * height);
    std::vector<float> buffer_depth(width * height);
    glBindFramebuffer(GL_FRAMEBUFFER, fbo[0]);
    glReadPixels(0, 0, width, height, GL_RGB, GL_UNSIGNED_BYTE, &buffer_rgb[0]);
    glReadPixels(0, 0, width, height, GL_DEPTH_COMPONENT, GL_FLOAT, &buffer_depth[0]);
    glBindFramebuffer(GL_FRAMEBUFFER, 0);
    for (int i = 0; i < width * height / 2; ++i) {
        const div_t coord = std::div(i, width);
        const int x = coord.rem, y = coord.quot, y_inv = height - 1 - y;
        std::swap(buffer_depth[i], buffer_depth[y_inv * width + x]);
        for (int j = 0; j < 3; ++j)
            std::swap(buffer_rgb[3 * i + j], buffer_rgb[3 * (y_inv * width + x) + j]);
    }
    Eigen::Map<Eigen::Matrix4d> proj(projection_matrix.data());
    const auto m = proj.inverse();
    for (unsigned i = 0; i < buffer_depth.size(); ++i) {
        const std::div_t coords = std::div(i, width);
        const Eigen::Vector4d p (
            2.0 * static_cast<double>(coords.rem) / width - 1.0,
            2.0 * static_cast<double>(height - coords.quot) / height - 1.0,
            2.0 * static_cast<double>(buffer_depth[i]) - 1.0,
            1.0
        );
        const auto p2 = m * p;
        for (int j = 0; j < 3; ++j)
            point_cloud[i * 3 + j] = p2[j] / p2[3];
    }
    glBindVertexArray(vao[0]);
    glBindBuffer(GL_ARRAY_BUFFER, vbo[0]);
    glBufferData(GL_ARRAY_BUFFER, point_cloud.size() * sizeof(float), &point_cloud[0], GL_STATIC_DRAW);
    glVertexPointer(3, GL_FLOAT, 0, 0);
    glEnableClientState(GL_VERTEX_ARRAY);
    glBindVertexArray(0);
    n_points = point_cloud.size() / 3;
}