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;
}
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;
}