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