Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

T265 query devices fix #3515

Merged
merged 10 commits into from
Mar 21, 2019
Merged

T265 query devices fix #3515

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
e815418
TM2 Core fixes:
ev-mp Mar 18, 2019
de438bf
Make TM2 query_devices synchronized
ev-mp Mar 18, 2019
057cf32
Multicam demo adaptation to T265 with a shared context:
ev-mp Mar 18, 2019
a13f824
Rework MSVC non-compliency:
ev-mp Mar 18, 2019
e50f067
Reorder query_devices to prioritize D400 over T265.
ev-mp Mar 18, 2019
d83b9a0
Clean log messages
ev-mp Mar 19, 2019
49a05f1
CodeReview fixes:
ev-mp Mar 19, 2019
c379a46
Documentation fixes
ev-mp Mar 20, 2019
7b68dce
rs-multicam: fix GCC build
ev-mp Mar 20, 2019
4455f4f
CodeReview:
ev-mp Mar 21, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions examples/align/rs-align.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -136,7 +136,7 @@ float get_depth_scale(rs2::device dev)
return dpt.get_depth_scale();
}
}
throw std::runtime_error("Device does not have a depth sensor");
throw std::runtime_error("The demo requires a Depth sensor to run");
}

void render_slider(rect location, float& clipping_dist)
Expand Down Expand Up @@ -215,7 +215,7 @@ rs2_stream find_stream_to_align(const std::vector<rs2::stream_profile>& streams)
rs2_stream align_to = RS2_STREAM_ANY;
bool depth_stream_found = false;
bool color_stream_found = false;
for (rs2::stream_profile sp : streams)
for (const rs2::stream_profile& sp : streams)
{
rs2_stream profile_stream = sp.stream_type();
if (profile_stream != RS2_STREAM_DEPTH)
Expand Down
293 changes: 201 additions & 92 deletions examples/example.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,11 @@
#include <cmath>
#include <map>

#define PI 3.14159265358979323846
#define IMU_FRAME_WIDTH 1280
#define IMU_FRAME_HEIGHT 720
#ifndef PI
const double PI = 3.14159265358979323846;
#endif
const size_t IMU_FRAME_WIDTH = 1280;
const size_t IMU_FRAME_HEIGHT = 720;
//////////////////////////////
// Basic Data Types //
//////////////////////////////
Expand Down Expand Up @@ -101,83 +103,7 @@ void set_viewport(const rect& r)
glOrtho(0, r.w, r.h, 0, -1, +1);
}

////////////////////////
// Image display code //
////////////////////////
class texture
{
GLuint gl_handle = 0;
rs2_stream stream = RS2_STREAM_ANY;

public:
void render(const rs2::video_frame& frame, const rect& rect)
{
upload(frame);
show(rect.adjust_ratio({ (float)frame.get_width(), (float)frame.get_height() }));
}

void upload(const rs2::video_frame& frame)
{
if (!frame) return;

if (!gl_handle)
glGenTextures(1, &gl_handle);
GLenum err = glGetError();

auto format = frame.get_profile().format();
auto width = frame.get_width();
auto height = frame.get_height();
stream = frame.get_profile().stream_type();

glBindTexture(GL_TEXTURE_2D, gl_handle);

switch (format)
{
case RS2_FORMAT_RGB8:
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, frame.get_data());
break;
case RS2_FORMAT_RGBA8:
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, frame.get_data());
break;
case RS2_FORMAT_Y8:
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, frame.get_data());
break;
default:
throw std::runtime_error("The requested format is not supported by this demo!");
}

glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
glBindTexture(GL_TEXTURE_2D, 0);
}

void show(const rect& r) const
{
if (!gl_handle)
return;

set_viewport(r);

glBindTexture(GL_TEXTURE_2D, gl_handle);
glEnable(GL_TEXTURE_2D);
glBegin(GL_QUADS);
glTexCoord2f(0, 0); glVertex2f(0, 0);
glTexCoord2f(0, 1); glVertex2f(0, r.h);
glTexCoord2f(1, 1); glVertex2f(r.w, r.h);
glTexCoord2f(1, 0); glVertex2f(r.w, 0);
glEnd();
glDisable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, 0);
draw_text(int(0.05f * r.w), int(r.h - 0.05f*r.h), rs2_stream_to_string(stream));
}

GLuint get_gl_handle() { return gl_handle; }
};

class imu_drawer
class imu_renderer
{
public:
void render(const rs2::motion_frame& frame, const rect& r)
Expand All @@ -196,7 +122,7 @@ class imu_drawer
glGenTextures(1, &_gl_handle);

set_viewport(r);
draw_text(int(0.05f * r.w), int(r.h - 0.1f*r.h), f.get_profile().stream_name().c_str());
draw_text(int(0.05f * r.w), int(0.05f * r.h), f.get_profile().stream_name().c_str());

auto md = f.get_motion_data();
auto x = md.x;
Expand All @@ -218,7 +144,7 @@ class imu_drawer
glRotatef(180, 0.0f, 0.0f, 1.0f);
glRotatef(-90, 0.0f, 1.0f, 0.0f);

draw_axes();
draw_axes(1,2);

draw_circle(1, 0, 0, 0, 1, 0);
draw_circle(0, 1, 0, 0, 0, 1);
Expand Down Expand Up @@ -247,7 +173,7 @@ class imu_drawer
}
else
{
auto vectorWidth = 5.f;
auto vectorWidth = 3.f;
glLineWidth(vectorWidth);
glBegin(GL_LINES);
glColor3f(1.0f, 1.0f, 1.0f);
Expand Down Expand Up @@ -397,6 +323,154 @@ class imu_drawer

};

class pose_renderer
{
public:
void render(const rs2::pose_frame& frame, const rect& r)
{
draw_pose(frame, r.adjust_ratio({ IMU_FRAME_WIDTH, IMU_FRAME_HEIGHT }));
}

GLuint get_gl_handle() { return _gl_handle; }

private:
mutable GLuint _gl_handle = 0;

// Provide textual representation only
void draw_pose(const rs2::pose_frame& f, const rect& r)
{
if (!_gl_handle)
glGenTextures(1, &_gl_handle);

set_viewport(r);
std::string caption(f.get_profile().stream_name());
if (f.get_profile().stream_index())
caption += std::to_string(f.get_profile().stream_index());
draw_text(int(0.05f * r.w), int(0.05f * r.h), caption.c_str());

auto pose = f.get_pose_data();
std::stringstream ss;
ss << "Pos (meter): \t\t" << std::fixed << std::setprecision(2) << pose.translation.x << ", " << pose.translation.y << ", " << pose.translation.z;
draw_text(int(0.05f * r.w), int(0.2f*r.h), ss.str().c_str());
ss.clear(); ss.str("");
ss << "Orient (quaternion): \t" << pose.rotation.x << ", " << pose.rotation.y << ", " << pose.rotation.z << ", " << pose.rotation.w;
draw_text(int(0.05f * r.w), int(0.3f*r.h), ss.str().c_str());
ss.clear(); ss.str("");
ss << "Lin Velocity (m/sec): \t" << pose.velocity.x << ", " << pose.velocity.y << ", " << pose.velocity.z;
draw_text(int(0.05f * r.w), int(0.4f*r.h), ss.str().c_str());
ss.clear(); ss.str("");
ss << "Ang. Velocity (rad/sec): \t" << pose.angular_velocity.x << ", " << pose.angular_velocity.y << ", " << pose.angular_velocity.z;
draw_text(int(0.05f * r.w), int(0.5f*r.h), ss.str().c_str());
}
};

/// \brief Print flat 2D text over openGl window
struct text_renderer
{
// Provide textual representation only
void put_text(const std::string& msg, float norm_x_pos, float norm_y_pos, const rect& r)
{
set_viewport(r);
draw_text(int(norm_x_pos * r.w), int(norm_y_pos * r.h), msg.c_str());
}
};

////////////////////////
// Image display code //
////////////////////////
/// \brief The texture class
class texture
{
public:

void upload(const rs2::video_frame& frame)
{
if (!frame) return;

if (!_gl_handle)
glGenTextures(1, &_gl_handle);
GLenum err = glGetError();

auto format = frame.get_profile().format();
auto width = frame.get_width();
auto height = frame.get_height();
_stream_type = frame.get_profile().stream_type();
_stream_index = frame.get_profile().stream_index();

glBindTexture(GL_TEXTURE_2D, _gl_handle);

switch (format)
{
case RS2_FORMAT_RGB8:
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, frame.get_data());
break;
case RS2_FORMAT_RGBA8:
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, frame.get_data());
break;
case RS2_FORMAT_Y8:
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, frame.get_data());
break;
default:
throw std::runtime_error("The requested format is not supported by this demo!");
}

glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
glBindTexture(GL_TEXTURE_2D, 0);
}

void show(const rect& r) const
{
if (!_gl_handle)
return;

set_viewport(r);

glBindTexture(GL_TEXTURE_2D, _gl_handle);
glEnable(GL_TEXTURE_2D);
glBegin(GL_QUADS);
glTexCoord2f(0, 0); glVertex2f(0, 0);
glTexCoord2f(0, 1); glVertex2f(0, r.h);
glTexCoord2f(1, 1); glVertex2f(r.w, r.h);
glTexCoord2f(1, 0); glVertex2f(r.w, 0);
glEnd();
glDisable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, 0);
draw_text(int(0.05f * r.w), int(0.05f * r.h), rs2_stream_to_string(_stream_type));
}

GLuint get_gl_handle() { return _gl_handle; }

void render(const rs2::frame& frame, const rect& rect)
{
if (auto vf = frame.as<rs2::video_frame>())
{
upload(vf);
show(rect.adjust_ratio({ (float)vf.get_width(), (float)vf.get_height() }));
}
else if (auto mf = frame.as<rs2::motion_frame>())
{
_imu_render.render(frame, rect.adjust_ratio({ IMU_FRAME_WIDTH, IMU_FRAME_HEIGHT }));
}
else if (auto pf = frame.as<rs2::pose_frame>())
{
_pose_render.render(frame, rect.adjust_ratio({ IMU_FRAME_WIDTH, IMU_FRAME_HEIGHT }));
}
else
throw std::runtime_error("Rendering is currently supported for video, motion and pose frames only");
}

private:
GLuint _gl_handle = 0;
rs2_stream _stream_type = RS2_STREAM_ANY;
int _stream_index{};
imu_renderer _imu_render;
pose_renderer _pose_render;
};

class window
{
public:
Expand Down Expand Up @@ -443,6 +517,12 @@ class window
});
}

~window()
{
glfwDestroyWindow(win);
glfwTerminate();
}

float width() const { return float(_width); }
float height() const { return float(_height); }

Expand All @@ -468,12 +548,6 @@ class window
return res;
}

~window()
{
glfwDestroyWindow(win);
glfwTerminate();
}

void show(rs2::frame frame)
{
show(frame, { 0, 0, (float)_width, (float)_height });
Expand All @@ -486,15 +560,44 @@ class window
if (auto vf = frame.as<rs2::video_frame>())
render_video_frame(vf, rect);
if (auto mf = frame.as<rs2::motion_frame>())
render_motoin_frame(mf, rect);
render_motion_frame(mf, rect);
if (auto pf = frame.as<rs2::pose_frame>())
render_pose_frame(pf, rect);
}

void show(const std::map<int, rs2::frame> frames)
{
// Render openGl mosaic of frames
if (frames.size())
{
int cols = int(std::ceil(std::sqrt(frames.size())));
int rows = int(std::ceil(frames.size() / static_cast<float>(cols)));

float view_width = float(_width / cols);
float view_height = float(_height / rows);
int stream_no =0;
for (auto& frame : frames)
{
rect viewport_loc{ view_width * (stream_no % cols), view_height * (stream_no / cols), view_width, view_height };
show(frame.second, viewport_loc);
stream_no++;
}
}
else
{
_main_win.put_text("Connect one or more Intel RealSense devices and rerun the example",
0.4f, 0.5f, { 0.f,0.f, float(_width) , float(_height) });
}
}

operator GLFWwindow*() { return win; }

private:
GLFWwindow * win;
std::map<int, texture> _textures;
std::map<int, imu_drawer> _imus;
std::map<int, imu_renderer> _imus;
std::map<int, pose_renderer> _poses;
text_renderer _main_win;
int _width, _height;

void render_video_frame(const rs2::video_frame& f, const rect& r)
Expand All @@ -503,12 +606,18 @@ class window
t.render(f, r);
}

void render_motoin_frame(const rs2::motion_frame& f, const rect& r)
void render_motion_frame(const rs2::motion_frame& f, const rect& r)
{
auto& i = _imus[f.get_profile().unique_id()];
i.render(f, r);
}

void render_pose_frame(const rs2::pose_frame& f, const rect& r)
{
auto& i = _poses[f.get_profile().unique_id()];
i.render(f, r);
}

void render_frameset(const rs2::frameset& frames, const rect& r)
{
std::vector<rs2::frame> supported_frames;
Expand Down
Loading