IntelRealSense / librealsense

Intel® RealSense™ SDK
https://www.intelrealsense.com/
Apache License 2.0
7.59k stars 4.82k forks source link

Some grid line artifacts exist in the mesh of the rs2::aligned depth map #4422

Closed Mad-Thanos closed 5 years ago

Mad-Thanos commented 5 years ago
Required Info
Camera Model D415
Firmware Version 5.11.4
Operating System & Version Android 8.1.0
Kernel Version (Linux Only) linux 4.9.65
Platform Snapdragon 845
SDK Version 2.22.0
Language C++/Java/Android NDK
Segment Dense SLAM/3D Reconstruction

Issue Description

The aligned depth map contains some small grid line artifacts, which are visible in the mesh generated from the depth map. (Maybe this is not an issue, more like a question or discussion.)

Here is my code to get the aligned depth map and triangulate it into a mesh:

click to expand the code ```c++ using namespace std; rs2::align align_d2c(RS2_STREAM_COLOR); auto fs_aligned = align_d2c.process(fs); auto depthPtr = (ushort*)fs_aligned.get_depth_frame().get_data(); auto scale==rs2_get_depth_scale(depthFrame.get_sensor(), nullptr);// actually it's 0.001 auto get_depth = [depthPtr,scale](int idx)->float{return depthPtr[idx]*scale;} // Simply triangulate the aligned depth map const int W=depthIntrin.width; const int H=depthIntrin.height; std::map index2reducedIndex; //vertex position for (int py = 0; py < H; py++) { for (int px = 0; px < W; px++) { int idx = px + py * W; float depth = get_depth(idx); float3 vtx; float2 pixel{(float)px, (float)py}; rs2_deproject_pixel_to_point((float *)&vtx, &depthIntrin, (float *)&pixel, depth); if ((fabs(vtx.x) < MIN_DISTANCE && fabs(vtx.y) < MIN_DISTANCE && fabs(vtx.z) < MIN_DISTANCE) || depth >= 2.f) continue; index2reducedIndex[idx] = (int)vertPosArr.size(); vertPosArr.emplace_back(vtx.x, -vtx.y, -vtx.z); } } //face index const auto threshold = 0.05f; auto itEnd = index2reducedIndex.end(); for (int y = 0; y < H; y++) { for (int x = 0; x < W; x++) { auto a = y * W + x, b = y * W + x + 1, c = (y + 1) * W + x, d = (y + 1) * W + x + 1; auto ita = index2reducedIndex.find(a), itb = index2reducedIndex.find(b), itc = index2reducedIndex.find(c), itd = index2reducedIndex.find(d); if (ita == itEnd || itb == itEnd || itc == itEnd || itd == itEnd) continue; auto aZ = vertPosArr[ita->second].z, bZ = vertPosArr[itb->second].z, cZ = vertPosArr[itc->second].z, dZ = vertPosArr[itd->second].z; if (abs(aZ - bZ) < threshold && abs(aZ - cZ) < threshold && abs(bZ - dZ) < threshold &&abs(cZ - dZ) < threshold) { faceArr.emplace_back(ita->second, itd->second, itb->second); faceArr.emplace_back(itd->second, ita->second, itc->second); } } } //write the vertex position array and face indices into the .obj file ofstream ofObj("./mesh-rsaligned.obj", ios_base::trunc); if(!ofObj.good()){throw std::runtime_error("cannot open file for wirte");} for (int i = 0; i < vertPosArr.size(); ++i){ ofObj<

note: The captured RGBD stream is configured at 640x480@30fps@default-preset. And the depth scale is 0.001m.

Here is the generated mesh-rsaligned.obj on google drive.

I look into it using the Meshlab application and record the process showing the grid line artifacts:

I also saved the colorized depth map(eroded and colorized by myself using a colormap with only 64 fixed colors, superimposed on the RGB image):

The colorful depth map seems nothing wrong.

Question: Where are these grid lines coming from? will those lines affect the 3D reconstruction result?

agrunnet commented 5 years ago

Try setting depth units to 100 and not 1000 which is default. This changes depth quantization from 1mm steps to 100um.

dorodnic commented 5 years ago

Hi @GucciPrada Did you try using rs2::pointcloud method export_to_ply without alignment? By aligning depth to color, and then generating a mesh, you force resampling of the depth data, and this cannot be good (should not be bad either).

Mad-Thanos commented 5 years ago

Hi, @dorodnic Thanks for your response!

I've tried that. The result is good, no grid line artifacts:

Actually, the code I posted above is originated from rs2::pointcloud::export_to_ply with the only difference that I changed it to export mesh in .obj format.

After reading the source code, I've learned that by aligning depth to color, the rs2::align must do the remapping and resampling since there is an extrinsic between depth sensor and color sensor, and two sensors have different intrinsics. In order to look into the rs2::align's behavior, I wrote some code to forward remap depth pixels to color pixels but without resampling. rs2::align do the resampling by projecting each depth pixel's two corners into the color image plane and sampling depth value from original depth image for each projected pixel in the rectangle area formed by the two projected corners in color image plane. https://github.com/IntelRealSense/librealsense/blob/f112ed78e3917df79047254f864157195d4488b0/src/proc/align.cpp#L32-L58

What I was doing is just project each depth pixel's center(depth_x+0.5f,depth_y+0.5f) to the color image space and set the depth value to the nearest pixel of the projected point, namely forward remapping. Then I got:

The grid line gaps in the above colorful depth image coincide with the grid lines of the mesh, namely, they occur at the same place in the image and in the same warped shape. With the exception that there is no valid depth value in the gaps.

Back to my question, will those lines affect the 3D reconstruction result?

 By aligning depth to color, and then generating a mesh, you force resampling of the depth data, and this cannot be good (should not be bad either).

By saying 'this cannot be good', you mean the aligned depth image cannot be as good as the original one? By saying 'should not be bad either', mean the aligned depth image should be good enough for many computer vision tasks like Dense SLAM or 3D Reconstruction?

Mad-Thanos commented 5 years ago

Hi, @agrunnet Thanks for your advice! I was thinking about the same idea. Before I got the same idea as yours, I tried to remove the grid line artifacts on the mesh surface by using linear interpolation instead of nearest interpolation. I got the following result(the left is generated from rs2::align depth map; the right is from modified rs2::align): 51563175818_ pic

The right one is more visually pleasant since the grid line artifact is alleviated to some extent. BUT at the expenses of generating non-existent points or depth values(depicted as pink parts in the following screenshot): 61563175828_ pic

Maybe you are right! I'd like to give it a try.

Mad-Thanos commented 5 years ago

I've tried to set the depthUnit to 100um, disparityShift to 70(i.e. work range from about 17cm to 47cm, VGA RGBD@30fps configuration). However the artifacts do not disappear. It even appears on the mesh surface.

agrunnet commented 5 years ago

It looks better. It does not look like the lines are due to quantization now. Now, try to set the new A-factor to 0.08 under advanced settings. This helps the subpixel linearity. See new white paper and make sure you have latest FW installed. This should help reduce ripples on a tilted surface. In the white paper it describes the optimal A-factor for each depth preset. If you are using high accuracy you will need A=0.18, but most other depth presets require A=0.08.

Mad-Thanos commented 5 years ago

Hi @agrunnet Thanks! I'll read your white paper about the A-factor and try it out.

BTW, I was using a modified version of MidResHighAccuracyPreset.json, just simply appending a few depth table parameters I mentioned in my last post.

agrunnet commented 5 years ago

Ok for the high accuracy setting medium resolution you definitely need closer to A=0.18 but you can adjust values to see what works best for you.

Mad-Thanos commented 5 years ago

Sorry for this late report. I've read the white paper named Subpixel Linearity Improvement for Intel® RealSense™ Depth Camera D400 Series written by @agrunnet As far as I can understand, the A-factor will be really good for reducing the ripple effects, but not the grid line artifacts.

Here is the ripples(depicted in red circle): image

And here is the grid lines artifacst(depicted along the green lines): image image

The grid line artifacts are caused by nearest-interpolation as described by @dorodnic

agrunnet commented 5 years ago

Easy enough to try the a-factor though and see if it works. (I do not believe it has anything to do with interpolation). It helps to know the size of the steps. From what I can make out, the grid lines in the top image are quantization effects. This comes either from capture resolution being set to 1mm or some integer math or from graphing.

The ripples in the bottom image look different. They look more like the ripples you may see if there is a slight non-linearity in the depth measure, as described in the white paper.

That is currently the best advice I can offer.

Mad-Thanos commented 5 years ago

Hi agrunnet , Thank you very much. I already tried the A-factor with different values (0.08~0.18). It could help to reduce the ripple effects on the 2.5D mesh(triangulated from a single view depth map), however, the grid lines still exist on the 2.5D mesh or reconstructed global mesh model.

The depth map is resampled several times during the process of aligning it to the color map. I think the grid lines are coming from this process. Maybe I should not align the depth map to the color map. I can still achieve what I want without aligning the two images.

MartyG-RealSense commented 5 years ago

@GucciPrada I just wanted to update you that new Development Firmware 5.11.11.100 is available from the Firmware Releases page if you have not used it already. Among the features that it adds are improved depth linearity. The details of the additions in the firmware can be found in the link below.

https://support.intelrealsense.com/hc/en-us/community/posts/360034784453