CVertexBufferObject::CVertexBufferObject(bool isStatic, int numVertices, const CVertexAttributes& attributes) :
m_bufferHandle(0),
m_tmpHandle(0),
m_isDirect(true),
m_usage(0),
m_isDirty(false),
m_attributes(attributes),
m_isBound(false),
m_isStatic(isStatic)
{
m_byteBuffer = byte_buffer(attributes.vertexSize * numVertices);
m_byteBuffer.flip();
m_buffer = float_buffer(attributes.vertexSize * numVertices);
//m_buffer = m_byteBuffer.convert<float>();
m_buffer.flip();
m_bufferHandle = CreateBufferObject();
m_usage = isStatic ? GL_STATIC_DRAW : GL_DYNAMIC_DRAW;
}
void Visualizer::CaptureDepthImage(const std::string &filename/* = ""*/,
bool do_render/* = true*/, double depth_scale/* = 1000.0*/)
{
std::string png_filename = filename;
std::string camera_filename;
if (png_filename.empty()) {
std::string timestamp = GetCurrentTimeStamp();
png_filename = "DepthCapture_" + timestamp + ".png";
camera_filename = "DepthCamera_" + timestamp + ".json";
}
Image depth_image;
depth_image.PrepareImage(view_control_ptr_->GetWindowWidth(),
view_control_ptr_->GetWindowHeight(), 1, 4);
if (do_render) {
Render();
is_redraw_required_ = false;
}
glFinish();
#if __APPLE__
// On OSX with Retina display and glfw3, there is a bug with glReadPixels().
// When using glReadPixels() to read a block of depth data. The data is
// horizontally streched (vertically it is fine). This issue is related
// to GLFW_SAMPLES hint. When it is set to 0 (anti-aliasing disabled),
// glReadPixels() works fine. See this post for details:
// http://stackoverflow.com/questions/30608121/glreadpixel-one-pass-vs-looping-through-points
// The reason of this bug is unknown. The current workaround is to read
// depth buffer column by column. This is 15~30 times slower than one block
// reading glReadPixels().
std::vector<float> float_buffer(depth_image.height_);
float *p = (float *)depth_image.data_.data();
for (int j = 0; j < depth_image.width_; j++) {
glReadPixels(j, 0, 1, depth_image.width_,
GL_DEPTH_COMPONENT, GL_FLOAT,
float_buffer.data());
for (int i = 0; i < depth_image.height_; i++) {
p[i * depth_image.width_ + j] = float_buffer[i];
}
}
#else //__APPLE__
// By default, glReadPixels read a block of depth buffer.
glReadPixels(0, 0, depth_image.width_, depth_image.height_,
GL_DEPTH_COMPONENT, GL_FLOAT, depth_image.data_.data());
#endif //__APPLE__
// glReadPixels get the screen in a vertically flipped manner
// We should flip it back, and convert it to the correct depth value
Image png_image;
double z_near = view_control_ptr_->GetZNear();
double z_far = view_control_ptr_->GetZFar();
png_image.PrepareImage(view_control_ptr_->GetWindowWidth(),
view_control_ptr_->GetWindowHeight(), 1, 2);
for (int i = 0; i < depth_image.height_; i++) {
float *p_depth = (float *)(depth_image.data_.data() +
depth_image.BytesPerLine() * (depth_image.height_ - i - 1));
uint16_t *p_png = (uint16_t *)(png_image.data_.data() +
png_image.BytesPerLine() * i);
for (int j = 0; j < depth_image.width_; j++) {
if (p_depth[j] == 1.0) {
continue;
}
double z_depth = 2.0 * z_near * z_far /
(z_far + z_near - (2.0 * (double)p_depth[j] - 1.0) *
(z_far - z_near));
p_png[j] = (uint16_t)std::min(std::round(depth_scale * z_depth),
(double)INT16_MAX);
}
}
PrintDebug("[Visualizer] Depth capture to %s\n", png_filename.c_str());
WriteImage(png_filename, png_image);
if (!camera_filename.empty()) {
PrintDebug("[Visualizer] Depth camera capture to %s\n",
camera_filename.c_str());
PinholeCameraTrajectory trajectory;
trajectory.extrinsic_.resize(1);
view_control_ptr_->ConvertToPinholeCameraParameters(
trajectory.intrinsic_, trajectory.extrinsic_[0]);
WriteIJsonConvertible(camera_filename, trajectory);
}
}
void Visualizer::CaptureDepthPointCloud(const std::string &filename/* = ""*/,
bool do_render/* = true*/, bool convert_to_world_coordinate/* = false*/)
{
std::string ply_filename = filename;
std::string camera_filename;
if (ply_filename.empty()) {
std::string timestamp = GetCurrentTimeStamp();
ply_filename = "DepthCapture_" + timestamp + ".ply";
camera_filename = "DepthCamera_" + timestamp + ".json";
}
Image depth_image;
depth_image.PrepareImage(view_control_ptr_->GetWindowWidth(),
view_control_ptr_->GetWindowHeight(), 1, 4);
if (do_render) {
Render();
is_redraw_required_ = false;
}
glFinish();
#if __APPLE__
// On OSX with Retina display and glfw3, there is a bug with glReadPixels().
// When using glReadPixels() to read a block of depth data. The data is
// horizontally stretched (vertically it is fine). This issue is related
// to GLFW_SAMPLES hint. When it is set to 0 (anti-aliasing disabled),
// glReadPixels() works fine. See this post for details:
// http://stackoverflow.com/questions/30608121/glreadpixel-one-pass-vs-looping-through-points
// The reason of this bug is unknown. The current workaround is to read
// depth buffer column by column. This is 15~30 times slower than one block
// reading glReadPixels().
std::vector<float> float_buffer(depth_image.height_);
float *p = (float *)depth_image.data_.data();
for (int j = 0; j < depth_image.width_; j++) {
glReadPixels(j, 0, 1, depth_image.width_,
GL_DEPTH_COMPONENT, GL_FLOAT,
float_buffer.data());
for (int i = 0; i < depth_image.height_; i++) {
p[i * depth_image.width_ + j] = float_buffer[i];
}
}
#else //__APPLE__
// By default, glReadPixels read a block of depth buffer.
glReadPixels(0, 0, depth_image.width_, depth_image.height_,
GL_DEPTH_COMPONENT, GL_FLOAT, depth_image.data_.data());
#endif //__APPLE__
GLHelper::GLMatrix4f mvp_matrix;
if (convert_to_world_coordinate) {
mvp_matrix = view_control_ptr_->GetMVPMatrix();
}
else {
mvp_matrix = view_control_ptr_->GetProjectionMatrix();
}
// glReadPixels get the screen in a vertically flipped manner
// We should flip it back, and convert it to the correct depth value
PointCloud depth_pointcloud;
for (int i = 0; i < depth_image.height_; i++) {
float *p_depth = (float *)(depth_image.data_.data() +
depth_image.BytesPerLine() * i);
for (int j = 0; j < depth_image.width_; j++) {
if (p_depth[j] == 1.0) {
continue;
}
depth_pointcloud.points_.push_back(GLHelper::Unproject(
Eigen::Vector3d(j + 0.5, i + 0.5, p_depth[j]),
mvp_matrix,
view_control_ptr_->GetWindowWidth(),
view_control_ptr_->GetWindowHeight()));
}
}
PrintDebug("[Visualizer] Depth point cloud capture to %s\n",
ply_filename.c_str());
WritePointCloud(ply_filename, depth_pointcloud);
if (!camera_filename.empty()) {
PrintDebug("[Visualizer] Depth camera capture to %s\n",
camera_filename.c_str());
PinholeCameraTrajectory trajectory;
trajectory.extrinsic_.resize(1);
view_control_ptr_->ConvertToPinholeCameraParameters(
trajectory.intrinsic_, trajectory.extrinsic_[0]);
WriteIJsonConvertible(camera_filename, trajectory);
}
}
std::shared_ptr<Image> Visualizer::CaptureDepthFloatBuffer(
bool do_render/* = true*/)
{
Image depth_image;
depth_image.PrepareImage(view_control_ptr_->GetWindowWidth(),
view_control_ptr_->GetWindowHeight(), 1, 4);
if (do_render) {
Render();
is_redraw_required_ = false;
}
glFinish();
#if __APPLE__
// On OSX with Retina display and glfw3, there is a bug with glReadPixels().
// When using glReadPixels() to read a block of depth data. The data is
// horizontally stretched (vertically it is fine). This issue is related
// to GLFW_SAMPLES hint. When it is set to 0 (anti-aliasing disabled),
// glReadPixels() works fine. See this post for details:
// http://stackoverflow.com/questions/30608121/glreadpixel-one-pass-vs-looping-through-points
// The reason of this bug is unknown. The current workaround is to read
// depth buffer column by column. This is 15~30 times slower than one block
// reading glReadPixels().
std::vector<float> float_buffer(depth_image.height_);
float *p = (float *)depth_image.data_.data();
for (int j = 0; j < depth_image.width_; j++) {
glReadPixels(j, 0, 1, depth_image.width_,
GL_DEPTH_COMPONENT, GL_FLOAT,
float_buffer.data());
for (int i = 0; i < depth_image.height_; i++) {
p[i * depth_image.width_ + j] = float_buffer[i];
}
}
#else //__APPLE__
// By default, glReadPixels read a block of depth buffer.
glReadPixels(0, 0, depth_image.width_, depth_image.height_,
GL_DEPTH_COMPONENT, GL_FLOAT, depth_image.data_.data());
#endif //__APPLE__
// glReadPixels get the screen in a vertically flipped manner
// We should flip it back, and convert it to the correct depth value
auto image_ptr = std::make_shared<Image>();
double z_near = view_control_ptr_->GetZNear();
double z_far = view_control_ptr_->GetZFar();
image_ptr->PrepareImage(view_control_ptr_->GetWindowWidth(),
view_control_ptr_->GetWindowHeight(), 1, 4);
for (int i = 0; i < depth_image.height_; i++) {
float *p_depth = (float *)(depth_image.data_.data() +
depth_image.BytesPerLine() * (depth_image.height_ - i - 1));
float *p_image = (float *)(image_ptr->data_.data() +
image_ptr->BytesPerLine() * i);
for (int j = 0; j < depth_image.width_; j++) {
if (p_depth[j] == 1.0) {
continue;
}
double z_depth = 2.0 * z_near * z_far /
(z_far + z_near - (2.0 * (double)p_depth[j] - 1.0) *
(z_far - z_near));
p_image[j] = (float)z_depth;
}
}
return image_ptr;
}
