void raster_curve(float tf_x0, float tf_y0, float tf_x1, float tf_y1, float tf_x2, float tf_y2){ float tf_x11; float tf_y11; float tf_x22; float tf_y22; float tf_x; float tf_y; if(fabs(tf_x2 - tf_x0) <= 1 && fabs(tf_y2 - tf_y0) <= 1){ raster_line(tf_x0, tf_y0, tf_x2, tf_y2); } else{ tf_x11 = (tf_x0 + tf_x1) / 2; tf_y11 = (tf_y0 + tf_y1) / 2; tf_x22 = (tf_x2 + tf_x1) / 2; tf_y22 = (tf_y2 + tf_y1) / 2; tf_x = (tf_x11 + tf_x22) / 2; tf_y = (tf_y11 + tf_y22) / 2; raster_curve(tf_x0, tf_y0, tf_x11, tf_y11, tf_x, tf_y); raster_curve(tf_x, tf_y, tf_x22, tf_y22, tf_x2, tf_y2); } }
int main(int argc, char * argv[]) try { std::cout << "Waiting for device..." << std::endl; // Declare RealSense pipeline, encapsulating the actual device and sensors rs2::pipeline pipe; // Create a configuration for configuring the pipeline with a non default profile rs2::config cfg; // Enable fisheye and pose streams cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF); cfg.enable_stream(RS2_STREAM_FISHEYE, 1); cfg.enable_stream(RS2_STREAM_FISHEYE, 2); // Start pipeline with chosen configuration rs2::pipeline_profile pipe_profile = pipe.start(cfg); // T265 has two fisheye sensors, we can choose any of them (index 1 or 2) const int fisheye_sensor_idx = 1; // Get fisheye sensor intrinsics parameters rs2::stream_profile fisheye_stream = pipe_profile.get_stream(RS2_STREAM_FISHEYE, fisheye_sensor_idx); rs2_intrinsics intrinsics = fisheye_stream.as<rs2::video_stream_profile>().get_intrinsics(); // Get fisheye sensor extrinsics parameters. // This is the pose of the fisheye sensor relative to the T265 coordinate system. rs2_extrinsics extrinsics = fisheye_stream.get_extrinsics_to(pipe_profile.get_stream(RS2_STREAM_POSE)); std::cout << "Device got. Streaming data" << std::endl; // Create an OpenGL display window and a texture to draw the fisheye image window app(intrinsics.width, intrinsics.height, "Intel RealSense T265 Augmented Reality Example"); window_key_listener key_watcher(app); texture fisheye_image; // Create the vertices of a simple virtual object. // This virtual object is 4 points in 3D space that describe 3 XYZ 20cm long axes. // These vertices are relative to the object's own coordinate system. const float length = 0.20; const object virtual_object = {{ { 0, 0, 0 }, // origin { length, 0, 0 }, // X { 0, length, 0 }, // Y { 0, 0, length } // Z }}; // This variable will hold the pose of the virtual object in world coordinates. // We we initialize it once we get the first pose frame. rs2_pose object_pose_in_world; bool object_pose_in_world_initialized = false; // Main loop while (app) { rs2_pose device_pose_in_world; // This will contain the current device pose { // Wait for the next set of frames from the camera auto frames = pipe.wait_for_frames(); // Get a frame from the fisheye stream rs2::video_frame fisheye_frame = frames.get_fisheye_frame(fisheye_sensor_idx); // Get a frame from the pose stream rs2::pose_frame pose_frame = frames.get_pose_frame(); // Copy current camera pose device_pose_in_world = pose_frame.get_pose_data(); // Render the fisheye image fisheye_image.render(fisheye_frame, { 0, 0, app.width(), app.height() }); // By closing the current scope we let frames be deallocated, so we do not fill up librealsense queues while we do other computation. } // If we have not set the virtual object in the world yet, set it in front of the camera now. if (!object_pose_in_world_initialized) { object_pose_in_world = reset_object_pose(device_pose_in_world); object_pose_in_world_initialized = true; } // Compute the pose of the object relative to the current pose of the device rs2_pose world_pose_in_device = pose_inverse(device_pose_in_world); rs2_pose object_pose_in_device = pose_multiply(world_pose_in_device, object_pose_in_world); // Get the object vertices in device coordinates object object_in_device = convert_object_coordinates(virtual_object, object_pose_in_device); // Convert object vertices from device coordinates into fisheye sensor coordinates using extrinsics object object_in_sensor; for (size_t i = 0; i < object_in_device.size(); ++i) { rs2_transform_point_to_point(object_in_sensor[i].f, &extrinsics, object_in_device[i].f); } for (size_t i = 1; i < object_in_sensor.size(); ++i) { // Discretize the virtual object line into smaller 1cm long segments std::vector<point3d> points_in_sensor = raster_line(object_in_sensor[0], object_in_sensor[i], 0.01); std::vector<pixel> projected_line; projected_line.reserve(points_in_sensor.size()); for (auto& point : points_in_sensor) { // A 3D point is visible in the image if its Z coordinate relative to the fisheye sensor is positive. if (point.z() > 0) { // Project 3D sensor coordinates to 2D fisheye image coordinates using intrinsics projected_line.emplace_back(); rs2_project_point_to_pixel(projected_line.back().f, &intrinsics, point.f); } } // Display the line in the image render_line(projected_line, i); } // Display text in the image render_text(app.height(), "Press spacebar to reset the pose of the virtual object. Press ESC to exit"); // Check if some key is pressed switch (key_watcher.get_key()) { case GLFW_KEY_SPACE: // Reset virtual object pose if user presses spacebar object_pose_in_world = reset_object_pose(device_pose_in_world); std::cout << "Setting new pose for virtual object: " << object_pose_in_world.translation << std::endl; break; case GLFW_KEY_ESCAPE: // Exit if user presses escape app.close(); break; } } return EXIT_SUCCESS; } catch (const rs2::error & e) { std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl; return EXIT_FAILURE; } catch (const std::exception& e) { std::cerr << e.what() << std::endl; return EXIT_FAILURE; }
static void draw_lines(struct graphics_priv *gr, struct graphics_gc_priv *gc, struct point *p, int count) { if ((gr->overlay_parent && !gr->overlay_parent->overlay_enable) || (gr->overlay_parent && gr->overlay_parent->overlay_enable && !gr->overlay_enable) ) { return; } /* you might expect lines to be simpler than the other shapes. but, that would be wrong. 1 line can generate 1 polygon + 2 circles and even worse, we have to calculate their parameters! go dust off your trigonometry hat. */ /* sort of based on graphics_opengl.c::draw_lines */ /* FIXME: should honor ./configure flag for no fp. this could be 100% integer code pretty easily, except that i am lazy */ struct point vert[4]; int lw = gc->linewidth; //int lw = 1; int i; for(i = 0; i < count-1; i++) { float dx=p[i+1].x-p[i].x; float dy=p[i+1].y-p[i].y; float angle; int x_lw_adj, y_lw_adj; if(lw == 1) { if(gr->aa) { raster_aaline(gr->screen, p[i].x, p[i].y, p[i+1].x, p[i+1].y, SDL_MapRGBA(gr->screen->format, gc->fore_r, gc->fore_g, gc->fore_b, gc->fore_a)); } else { raster_line(gr->screen, p[i].x, p[i].y, p[i+1].x, p[i+1].y, SDL_MapRGBA(gr->screen->format, gc->fore_r, gc->fore_g, gc->fore_b, gc->fore_a)); } } else { /* there is probably a much simpler way but this works ok */ /* FIXME: float + double mixture */ /* FIXME: lrint(round())? */ if(dy == 0.0) { angle = 0.0; x_lw_adj = 0; y_lw_adj = round((float)lw/2.0); } else if(dx == 0.0) { angle = 0.0; x_lw_adj = round((float)lw/2.0); y_lw_adj = 0; } else { angle = (M_PI/2.0) - atan(abs(dx)/abs(dy)); x_lw_adj = round(sin(angle)*(float)lw/2.0); y_lw_adj = round(cos(angle)*(float)lw/2.0); if((x_lw_adj < 0) || (y_lw_adj < 0)) { dbg(lvl_debug, "i=%d\n", i); dbg(lvl_debug, " %d,%d->%d,%d\n", p[i].x, p[i].y, p[i+1].x, p[i+1].y); dbg(lvl_debug, " lw=%d angle=%f\n", lw, 180.0 * angle / M_PI); dbg(lvl_debug, " x_lw_adj=%d y_lw_adj=%d\n", x_lw_adj, y_lw_adj); } } if(p[i+1].x > p[i].x) { x_lw_adj = -x_lw_adj; } if(p[i+1].y > p[i].y) { y_lw_adj = -y_lw_adj; } /* FIXME: draw a circle/square if p[i]==p[i+1]? */ /* FIXME: clipping, check for neg values. hoping sdl-gfx does this */ vert[0].x = p[i].x + x_lw_adj; vert[0].y = p[i].y - y_lw_adj; vert[1].x = p[i].x - x_lw_adj; vert[1].y = p[i].y + y_lw_adj; vert[2].x = p[i+1].x - x_lw_adj; vert[2].y = p[i+1].y + y_lw_adj; vert[3].x = p[i+1].x + x_lw_adj; vert[3].y = p[i+1].y - y_lw_adj; draw_polygon(gr, gc, vert, 4); /* draw small circles at the ends. this looks better than nothing, and slightly * better than the triangle used by graphics_opengl, but is more expensive. * should have an ifdef/xml attr? */ /* FIXME: should just draw a half circle */ /* now some circular endcaps, if the width is over 2 */ if(lw > 2) { if(i == 0) { draw_circle(gr, gc, &p[i], lw/2); } /* we truncate on the divide on purpose, so we don't go outside the line */ draw_circle(gr, gc, &p[i+1], lw/2); } } } }