- Mark as New
- Bookmark
- Subscribe
- Mute
- Subscribe to RSS Feed
- Permalink
- Report Inappropriate Content
Hello everyone,
This has been asked before, and I believe I am doing the right steps but cannot get it work. I am trying to upgrade from RealSense to RealSense2 using SR300 camera. I am trying to abstract getting a point cloud with color (x, y, z, r, g, b) for other uses. I am trying to map the color to the depth points as follows:
Camera() : pipe(), pipeProfile(), depthScale(0) {
rs2::config cameraConfig;
cameraConfig.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16);
cameraConfig.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGB8);
pipeProfile = pipe.start(cameraConfig);
depthScale = pipeProfile.get_device().first().get_depth_scale();
}
void Camera::getCapture() {
rs2::frameset frames = pipe.wait_for_frames();
rs2::depth_frame depthFrame = frames.get_depth_frame();
rs2::video_frame colorFrame = frames.get_color_frame();
rs2_intrinsics depth_intrinsic = depthFrame.get_profile().as().get_intrinsics();
rs2_intrinsics color_intrinsic = colorFrame.get_profile().as().get_intrinsics();
rs2_extrinsics depth_to_color = depthFrame.get_profile().get_extrinsics_to(colorFrame.get_profile());
const uint16_t* depthImage = (const uint16_t*)depthFrame.get_data();
const unsigned char* colorImage = (const unsigned char*)colorFrame.get_data();
for (int dy = 0; dy < depth_intrinsic.height; dy++) {
for (int dx = 0; dx < depth_intrinsic.width; dx++) {
// get depth value at dy, dx
unsigned int i = dy * depth_intrinsic.width + dx;
uint16_t depth_value = depthImage[i];
// if the depth value is 0, we ignore this points.
if (depth_value == 0) {
continue;
}
float depth_in_meters = depth_value * depthScale;
// map the pixel coordinate in depth image to pixel coordinate in color image
float depth_pixel[2] = {(float)dx - 0.5f, (float)dy - 0.5f};
float depth_point[3], color_point[3], color_pixel[2];
rs2_deproject_pixel_to_point(depth_point, &depth_intrinsic, depth_pixel, depth_in_meters);
// if it's out of acceptable range, skip it
if (depth_point[2] > maxAcceptDistance) {
continue;
}
rs2_transform_point_to_point(color_point, &depth_to_color, depth_point);
rs2_project_point_to_pixel(color_pixel, &color_intrinsic, color_point);
// get closest color pixel obtained from calculations and use as the point's color, or white if ouside color image
const int cx = static_cast(color_pixel[0] + 0.5f);
const int cy = static_cast(color_pixel[1] + 0.5f);
// if outside intrinsic parameters, skip the point
if (cx < 0 || cy < 0 || cx > color_intrinsic.width || cy > color_intrinsic.height) {
continue;
}
const unsigned char* offset = static_cast(colorImage + (cy * color_intrinsic.width + cx) * 3);
float x = -1 * depth_point[0];
float y = -1 * depth_point[1];
float z = depth_point[2];
unsigned char r = *(offset);
unsigned char g = *(offset + 1);
unsigned char b = *(offset + 2);
}
}
}
I only left the redundant parts. Basically, in the end, (r, g, b) are 0 (or they appear black in the image). Any suggestions?
PS. (Code editor is weird).
- Mark as New
- Bookmark
- Subscribe
- Mute
- Subscribe to RSS Feed
- Permalink
- Report Inappropriate Content
For anyone that may read this, I had to swap the map_to and calculate statements. However, you also need to get some captures. do as follows:
rs2::pointcloud pc;
rs2::points points;
for (int i = 0; i < 10; i++) pipe.wait_for_frames();
rs2::frameset frames = pipe.wait_for_frames();
rs2::depth_frame depth = frames.get_depth_frame();
rs2::video_frame color_frame = frames.get_color_frame();
pc.map_to(color_frame);
points = pc.calculate(depth);
int width = color_frame.get_width();
int height = color_frame.get_height();
const rs2::vertex* ptr = points.get_vertices();
const rs2::texture_coordinate* tex_coords = points.get_texture_coordinates();
const unsigned char* colorStream = static_cast<</span>const unsigned char*>(color_frame.get_data());
for (int i = 0; i < points.size(); i++) {
int x = static_cast(textureCoordinates[i].u * width);
int y = static_cast(textureCoordinates[i].v * height);
if (y < 0 || y >= height) {
continue;
}
if (x < 0 || x >= width) {
continue;
}
if (rs_vertices[i].z <= 0 || rs_vertices[i].z > maxAcceptDistance) { // Rule out values further than this value
continue;
}
int colorLocation = (y * width + x) * 3;
unsigned char color[3] = {colorData[colorLocation], colorData[colorLocation +1], colorData[colorLocation +2]}; // RGB
float coordinates[3] = { ptr[i].x, ptr[i].y, ptr[i].z };
}
Link Copied
- Mark as New
- Bookmark
- Subscribe
- Mute
- Subscribe to RSS Feed
- Permalink
- Report Inappropriate Content
Have you seen the SDK 2.0 example for creating a point cloud?
https://github.com/IntelRealSense/librealsense/tree/master/examples/pointcloud librealsense/examples/pointcloud at master · IntelRealSense/librealsense · GitHub
- Mark as New
- Bookmark
- Subscribe
- Mute
- Subscribe to RSS Feed
- Permalink
- Report Inappropriate Content
Hello MartyG,
I did see the example, but I couldn't figure out how to use the (u, v) values to obtain the (r, g, b) values from the color frame. Any help with that? Thanks.
- Mark as New
- Bookmark
- Subscribe
- Mute
- Subscribe to RSS Feed
- Permalink
- Report Inappropriate Content
I'll link RealSense stream programming expert jb455 into this discussion to seek his wide knowledge on the subject.
- Mark as New
- Bookmark
- Subscribe
- Mute
- Subscribe to RSS Feed
- Permalink
- Report Inappropriate Content
So I know that once you do
rs2::pointcloud pc;
rs2::points points;
rs2::frameset frames = pipe.wait_for_frames();
rs2::depth_frame depth = frames.get_depth_frame();
rs2::video_frame color_frame = frames.get_color_frame();
points = pc.calculate(depth);
pc.map_to(color_frame);
Then, you can get the texture coordinates that correspond to each vertex:
const rs2::vertex* ptr = points.get_vertices();
const rs2::texture_coordinate* tex_coords = points.get_texture_coordinates();
const unsigned char* colorStream = static_cast(color_frame.get_data());
Each texture coordinate has (u, v) value which ranges from [0-1] that can be mapped to the color frame. I did this:
int x = tex_coords[i].u * color_intrinsic.width;
int y = tex_coords[i].v * color_intrinsic.height;
int colorLocation = y * color_intrinsic.width + x;
unsigned char color[3] = {colorStream[colorLocation], colorStream[colorLocation +1], colorStream[colorLocation +2]};
However, everything comes back as a single color. I stepped through and it seems both u and v are always 0.
- Mark as New
- Bookmark
- Subscribe
- Mute
- Subscribe to RSS Feed
- Permalink
- Report Inappropriate Content
For anyone that may read this, I had to swap the map_to and calculate statements. However, you also need to get some captures. do as follows:
rs2::pointcloud pc;
rs2::points points;
for (int i = 0; i < 10; i++) pipe.wait_for_frames();
rs2::frameset frames = pipe.wait_for_frames();
rs2::depth_frame depth = frames.get_depth_frame();
rs2::video_frame color_frame = frames.get_color_frame();
pc.map_to(color_frame);
points = pc.calculate(depth);
int width = color_frame.get_width();
int height = color_frame.get_height();
const rs2::vertex* ptr = points.get_vertices();
const rs2::texture_coordinate* tex_coords = points.get_texture_coordinates();
const unsigned char* colorStream = static_cast<</span>const unsigned char*>(color_frame.get_data());
for (int i = 0; i < points.size(); i++) {
int x = static_cast(textureCoordinates[i].u * width);
int y = static_cast(textureCoordinates[i].v * height);
if (y < 0 || y >= height) {
continue;
}
if (x < 0 || x >= width) {
continue;
}
if (rs_vertices[i].z <= 0 || rs_vertices[i].z > maxAcceptDistance) { // Rule out values further than this value
continue;
}
int colorLocation = (y * width + x) * 3;
unsigned char color[3] = {colorData[colorLocation], colorData[colorLocation +1], colorData[colorLocation +2]}; // RGB
float coordinates[3] = { ptr[i].x, ptr[i].y, ptr[i].z };
}
- Subscribe to RSS Feed
- Mark Topic as New
- Mark Topic as Read
- Float this Topic for Current User
- Bookmark
- Subscribe
- Printer Friendly Page