Skip to content

Commit 68da159

Browse files
mohammedarimohammedari
committed
depth projection with target extraction based on color image works
1 parent a5ee3c3 commit 68da159

File tree

6 files changed

+136
-42
lines changed

6 files changed

+136
-42
lines changed

‎opengl_ros/launch/depth_image_projection.launch‎

Lines changed: 14 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -2,13 +2,11 @@
22
<launch>
33
<arg name="depth_width" default="1280"/>
44
<arg name="depth_height" default="720"/>
5-
<!-- <arg name="depth_width" default="640"/> -->
6-
<!-- <arg name="depth_height" default="360"/> -->
7-
<arg name="grid_map_width" default="640"/>
8-
<arg name="grid_map_height" default="480"/>
9-
<arg name="grid_map_resolution" default="0.1"/>
5+
<arg name="grid_map_width" default="300"/>
6+
<arg name="grid_map_height" default="300"/>
7+
<arg name="grid_map_resolution" default="0.01"/>
108
<arg name="grid_map_layer_height" default="0.5"/>
11-
<arg name="grid_map_accumulation_weight" default="0.004"/> <!-- 1/255 -->
9+
<arg name="grid_map_accumulation_weight" default="0.004"/> <!-- 0.004 for 1/255 -->
1210
<arg name="frame_rate" default="30"/>
1311

1412
<group ns="d435">
@@ -43,19 +41,25 @@
4341
<param name="colorHeight" value="$(arg depth_height)"/>
4442
<param name="depthWidth" value="$(arg depth_width)"/>
4543
<param name="depthHeight" value="$(arg depth_height)"/>
44+
4645
<param name="gridMapWidth" value="$(arg grid_map_width)"/>
4746
<param name="gridMapHeight" value="$(arg grid_map_height)"/>
4847
<param name="gridMapResolution" value="$(arg grid_map_resolution)"/>
4948
<param name="gridMapLayerHeight" value="$(arg grid_map_layer_height)"/>
5049
<param name="gridMapAccumulationWeight" value="$(arg grid_map_accumulation_weight)"/>
50+
51+
<param name="threshold_l" value="0"/>
52+
<param name="svm_coef_a" value="0.266602955271"/>
53+
<param name="svm_coef_b" value="0.0444656815907"/>
54+
<param name="svm_intercept" value="-44.3271851592"/>
55+
5156
<param name="vertex_shader" value="$(find opengl_ros_lib)/shader/vs_depth_image_projection.glsl"/>
5257
<param name="fragment_shader" value="$(find opengl_ros_lib)/shader/fs_depth_image_projection.glsl"/>
5358

5459
<remap from="~depth_to_color" to="/d435/extrinsics/depth_to_color"/>
55-
56-
<remap from="color_in" to="/d435/color/image_raw"/>
57-
<remap from="depth_in" to="/d435/depth/image_rect_raw"/>
58-
<remap from="image_out" to="/occupancy_grid_image"/>
60+
<remap from="color_in" to="/d435/color/image_raw"/>
61+
<remap from="depth_in" to="/d435/depth/image_rect_raw"/>
62+
<remap from="image_out" to="/occupancy_grid_image"/>
5963
</node>
6064

6165
<node pkg="image_view" type="image_view" name="viewer_depth">

‎opengl_ros/src/depth_image_projector_nodecore.cpp‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,17 @@ DepthImageProjectorNode::DepthImageProjectorNode(const ros::NodeHandle& nh, cons
4343
vertexShader, fragmentShader
4444
);
4545

46+
float threshold_l, svm_coef_a, svm_coef_b, svm_intercept;
47+
nh_.param<float>("threshold_l" , threshold_l , 0);
48+
nh_.param<float>("svm_coef_a" , svm_coef_a , 0);
49+
nh_.param<float>("svm_coef_b" , svm_coef_b , 0);
50+
nh_.param<float>("svm_intercept", svm_intercept, 0);
51+
52+
projector_->uniform("threshold_l" , threshold_l);
53+
projector_->uniform("svm_coef_a" , svm_coef_a);
54+
projector_->uniform("svm_coef_b" , svm_coef_b);
55+
projector_->uniform("svm_intercept", svm_intercept);
56+
4657
output_.create(gridMapHeight, gridMapWidth, CV_8UC3);
4758
}
4859

‎opengl_ros_lib/include/depth_image_projector.h‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,19 @@ class DepthImageProjector final
2424
const std::string& vertexShader, const std::string& fragmentShader);
2525
~DepthImageProjector();
2626

27+
void uniform(const std::string& name, float v1);
28+
void uniform(const std::string& name, float v1, float v2);
29+
void uniform(const std::string& name, float v1, float v2, float v3);
30+
void uniform(const std::string& name, float v1, float v2, float v3, float v4);
31+
void uniform(const std::string& name, int v1);
32+
void uniform(const std::string& name, int v1, int v2);
33+
void uniform(const std::string& name, int v1, int v2, int v3);
34+
void uniform(const std::string& name, int v1, int v2, int v3, int v4);
35+
void uniform(const std::string& name, unsigned int v1);
36+
void uniform(const std::string& name, unsigned int v1, unsigned int v2);
37+
void uniform(const std::string& name, unsigned int v1, unsigned int v2, unsigned int v3);
38+
void uniform(const std::string& name, unsigned int v1, unsigned int v2, unsigned int v3, unsigned int v4);
39+
2740
void updateProjectionMatrix(
2841
const std::array<float, 2> colorFocalLength, const std::array<float, 2> colorCenter,
2942
const std::array<float, 2> depthFocalLength, const std::array<float, 2> depthCenter,

‎opengl_ros_lib/shader/fs_depth_image_projection.glsl‎

Lines changed: 59 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,11 @@ uniform sampler2D colorTexture;
44
uniform float gridMapLayerHeight;
55
uniform float gridMapAccumulationWeight;
66

7+
uniform float threshold_l;
8+
uniform float svm_coef_a;
9+
uniform float svm_coef_b;
10+
uniform float svm_intercept;
11+
712
in vec2 colorUV; //The UV coordinate of corresponding pixel in the color image
813
//x = 0 to 1
914
//y = 0 to 1
@@ -12,16 +17,65 @@ in float height; //The height of the point, which is 0 if the point is at the mi
1217

1318
out vec3 fragmentColor;
1419

20+
vec3 rgb2xyz(vec3 rgb)
21+
{
22+
//Column major order
23+
const mat3 mat = mat3(0.412453, 0.212671, 0.019334, 0.357580, 0.715160, 0.119193, 0.180423, 0.072169, 0.950227);
24+
return mat * rgb;
25+
}
26+
27+
float f(float t)
28+
{
29+
if (t > 0.008856)
30+
return pow(t, 1.0 / 3.0);
31+
else
32+
return 7.787 * t + 16.0 / 116.0;
33+
}
34+
35+
vec3 xyz2lab(vec3 xyz)
36+
{
37+
float x = xyz.x / 0.950456;
38+
float y = xyz.y;
39+
float z = xyz.z / 1.088754;
40+
41+
float L;
42+
if (y > 0.008856)
43+
L = 116.0 * pow(y, 1.0 / 3.0) - 16.0;
44+
else
45+
L = 903.3 * y;
46+
47+
float a = 500.0 * ( f(x) - f(y) );
48+
float b = 200.0 * ( f(y) - f(z) );
49+
50+
return vec3(
51+
L / 100.0,
52+
a / 127.0 / 2.0 + 0.5,
53+
b / 127.0 / 2.0 + 0.5
54+
);
55+
}
56+
1557
void main(void)
1658
{
1759
//If the height is out of the range, discard the fragment.
18-
//if (height < -gridMapLayerHeight / 2 || gridMapLayerHeight / 2 < height)
19-
// discard;
60+
if (height < -gridMapLayerHeight / 2 || gridMapLayerHeight / 2 < height)
61+
discard;
2062

2163
vec3 color = texture2D(colorTexture, colorUV).xyz;
2264

23-
//fragmentColor = color * gridMapAccumulationWeight;
65+
//Convert to CIE La*b* space
66+
vec3 lab = xyz2lab(rgb2xyz(color));
67+
68+
float l = lab.x * 255.0;
69+
float a = lab.y * 255.0;
70+
float b = lab.z * 255.0;
71+
72+
//Apply SVM to extract a target
73+
if (l > threshold_l && svm_coef_a * a + svm_coef_b * b + svm_intercept > 0)
74+
{
75+
fragmentColor = vec3(1.0, 0.0, 0.0) * gridMapAccumulationWeight;
76+
return;
77+
}
2478

25-
//test
26-
fragmentColor = color;
79+
//Return white if the pixel is not a target
80+
fragmentColor = vec3(1.0, 1.0, 1.0) * gridMapAccumulationWeight;
2781
}

‎opengl_ros_lib/shader/vs_depth_image_projection.glsl‎

Lines changed: 1 addition & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -56,16 +56,9 @@ void main(void)
5656
1 - point.z / gridMapResolution / gridMapSize.y * 2 //set the origin on the bottom and flip upside down
5757
);
5858

59-
//gl_Position = vec4(plane, 0.0, 1.0);
59+
gl_Position = vec4(plane, 0.0, 1.0);
6060
height = point.y;
6161

62-
//test
63-
gl_Position = vec4(
64-
(position.x / depthSize.x - 0.5) * 2,
65-
(position.y / depthSize.y - 0.5) * 2,
66-
0,
67-
1);
68-
6962
//Calculate coordinate in color image
7063
vec4 colorPoint = depthToColor * vec4(point, 1);
7164

‎opengl_ros_lib/src/depth_image_projector.cpp‎

Lines changed: 38 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,7 @@ DepthImageProjector::Impl::Impl(
7373
vbo_(verticies_, GL_STATIC_DRAW),
7474
colorIn_(GL_SRGB8, colorWidth_, colorHeight_),
7575
depthIn_(GL_R16UI, depthWidth_, depthHeight_),
76-
textureOut_(GL_SRGB8, gridMapWidth, gridMapHeight), //TODO fix to RGB
76+
textureOut_(GL_RGB8, gridMapWidth, gridMapHeight),
7777
colorSampler_(GL_LINEAR, GL_LINEAR, GL_CLAMP_TO_BORDER, GL_CLAMP_TO_BORDER),
7878
depthSampler_(GL_NEAREST, GL_NEAREST, GL_CLAMP_TO_EDGE, GL_CLAMP_TO_EDGE),
7979
fbo_(textureOut_)
@@ -94,7 +94,7 @@ DepthImageProjector::Impl::Impl(
9494
vao_.mapVariable(vbo_, glGetAttribLocation(program_.get(), "position"), 3, GL_FLOAT, 0);
9595

9696
//Enable blending
97-
//glEnable(GL_BLEND);
97+
glEnable(GL_BLEND);
9898
glBlendFunc(GL_ONE, GL_ONE);
9999
glBlendEquation(GL_FUNC_ADD);
100100
}
@@ -109,23 +109,6 @@ void DepthImageProjector::Impl::updateProjectionMatrix(
109109
glUniform2fv(glGetUniformLocation(program_.get(), "depthFocalLength") , 1, depthFocalLength.data());
110110
glUniform2fv(glGetUniformLocation(program_.get(), "depthCenter") , 1, depthCenter.data());
111111
glUniformMatrix4fv(glGetUniformLocation(program_.get(), "depthToColor"), 1, false, depthToColor.data());
112-
113-
////TODO update projection matrix
114-
//glUniform2f(glGetUniformLocation(program_.get(), "depthFocalLength"), 323.7779846191406, 323.7779846191406);
115-
//glUniform2f(glGetUniformLocation(program_.get(), "depthCenter"), 322.2593078613281, 182.6495361328125);
116-
//glUniform2f(glGetUniformLocation(program_.get(), "colorFocalLength"), 463.1402587890625, 463.0929870605469);
117-
//glUniform2f(glGetUniformLocation(program_.get(), "colorCenter"), 320.7187194824219, 176.80926513671875);
118-
119-
////extrinsics matrix in column major order
120-
////TODO update the matrix using depth_to_color message
121-
////Note that the rotation variable in the message is column major order
122-
//std::array<float, 16> depthToColorT = {
123-
// 0.9999825954437256 , 0.0040609221905469894 , -0.004279938992112875 , 0,
124-
// -0.00407175999134779, 0.9999884963035583 , -0.0025265226140618324, 0,
125-
// 0.004269629716873169, 0.0025439055170863867 , 0.9999876618385315 , 0,
126-
// 0.014667361974716187, 0.00030949467327445745, 0.0012093170080333948 , 1,
127-
//};
128-
//glUniformMatrix4fv(glGetUniformLocation(program_.get(), "depthToColor"), 1, false, depthToColorT.data());
129112
}
130113

131114
void DepthImageProjector::Impl::project(cv::Mat& dest, const cv::Mat& color, const cv::Mat& depth) //TODO add projection matrix argument
@@ -217,6 +200,42 @@ catch (cgs::gl::Exception& e)
217200

218201
DepthImageProjector::~DepthImageProjector() = default;
219202

203+
void DepthImageProjector::uniform(const std::string& name, float v1) {
204+
glUniform1f(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1);
205+
}
206+
void DepthImageProjector::uniform(const std::string& name, float v1, float v2) {
207+
glUniform2f(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1, v2);
208+
}
209+
void DepthImageProjector::uniform(const std::string& name, float v1, float v2, float v3) {
210+
glUniform3f(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1, v2, v3);
211+
}
212+
void DepthImageProjector::uniform(const std::string& name, float v1, float v2, float v3, float v4) {
213+
glUniform4f(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1, v2, v3, v4);
214+
}
215+
void DepthImageProjector::uniform(const std::string& name, int v1) {
216+
glUniform1i(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1);
217+
}
218+
void DepthImageProjector::uniform(const std::string& name, int v1, int v2) {
219+
glUniform2i(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1, v2);
220+
}
221+
void DepthImageProjector::uniform(const std::string& name, int v1, int v2, int v3) {
222+
glUniform3i(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1, v2, v3);
223+
}
224+
void DepthImageProjector::uniform(const std::string& name, int v1, int v2, int v3, int v4) {
225+
glUniform4i(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1, v2, v3, v4);
226+
}
227+
void DepthImageProjector::uniform(const std::string& name, unsigned int v1) {
228+
glUniform1ui(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1);
229+
}
230+
void DepthImageProjector::uniform(const std::string& name, unsigned int v1, unsigned int v2) {
231+
glUniform2ui(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1, v2);
232+
}
233+
void DepthImageProjector::uniform(const std::string& name, unsigned int v1, unsigned int v2, unsigned int v3) {
234+
glUniform3ui(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1, v2, v3);
235+
}
236+
void DepthImageProjector::uniform(const std::string& name, unsigned int v1, unsigned int v2, unsigned int v3, unsigned int v4) {
237+
glUniform4ui(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1, v2, v3, v4);
238+
}
220239

221240
void DepthImageProjector::updateProjectionMatrix(
222241
const std::array<float, 2> colorFocalLength, const std::array<float, 2> colorCenter,

0 commit comments

Comments
 (0)