CameraWrapper::CameraWrapper(int frames)
{
// init camera
e = 0;
ctx = rs_create_context(RS_API_VERSION, &e);
check_error();
printf("There are %d connected RealSense devices.\n", rs_get_device_count(ctx, &e));
check_error();
if(rs_get_device_count(ctx, &e) == 0)
{
std::cout << "camera init broken" << std::cout;
}
else
{
dev = rs_get_device(ctx, 0, &e);
check_error();
printf("\nUsing device 0, an %s\n", rs_get_device_name(dev, &e));
check_error();
printf(" Serial number: %s\n", rs_get_device_serial(dev, &e));
check_error();
printf(" Firmware version: %s\n", rs_get_device_firmware_version(dev, &e));
check_error();
// set controls
// NOTE find good laser power, max 15
//rs_enable_stream_preset(dev, RS_STREAM_DEPTH, RS_PRESET_BEST_QUALITY, &e);
rs_enable_stream(dev, RS_STREAM_DEPTH, 640, 480, RS_FORMAT_Z16, 30, &e);
check_error();
rs_set_device_option(dev, RS_OPTION_F200_ACCURACY, 1, &e);
check_error();
rs_set_device_option(dev, RS_OPTION_F200_FILTER_OPTION, 0, &e);
check_error();
rs_enable_stream(dev, RS_STREAM_COLOR, 1920, 1080, RS_FORMAT_RGB8, 30, &e);
// rs_enable_stream_preset(dev, RS_STREAM_COLOR, RS_PRESET_BEST_QUALITY, &e);
check_error();
rs_enable_stream(dev, RS_STREAM_INFRARED, 640,480, RS_FORMAT_Y8, 30, &e);
//rs_enable_stream_preset(dev, RS_STREAM_INFRARED, RS_PRESET_BEST_QUALITY, &e);
check_error();
rs_start_device(dev, &e);
check_error();
// get intrinsics and convert them to opencv mat
// IR and DEPTH are the same for f200, so this is basically overhead
rs_get_stream_intrinsics(dev, RS_STREAM_DEPTH, &depth_intrin, &e);
check_error();
rs_get_device_extrinsics(dev, RS_STREAM_DEPTH, RS_STREAM_COLOR, &depth_to_color, &e);
check_error();
rs_get_stream_intrinsics(dev, RS_STREAM_COLOR, &color_intrin, &e);
check_error();
rs_get_stream_intrinsics(dev, RS_STREAM_INFRARED, &ir_intrin, &e);
check_error();
depthScale = rs_get_device_depth_scale(dev, &e);
check_error();
}
// set number of frames to record
setStackSize(frames);
}
int getRealsenseGenericCalibration(int devID,int streamID,struct calibration * calib)
{
fprintf(stderr,"For some reason rs_get_stream_intrinsics segfaults on call , so Calibration is disabled.. \n");
return 0;
calib->intrinsicParametersSet=0;
if (!device[devID].hasInit)
{
fprintf(stderr,"A Calibration was asked before setting up the streams .. \n");
return 0;
}
snapRealsenseFrames(devID);
getRealsenseColorPixels(devID) ;
getRealsenseDepthPixels(devID) ;
fprintf(stderr,"rs_intrinsics is now declared \n");
rs_intrinsics intrin;
fprintf(stderr,"rs_get_stream_intrinsics will now be called\n");
rs_get_stream_intrinsics( device[devID].dev, streamID , &intrin, &e);
if ( !check_error() )
{
fprintf(stderr,"Survived Intrinsics");
calib->intrinsicParametersSet=1;
calib->k1=intrin.coeffs[0]; calib->k2=intrin.coeffs[1];
calib->p1=intrin.coeffs[2]; calib->p2=intrin.coeffs[3];
calib->k3=intrin.coeffs[4];
//clear first
calib->intrinsic[0]=0.0; calib->intrinsic[1]=0.0; calib->intrinsic[2]=0.0;
calib->intrinsic[3]=0.0; calib->intrinsic[4]=0.0; calib->intrinsic[5]=0.0;
calib->intrinsic[6]=0.0; calib->intrinsic[7]=0.0; calib->intrinsic[8]=1.0;
//fill after..
calib->intrinsic[CALIB_INTR_FX] = (double) intrin.fx/intrin.ppx;
calib->intrinsic[CALIB_INTR_FY] = (double) intrin.fy/intrin.ppy;
calib->width = intrin.width;
calib->height = intrin.height;
}
rs_extrinsics extrin={0};
rs_get_motion_extrinsics_from( device[devID].dev, device[devID].colorStreamToUse , &extrin, &e);
if (!check_error())
{
fprintf(stderr,"Survived Extrinsics");
calib->extrinsicParametersSet = 1;
calib->extrinsic[0] = extrin.rotation[0]; calib->extrinsic[1] = extrin.rotation[1]; calib->extrinsic[2] = extrin.rotation[2]; calib->extrinsic[3]=0;
calib->extrinsic[4] = extrin.rotation[3]; calib->extrinsic[5] = extrin.rotation[4]; calib->extrinsic[6] = extrin.rotation[5]; calib->extrinsic[7]=0;
calib->extrinsic[8] = extrin.rotation[6]; calib->extrinsic[9] = extrin.rotation[7]; calib->extrinsic[10] = extrin.rotation[8]; calib->extrinsic[11]=0;
calib->extrinsic[12]=0; calib->extrinsic[13]=0; calib->extrinsic[14]=0; calib->extrinsic[15]=1.0;
calib->extrinsicTranslation[0] = extrin.translation[0];
calib->extrinsicTranslation[1] = extrin.translation[1];
calib->extrinsicTranslation[2] = extrin.translation[2];
}
return calib->intrinsicParametersSet;
}
int main()
{
/* Turn on logging. We can separately enable logging to console or to file, and use different severity filters for each. */
rs_log_to_console(RS_LOG_SEVERITY_WARN, &e);
check_error();
/*rs_log_to_file(RS_LOG_SEVERITY_DEBUG, "librealsense.log", &e);
check_error();*/
/* Create a context object. This object owns the handles to all connected realsense devices. */
rs_context * ctx = rs_create_context(RS_API_VERSION, &e);
check_error();
printf("There are %d connected RealSense devices.\n", rs_get_device_count(ctx, &e));
check_error();
if(rs_get_device_count(ctx, &e) == 0) return EXIT_FAILURE;
/* This tutorial will access only a single device, but it is trivial to extend to multiple devices */
rs_device * dev = rs_get_device(ctx, 0, &e);
check_error();
printf("\nUsing device 0, an %s\n", rs_get_device_name(dev, &e));
check_error();
printf(" Serial number: %s\n", rs_get_device_serial(dev, &e));
check_error();
printf(" Firmware version: %s\n", rs_get_device_firmware_version(dev, &e));
check_error();
/* Configure depth and color to run with the device's preferred settings */
rs_enable_stream_preset(dev, RS_STREAM_DEPTH, RS_PRESET_BEST_QUALITY, &e);
check_error();
rs_enable_stream_preset(dev, RS_STREAM_COLOR, RS_PRESET_BEST_QUALITY, &e);
check_error();
rs_start_device(dev, RS_SOURCE_VIDEO, &e);
check_error();
/* Open a GLFW window to display our output */
glfwInit();
GLFWwindow * win = glfwCreateWindow(1280, 960, "librealsense tutorial #3", NULL, NULL);
glfwSetCursorPosCallback(win, on_cursor_pos);
glfwSetMouseButtonCallback(win, on_mouse_button);
glfwMakeContextCurrent(win);
while(!glfwWindowShouldClose(win))
{
/* Wait for new frame data */
glfwPollEvents();
rs_wait_for_frames(dev, &e);
check_error();
/* Retrieve our images */
const uint16_t * depth_image = (const uint16_t *)rs_get_frame_data(dev, RS_STREAM_DEPTH, &e);
check_error();
const uint8_t * color_image = (const uint8_t *)rs_get_frame_data(dev, RS_STREAM_COLOR, &e);
check_error();
/* Retrieve camera parameters for mapping between depth and color */
rs_intrinsics depth_intrin, color_intrin;
rs_extrinsics depth_to_color;
rs_get_stream_intrinsics(dev, RS_STREAM_DEPTH, &depth_intrin, &e);
check_error();
rs_get_device_extrinsics(dev, RS_STREAM_DEPTH, RS_STREAM_COLOR, &depth_to_color, &e);
check_error();
rs_get_stream_intrinsics(dev, RS_STREAM_COLOR, &color_intrin, &e);
check_error();
float scale = rs_get_device_depth_scale(dev, &e);
check_error();
/* Set up a perspective transform in a space that we can rotate by clicking and dragging the mouse */
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(60, (float)1280/960, 0.01f, 20.0f);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0,0,0, 0,0,1, 0,-1,0);
glTranslatef(0,0,+0.5f);
glRotated(pitch, 1, 0, 0);
glRotated(yaw, 0, 1, 0);
glTranslatef(0,0,-0.5f);
/* We will render our depth data as a set of points in 3D space */
glPointSize(2);
glEnable(GL_DEPTH_TEST);
glBegin(GL_POINTS);
int dx, dy;
for(dy=0; dy<depth_intrin.height; ++dy)
{
for(dx=0; dx<depth_intrin.width; ++dx)
{
/* Retrieve the 16-bit depth value and map it into a depth in meters */
uint16_t depth_value = depth_image[dy * depth_intrin.width + dx];
float depth_in_meters = depth_value * scale;
/* Skip over pixels with a depth value of zero, which is used to indicate no data */
if(depth_value == 0) continue;
/* Map from pixel coordinates in the depth image to pixel coordinates in the color image */
float depth_pixel[2] = {(float)dx, (float)dy};
float depth_point[3], color_point[3], color_pixel[2];
rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth_in_meters);
rs_transform_point_to_point(color_point, &depth_to_color, depth_point);
rs_project_point_to_pixel(color_pixel, &color_intrin, color_point);
/* Use the color from the nearest color pixel, or pure white if this point falls outside the color image */
const int cx = (int)roundf(color_pixel[0]), cy = (int)roundf(color_pixel[1]);
if(cx < 0 || cy < 0 || cx >= color_intrin.width || cy >= color_intrin.height)
{
glColor3ub(255, 255, 255);
}
else
{
glColor3ubv(color_image + (cy * color_intrin.width + cx) * 3);
}
/* Emit a vertex at the 3D location of this depth pixel */
glVertex3f(depth_point[0], depth_point[1], depth_point[2]);
}
}
glEnd();
glfwSwapBuffers(win);
}
return EXIT_SUCCESS;
}
int RealSenseDevice::open()
{
rs_error * e = 0;
ctx = rs_create_context(RS_API_VERSION, &e);
int devices = rs_get_device_count(ctx, &e);
if (devices == 0) {
std::cerr << "no realsense capture device" << std::endl;
return 1;
}
dev = rs_get_device(ctx, 0, &e);
std::cout << "Realsense Device: " << rs_get_device_name(dev, &e) << std::endl;
std::cout << "Realsense Serial: " << rs_get_device_serial(dev, &e) << std::endl;
std::cout << "Realsense FW ver: " << rs_get_device_firmware_version(dev, &e) << std::endl;
int framerate = 30;
rs_enable_stream(dev, depthStream, 0, 0, RS_FORMAT_Z16, framerate, &e);
int depthWidth = rs_get_stream_width(dev,depthStream,NULL);
int depthHeight = rs_get_stream_height(dev,depthStream,NULL);
rs_enable_stream(dev, colorStream, depthWidth, depthHeight, RS_FORMAT_RGB8, framerate, &e);
rs_get_stream_intrinsics(dev,colorStream,&intrinsics,0);
std::cout << "width:" << intrinsics.width << " height:" << intrinsics.height
<< " focalx:" << intrinsics.fx << " focaly:" << intrinsics.fy
<< " ppx:" << intrinsics.ppx << " ppy:" << intrinsics.ppy
<< " coeff0:" << intrinsics.coeffs[0]
<< " coeff1:" << intrinsics.coeffs[1]
<< " coeff2:" << intrinsics.coeffs[2]
<< " coeff3:" << intrinsics.coeffs[3]
<< std::endl;
int colorWidth = rs_get_stream_width(dev,colorStream,NULL);
int colorHeight = rs_get_stream_height(dev,colorStream,NULL);
depthScale = rs_get_device_depth_scale(dev,0);
std::cout << "depth image " << depthWidth << "x" << depthHeight << " scale:" << depthScale << std::endl;
std::cout << "color image " << colorWidth << "x" << colorHeight << std::endl;
rs_start_device(dev, &e);
int res = pthread_create(&_thread, NULL, RealSenseDevice_freenect_threadfunc, this);
if(res){
std::cerr << "error starting realsense thread " << res << std::endl;
return 1;
}
depth_index = 0;
return 0;
}
