8 #define GL_SILENCE_DEPRECATION 9 #define GLFW_INCLUDE_GLU 21 #include "../third-party/stb_easy_font.h" 24 #define PI 3.14159265358979323846 25 #define PI_FL 3.141592f 39 return { x *
t, y *
t, z * t };
44 return { x -
t, y -
t, z - t };
78 auto H =
static_cast<float>(
h),
W = static_cast<float>(h) * size.
x / size.
y;
86 return{ x + (w -
W) / 2, y + (h -
H) / 2,
W,
H };
131 draw_motion(frame, r.
adjust_ratio({ IMU_FRAME_WIDTH, IMU_FRAME_HEIGHT }));
156 glOrtho(-2.8, 2.8, -2.4, 2.4, -7, 7);
169 draw_circle(1, 0, 0, 0, 1, 0);
170 draw_circle(0, 1, 0, 0, 0, 1);
171 draw_circle(1, 0, 0, 0, 0, 1);
173 const auto canvas_size = 230;
174 const auto vec_threshold = 0.01f;
175 float norm = std::sqrt(
x *
x +
y *
y +
z *
z);
176 if (norm < vec_threshold)
178 const auto radius = 0.05;
179 static const int circle_points = 100;
180 static const float angle = 2.0f * 3.1416f / circle_points;
185 glVertex2d(radius * cos(0.0), radius * sin(0.0));
187 for (i = 0; i < circle_points; i++)
189 glVertex2d(radius * cos(angle1), radius * sin(angle1));
196 auto vectorWidth = 3.f;
211 glOrtho(-canvas_size, canvas_size, -canvas_size, canvas_size, -1, +1);
213 std::ostringstream
s1;
218 s1 <<
"(" << std::fixed << std::setprecision(
precision) <<
x <<
"," << std::fixed << std::setprecision(
precision) <<
y <<
"," << std::fixed << std::setprecision(
precision) << z <<
")";
219 print_text_in_3d(
x,
y, z, s1.str().c_str(),
false,
model, proj, 1 / norm);
221 std::ostringstream s2;
222 s2 << std::setprecision(
precision) << norm;
223 print_text_in_3d(
x / 2,
y / 2, z / 2, s2.str().c_str(),
true,
model, proj, 1 / norm);
233 for (
int i = 0;
i < N;
i++)
236 for (
int j = 0;
j < N;
j++)
238 result[
i] += vec[
j] * mat[N *
j +
i];
250 const auto canvas_size = 230;
252 multiply_vector_by_matrix(vec, model, tmp_result);
253 multiply_vector_by_matrix(tmp_result, proj, result);
255 return{ canvas_size * vec_norm * result[0], canvas_size * vec_norm * result[1] };
260 auto xy = xyz_to_xy(x, y, z, model, proj, vec_norm);
263 draw_text((
int)(xy.x -
w / 2), (
int)xy.y, text);
266 static void draw_axes(
float axis_size = 1.
f,
float axisWidth = 4.
f)
272 glVertex3f(axis_size, -axis_size * 0.05f, 0.f);
273 glVertex3f(axis_size, axis_size * 0.05f, 0.f);
275 glVertex3f(axis_size, 0.f, -axis_size * 0.05f);
276 glVertex3f(axis_size, 0.f, axis_size * 0.05f);
283 glVertex3f(0.f, axis_size, 0.05f * axis_size);
284 glVertex3f(0.f, axis_size, -0.05f * axis_size);
286 glVertex3f(0.05f * axis_size, axis_size, 0.f);
287 glVertex3f(-0.05f * axis_size, axis_size, 0.f);
294 glVertex3f(0.0f, 0.05f * axis_size, 1.0f * axis_size);
295 glVertex3f(0.0f, -0.05f * axis_size, 1.0f * axis_size);
297 glVertex3f(0.05f * axis_size, 0.f, 1.0f * axis_size);
298 glVertex3f(-0.05f * axis_size, 0.f, 1.0f * axis_size);
323 static void draw_circle(
float xx,
float xy,
float xz,
float yx,
float yy,
float yz,
float radius = 1.1,
float3 center = { 0.0, 0.0, 0.0 },
float intensity = 0.5f)
326 glColor3f(intensity, intensity, intensity);
330 for (
int i = 0;
i <= N;
i++)
332 const double theta = (2 *
PI / N) *
i;
333 const auto cost =
static_cast<float>(cos(theta));
334 const auto sint =
static_cast<float>(sin(theta));
336 center.x + radius * (xx * cost + yx * sint),
337 center.y + radius * (xy * cost + yy * sint),
338 center.z + radius * (xz * cost + yz * sint)
351 draw_pose(frame, r.
adjust_ratio({ IMU_FRAME_WIDTH, IMU_FRAME_HEIGHT }));
369 draw_text(
int(0.05f * r.
w),
int(0.05f * r.
h), caption.c_str());
372 std::stringstream ss;
373 ss <<
"Pos (meter): \t\t" << std::fixed << std::setprecision(2) << pose.
translation.
x <<
", " << pose.translation.y <<
", " << pose.translation.z;
374 draw_text(
int(0.05f * r.
w),
int(0.2f * r.
h), ss.str().c_str());
375 ss.clear(); ss.str(
"");
376 ss <<
"Orient (quaternion): \t" << pose.rotation.x <<
", " << pose.rotation.y <<
", " << pose.rotation.z <<
", " << pose.rotation.w;
377 draw_text(
int(0.05f * r.
w),
int(0.3f * r.
h), ss.str().c_str());
378 ss.clear(); ss.str(
"");
379 ss <<
"Lin Velocity (m/sec): \t" << pose.velocity.x <<
", " << pose.velocity.y <<
", " << pose.velocity.z;
380 draw_text(
int(0.05f * r.
w),
int(0.4f * r.
h), ss.str().c_str());
381 ss.clear(); ss.str(
"");
382 ss <<
"Ang. Velocity (rad/sec): \t" << pose.angular_velocity.x <<
", " << pose.angular_velocity.y <<
", " << pose.angular_velocity.z;
383 draw_text(
int(0.05f * r.
w),
int(0.5f * r.
h), ss.str().c_str());
394 draw_text(
int(norm_x_pos * r.
w),
int(norm_y_pos * r.
h), msg.c_str());
437 throw std::runtime_error(
"The requested format is not supported by this demo!");
476 show(rect.
adjust_ratio({ (float)vf.get_width(), (float)vf.get_height() }),
alpha);
480 _imu_render.render(frame, rect.
adjust_ratio({ IMU_FRAME_WIDTH, IMU_FRAME_HEIGHT }));
484 _pose_render.render(frame, rect.
adjust_ratio({ IMU_FRAME_WIDTH, IMU_FRAME_HEIGHT }));
487 throw std::runtime_error(
"Rendering is currently supported for video, motion and pose frames only");
501 std::function<void(bool)> on_left_mouse = [](bool) {};
503 std::function<void(double, double)> on_mouse_move = [](double, double) {};
504 std::function<void(int)> on_key_release = [](int) {};
507 : _width(width), _height(height), _canvas_left_top_x(0), _canvas_left_top_y(0), _canvas_width(width), _canvas_height(height)
512 throw std::runtime_error(
"Could not open OpenGL window, please check your graphic drivers or use the textual SDK tools");
519 if (button == 0)
s->on_left_mouse(action ==
GLFW_PRESS);
525 s->on_mouse_scroll(xoffset, yoffset);
531 s->on_mouse_move(x, y);
539 s->on_key_release(key);
545 window(
unsigned width,
unsigned height,
const char* title,
unsigned tiles_in_row,
unsigned tiles_in_col,
float canvas_width = 0.8
f,
546 float canvas_height = 0.6
f,
float canvas_left_top_x = 0.1
f,
float canvas_left_top_y = 0.075
f)
547 : _width(width), _height(height), _tiles_in_row(tiles_in_row), _tiles_in_col(tiles_in_col)
551 if (canvas_width < 0 || canvas_width > 1 || canvas_height < 0 || canvas_height > 1 ||
552 canvas_left_top_x < 0 || canvas_left_top_x > 1 || canvas_left_top_y < 0 || canvas_left_top_y > 1)
554 std::cout <<
"Invalid window's size parameter entered, setting to default values" << std::endl;
556 canvas_height = 0.6f;
557 canvas_left_top_x = 0.15f;
558 canvas_left_top_y = 0.075f;
562 if (_tiles_in_row <= 0) {
565 if (_tiles_in_col <= 0) {
570 _canvas_width = int(_width * canvas_width);
571 _canvas_height = int(_height * canvas_height);
572 _canvas_left_top_x = _width * canvas_left_top_x;
573 _canvas_left_top_y = _height * canvas_left_top_y;
576 _tile_width_pixels = float(std::floor(_canvas_width / _tiles_in_row));
577 _tile_height_pixels = float(std::floor(_canvas_height / _tiles_in_col));
584 throw std::runtime_error(
"Could not open OpenGL window, please check your graphic drivers or use the textual SDK tools");
591 if (button == 0)
s->on_left_mouse(action ==
GLFW_PRESS);
597 s->on_mouse_scroll(xoffset, yoffset);
603 s->on_mouse_move(x, y);
611 s->on_key_release(key);
627 float width()
const {
return float(_width); }
628 float height()
const {
return float(_height); }
647 glOrtho(0, _width, _height, 0, -1, +1);
654 show(frame, { 0, 0, (float)_width, (
float)_height });
660 render_frameset(fs, rect);
662 render_video_frame(vf, rect);
664 render_motion_frame(mf, rect);
666 render_pose_frame(pf, rect);
674 int cols = int(std::ceil(std::sqrt(frames.size())));
675 int rows = int(std::ceil(frames.size() /
static_cast<float>(cols)));
677 float view_width = float(_width / cols);
678 float view_height = float(_height / rows);
679 unsigned int stream_no = 0;
680 for (
auto& frame : frames)
682 rect viewport_loc{ view_width * (stream_no % cols), view_height * (stream_no / cols), view_width, view_height };
683 show(frame.second, viewport_loc);
689 _main_win.put_text(
"Connect one or more Intel RealSense devices and rerun the example",
690 0.4
f, 0.5
f, { 0.f,0.f, float(_width) , float(_height) });
702 std::vector <frame_and_tile_property> vector_frames;
704 for (
const auto& frame : frames) { vector_frames.push_back(frame.second); }
706 std::sort(vector_frames.begin(), vector_frames.end(),
709 return frame1.second.priority < frame2.second.priority;
712 float frame_width_size_from_tile_width = 0.98f;
714 for (
const auto& frame : vector_frames)
717 rect viewport_loc{ _tile_width_pixels * attr.
x + _canvas_left_top_x, _tile_height_pixels * attr.
y + _canvas_left_top_y,
718 _tile_width_pixels * attr.
w * frame_width_size_from_tile_width, _tile_height_pixels * attr.
h };
719 show(frame.first, viewport_loc);
724 _main_win.put_text(
"Connect one or more Intel RealSense devices and rerun the example",
725 0.3
f, 0.5
f, { float(_canvas_left_top_x), float(_canvas_left_top_y), float(_canvas_width) , float(_canvas_height) });
763 std::vector<rs2::frame> supported_frames;
764 for (
auto f : frames)
767 supported_frames.push_back(
f);
769 if (supported_frames.empty())
775 auto image_grid =
calc_grid(r, supported_frames);
778 for (
auto f : supported_frames)
780 auto r = image_grid.at(image_index);
804 if (r.
w <= 0 || r.
h <= 0 || streams <= 0)
805 throw std::runtime_error(
"invalid window configuration request, failed to calculate window grid");
806 float ratio = r.
w / r.
h;
807 auto x = sqrt(ratio * (
float)streams);
808 auto y = (float)streams /
x;
811 if (
w == 0 ||
h == 0)
812 throw std::runtime_error(
"invalid window configuration request, failed to calculate window grid");
813 while (
w *
h > streams)
815 while (
w*
h < streams)
817 auto new_w = round(r.
w /
w);
818 auto new_h = round(r.
h /
h);
820 return rect{
static_cast<float>(
w), static_cast<float>(
h),
static_cast<float>(new_w), static_cast<float>(new_h) };
827 std::vector<rect> rv;
830 for (
int i = 0;
i < frames.size();
i++)
832 auto mod =
i % (int)
grid.x;
835 if (
auto vf = frames[
i].as<rs2::video_frame>())
837 fw = (float)vf.get_width();
838 fh = (float)vf.get_height();
840 float cell_x_postion = (float)(mod *
grid.w);
841 if (mod == 0) curr_line++;
842 float cell_y_position = curr_line *
grid.h;
844 auto r =
rect{ cell_x_postion + margin.
x, cell_y_position + margin.
y,
grid.w - 2 * margin.
x,
grid.h };
874 ml(false), offset_x(2.
f), offset_y(2.
f), tex() {}
900 gluPerspective(60, width / height, 0.01
f, 10.0
f);
904 gluLookAt(0, 0, 0, 0, 0, 1, 0, -1, 0);
915 float tex_border_color[] = { 0.8f, 0.8f, 0.8f, 0.8f };
925 for (
int i = 0;
i < points.
size();
i++)
945 H[0] = 1 - 2 * q.
y * q.
y - 2 * q.
z * q.
z; H[4] = 2 * q.
x * q.
y - 2 * q.
z * q.
w; H[8] = 2 * q.
x * q.
z + 2 * q.
y * q.
w; H[12] = 0.0f;
946 H[1] = 2 * q.
x * q.
y + 2 * q.
z * q.
w; H[5] = 1 - 2 * q.
x * q.
x - 2 * q.
z * q.
z; H[9] = 2 * q.
y * q.
z - 2 * q.
x * q.
w; H[13] = 0.0f;
947 H[2] = 2 * q.
x * q.
z - 2 * q.
y * q.
w; H[6] = 2 * q.
y * q.
z + 2 * q.
x * q.
w; H[10] = 1 - 2 * q.
x * q.
x - 2 * q.
y * q.
y; H[14] = 0.0f;
948 H[3] = 0.0f; H[7] = 0.0f; H[11] = 0.0f; H[15] = 1.0f;
966 gluPerspective(60, width / height, 0.01
f, 10.0
f);
983 for (
auto&&
v : trajectory)
1009 float tex_border_color[] = { 0.8f, 0.8f, 0.8f, 0.8f };
1018 for (
int i = 0;
i < points.
size();
i++)
1041 app_state.
ml = pressed;
1054 app_state.
yaw -= (x - app_state.
last_x);
1055 app_state.
yaw = std::max(app_state.
yaw, -120.0);
1058 app_state.
pitch = std::max(app_state.
pitch, -80.0);
static void draw_axes(float axis_size=1.f, float axisWidth=4.f)
bool can_render(const rs2::frame &f) const
#define glDisableClientState
std::function< void(bool)> on_left_mouse
#define GL_TEXTURE_MAG_FILTER
void render(const rs2::motion_frame &frame, const rect &r)
#define glEnableClientState
void print_text_in_3d(float x, float y, float z, const char *text, bool center_text, GLfloat model[], GLfloat proj[], float vec_norm)
GLFWAPI void glfwGetWindowSize(GLFWwindow *window, int *width, int *height)
Retrieves the size of the client area of the specified window.
void render(const rs2::frame &frame, const rect &rect, float alpha=1.f)
window_key_listener(window &win)
The header of the GLFW 3 API.
void draw_motion(const rs2::motion_frame &f, const rect &r)
window(int width, int height, const char *title)
void put_text(const std::string &msg, float norm_x_pos, float norm_y_pos, const rect &r)
void quat2mat(rs2_quaternion &q, GLfloat H[16])
GLenum GLenum GLenum GLenum GLenum scale
void render(const rs2::pose_frame &frame, const rect &r)
static int stb_easy_font_print(float x, float y, char *text, unsigned char color[4], void *vertex_buffer, int vbuf_size)
GLFWAPI void * glfwGetWindowUserPointer(GLFWwindow *window)
Returns the user pointer of the specified window.
stream_profile get_profile() const
std::vector< rect > calc_grid(rect r, std::vector< rs2::frame > &frames)
GLuint GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat t1
float2 xyz_to_xy(float x, float y, float z, GLfloat model[], GLfloat proj[], float vec_norm)
void show(const rs2::frame &frame, const rect &rect)
float3 operator*(float t)
const void * get_data() const
std::string stream_name() const
GLdouble GLdouble GLdouble w
GLsizei const GLchar *const * string
rect adjust_ratio(float2 size) const
static void draw_circle(float xx, float xy, float xz, float yx, float yy, float yz, float radius=1.1, float3 center={0.0, 0.0, 0.0}, float intensity=0.5f)
GLfloat GLfloat GLfloat GLfloat h
void operator=(float3 other)
const texture_coordinate * get_texture_coordinates() const
void sort(sort_type m_sort_type, const std::string &in, const std::string &out)
rs2_pose get_pose_data() const
void add(float t1, float t2, float t3)
::realsense_legacy_msgs::pose_< std::allocator< void > > pose
#define GL_TEXTURE_WRAP_T
void on_key_release(int key)
Quaternion used to represent rotation.
Print flat 2D text over openGl window.
GLFWAPI int glfwInit(void)
Initializes the GLFW library.
static const textual_icon grid
std::map< int, pose_renderer > _poses
GLfloat GLfloat GLfloat alpha
rs2_vector get_motion_data() const
GLFWAPI void glfwSetWindowUserPointer(GLFWwindow *window, void *pointer)
Sets the user pointer of the specified window.
#define GL_COLOR_BUFFER_BIT
std::map< int, imu_renderer > _imus
std::map< int, texture > _textures
GLenum GLint GLint * precision
void upload(const rs2::video_frame &frame)
void render_video_frame(const rs2::video_frame &f, const rect &r)
pose_renderer _pose_render
#define GL_PROJECTION_MATRIX
static const textual_icon upload
GLint GLsizei GLsizei height
#define GL_UNSIGNED_SHORT
std::map< int, frame_and_tile_property > frames_mosaic
GLFWAPI void glfwSwapBuffers(GLFWwindow *window)
Swaps the front and back buffers of the specified window.
GLint GLint GLsizei GLint GLenum format
GLFWAPI void glfwMakeContextCurrent(GLFWwindow *window)
Makes the context of the specified window current for the calling thread.
const vertex * get_vertices() const
def on_mouse_scroll(x, y, scroll_x, scroll_y)
#define GL_TEXTURE_MIN_FILTER
void draw_pose(const rs2::pose_frame &f, const rect &r)
void show(rs2::frame frame)
std::pair< rs2::frame, tile_properties > frame_and_tile_property
void multiply_vector_by_matrix(GLfloat vec[], GLfloat mat[], GLfloat *result)
void show(const frames_mosaic &frames)
#define GLFW_RESIZABLE
Window resize-ability window hint and attribute.
#define GL_DEPTH_BUFFER_BIT
rs2_stream
Streams are different types of data provided by RealSense devices.
GLFWAPI void glfwSetWindowShouldClose(GLFWwindow *window, int value)
Sets the close flag of the specified window.
void show(const std::map< int, rs2::frame > &frames)
GLFWAPI GLFWwindow * glfwCreateWindow(int width, int height, const char *title, GLFWmonitor *monitor, GLFWwindow *share)
Creates a window and its associated context.
std::function< void(int)> on_key_release
rect calc_grid(rect r, size_t streams)
rs2_format format() const
void draw_pointcloud(float width, float height, glfw_state &app_state, rs2::points &points)
glfw_state(float yaw=15.0, float pitch=15.0)
GLdouble GLdouble GLdouble q
const char * rs2_stream_to_string(rs2_stream stream)
static const struct @18 vertices[3]
GLint GLint GLint yoffset
static int stb_easy_font_width(char *text)
window(unsigned width, unsigned height, const char *title, unsigned tiles_in_row, unsigned tiles_in_col, float canvas_width=0.8f, float canvas_height=0.6f, float canvas_left_top_x=0.1f, float canvas_left_top_y=0.075f)
void show(const rect &r, float alpha=1.f) const
GLFWAPI void glfwDestroyWindow(GLFWwindow *window)
Destroys the specified window and its context.
GLFWAPI void glfwGetFramebufferSize(GLFWwindow *window, int *width, int *height)
Retrieves the size of the framebuffer of the specified window.
void draw_text(int x, int y, const char *text)
const float IMU_FRAME_HEIGHT
GLFWAPI void glfwTerminate(void)
Terminates the GLFW library.
#define GL_MODELVIEW_MATRIX
BOOST_MPL_AUX_ADL_BARRIER_NAMESPACE_OPEN typedef arg< 1 > _1
GLFWAPI void glfwPollEvents(void)
Processes all pending events.
#define GL_TEXTURE_BORDER_COLOR
std::function< void(double, double)> on_mouse_scroll
void render_motion_frame(const rs2::motion_frame &f, const rect &r)
GLuint GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat s1
void render_frameset(const rs2::frameset &frames, const rect &r)
#define GL_ALL_ATTRIB_BITS
#define GL_UNPACK_ROW_LENGTH
float3 operator-(float t)
std::function< void(double, double)> on_mouse_move
rs2_stream stream_type() const
void register_glfw_callbacks(window &app, glfw_state &app_state)
#define GL_TEXTURE_WRAP_S
struct GLFWwindow GLFWwindow
GLFWAPI void glfwWindowHint(int hint, int value)
Sets the specified window hint to the desired value.
void draw_pointcloud_wrt_world(float width, float height, glfw_state &app_state, rs2::points &points, rs2_pose &pose, float H_t265_d400[16], std::vector< rs2_vector > &trajectory)
GLdouble GLdouble GLint GLint const GLdouble * points
void set_viewport(const rect &r)
void render_pose_frame(const rs2::pose_frame &f, const rect &r)
GLFWAPI int glfwWindowShouldClose(GLFWwindow *window)
Checks the close flag of the specified window.
const float IMU_FRAME_WIDTH
std::string to_string(T value)