Skip to content

Commit 1ef9d9a

Browse files
mohammedarimohammedari
committed
implemented size gard and pixel-wise accumulation
1 parent ead0dc1 commit 1ef9d9a

File tree

4 files changed

+56
-41
lines changed

4 files changed

+56
-41
lines changed

‎opengl_ros/launch/object_position_extraction.launch‎

Lines changed: 19 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -3,24 +3,30 @@
33
<arg name="bag_prefix" default="/mnt/log/d435"/>
44
<arg name="depth_width" default="640"/>
55
<arg name="depth_height" default="360"/>
6-
<arg name="output_width" default="320"/>
7-
<arg name="output_height" default="180"/>
86
<arg name="frame_rate" default="30"/>
97

8+
<!-- use small size image to accumulate in CPU -->
9+
<arg name="output_width" default="160"/>
10+
<arg name="output_height" default="90"/>
11+
1012
<!-- valid depth range of D435 -->
1113
<arg name="depth_min" default="0.105"/>
1214
<arg name="depth_max" default="10"/>
1315

1416
<!-- color extraction parameter -->
1517
<arg name="threshold_l" default="50"/>
16-
<arg name="svm_coef_a" default="0.266602955271"/>
17-
<arg name="svm_coef_b" default="0.0444656815907"/>
18-
<arg name="svm_intercept" default="-44.3271851592"/>
18+
<arg name="svm_coef_a" default="0.181817179054"/>
19+
<arg name="svm_coef_b" default="-0.0205828687538"/>
20+
<arg name="svm_intercept" default="-27.8266303282"/>
1921

2022
<!-- object position extraction parameter -->
21-
<arg name="object_separation_distance" default="1"/>
22-
<arg name="min_pixel_count_for_detection" default="15"/>
23-
<arg name="sigma_coefficient" default="2"/>
23+
<arg name="object_separation_distance" default="2"/>
24+
<arg name="min_pixel_count_for_detection" default="30"/> <!-- specify in input resolution -->
25+
<arg name="sigma_coefficient" default="0.25"/>
26+
<arg name="object_size_min_x" default="0.175"/>
27+
<arg name="object_size_max_x" default="0.5"/>
28+
<arg name="object_size_min_y" default="0.175"/>
29+
<arg name="object_size_max_y" default="0.5"/>
2430

2531
<arg name="d435" default="false"/>
2632
<arg name="viewer" default="false"/>
@@ -77,6 +83,11 @@
7783
<param name="min_pixel_count_for_detection" value="$(arg min_pixel_count_for_detection)"/>
7884
<param name="sigma_coefficient" value="$(arg sigma_coefficient)"/>
7985

86+
<param name="object_size_min_x" value="$(arg object_size_min_x)"/>
87+
<param name="object_size_max_x" value="$(arg object_size_max_x)"/>
88+
<param name="object_size_min_y" value="$(arg object_size_min_y)"/>
89+
<param name="object_size_max_y" value="$(arg object_size_max_y)"/>
90+
8091
<param name="vertex_shader" value="$(find opengl_ros_lib)/shader/vs_object_position_extraction.glsl"/>
8192
<param name="fragment_shader" value="$(find opengl_ros_lib)/shader/fs_object_position_extraction.glsl"/>
8293

‎opengl_ros/src/object_position_extractor_nodecore.cpp‎

Lines changed: 26 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,10 @@ ObjectPositionExtractorNode::ObjectPositionExtractorNode(const ros::NodeHandle&
2828
nh_.param<double>("object_separation_distance", object_separation_distance_, 2);
2929
nh_.param<int>("min_pixel_count_for_detection", min_pixel_count_for_detection_, 10);
3030
nh_.param<double>("sigma_coefficient_", sigma_coefficient_, 2);
31+
nh_.param<double>("object_size_min_x", object_size_min_x_, 0.1);
32+
nh_.param<double>("object_size_max_x", object_size_max_x_, 0.5);
33+
nh_.param<double>("object_size_min_y", object_size_min_y_, 0.1);
34+
nh_.param<double>("object_size_max_y", object_size_max_y_, 0.5);
3135

3236
//OpenGL parameters
3337
int color_width, color_height;
@@ -133,8 +137,8 @@ void ObjectPositionExtractorNode::depthCallback(const sensor_msgs::Image::ConstP
133137
extractor_->updateProjectionMatrix(
134138
{static_cast<float>(latestColorCameraInfo.K[0]), static_cast<float>(latestColorCameraInfo.K[4])},
135139
{static_cast<float>(latestColorCameraInfo.K[2]), static_cast<float>(latestColorCameraInfo.K[5])},
136-
{static_cast<float>(cameraInfoMsg->K[0]) , static_cast<float>(cameraInfoMsg->K[4])},
137-
{static_cast<float>(cameraInfoMsg->K[2]) , static_cast<float>(cameraInfoMsg->K[5])},
140+
{static_cast<float>(cameraInfoMsg->K[0]) , static_cast<float>(cameraInfoMsg->K[4])},
141+
{static_cast<float>(cameraInfoMsg->K[2]) , static_cast<float>(cameraInfoMsg->K[5])},
138142
latestDepthToColor_
139143
);
140144

@@ -154,21 +158,23 @@ void ObjectPositionExtractorNode::depthCallback(const sensor_msgs::Image::ConstP
154158
std::vector<ObjectPositionExtractorNode::ObjectCandidate> candidates;
155159
for (auto it = positionOut_.begin<cv::Vec4f>(); it != positionOut_.end<cv::Vec4f>(); ++it)
156160
{
157-
auto x = (*it)[0];
158-
auto y = (*it)[1];
159-
auto z = (*it)[2];
161+
auto x = (*it)[0]; //
162+
auto y = (*it)[1]; //
163+
auto z = (*it)[2]; //accumulated coordinate value
164+
auto w = (*it)[3]; //number of accumulated point
160165

161166
//Not detected at this pixel
162-
if (x == 0 && y == 0 && z == 0)
167+
if (w == 0)
163168
continue;
164169

165-
Eigen::Vector3d point(x, y, z);
170+
Eigen::Vector3d point(x / w, y / w, z / w);
171+
int accumulated_pixel_count = static_cast<int>(std::round(w));
166172

167173
//If this is the first point, just add it to the candidates
168174
if (candidates.size() == 0)
169175
{
170176
candidates.emplace_back();
171-
candidates[0].add(point);
177+
candidates[0].add(point, accumulated_pixel_count);
172178
continue;
173179
}
174180

@@ -181,12 +187,12 @@ void ObjectPositionExtractorNode::depthCallback(const sensor_msgs::Image::ConstP
181187
if (object_separation_distance_ < minDistance)
182188
{
183189
candidates.emplace_back();
184-
candidates[candidates.size() - 1].add(point);
190+
candidates[candidates.size() - 1].add(point, accumulated_pixel_count);
185191
continue;
186192
}
187193

188194
//Add the point to the neareset candidate
189-
candidates[minIndex].add(point);
195+
candidates[minIndex].add(point, accumulated_pixel_count);
190196
}
191197

192198
//Publish Object Array
@@ -195,14 +201,19 @@ void ObjectPositionExtractorNode::depthCallback(const sensor_msgs::Image::ConstP
195201
{
196202
auto sigma = candidate.variance();
197203

198-
auto count = candidate.count(sigma_coefficient_ * sigma);
199-
if (count < min_pixel_count_for_detection_)
204+
//If number of pixel is lower than threshold, skip it
205+
if (candidate.number_of_detected_pixels < min_pixel_count_for_detection_)
200206
continue;
201207

202-
ROS_INFO_STREAM("detected object (" << count << "/" << candidate.count() << "px)");
208+
//If the size of the object along the camera direction is too small or too large, skip it
209+
auto candidate_size = candidate.size(sigma_coefficient_ * sigma);
210+
if (candidate_size.x() < object_size_min_x_ || object_size_max_x_ < candidate_size.x() ||
211+
candidate_size.y() < object_size_min_y_ || object_size_max_y_ < candidate_size.y())
212+
continue;
213+
214+
ROS_INFO_STREAM("detected object (" << candidate.number_of_detected_pixels << "px)");
203215

204216
auto candidate_pose = candidate.mean(sigma_coefficient_ * sigma);
205-
auto candidate_size = candidate.size(sigma_coefficient_ * sigma);
206217
geometry_msgs::Pose pose;
207218
{
208219
pose.position.x = candidate_pose.x();
@@ -229,7 +240,7 @@ void ObjectPositionExtractorNode::depthCallback(const sensor_msgs::Image::ConstP
229240
o.pose = pose;
230241
o.twist = zero_twist;
231242
o.size = size;
232-
o.confidence = static_cast<double>(count) / candidate.count(); //set number of pixels in threshold as confidence
243+
o.confidence = 1.0; //TODO
233244

234245
objectArray.objects.push_back(o);
235246
}

‎opengl_ros/src/object_position_extractor_nodecore.h‎

Lines changed: 5 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,8 @@ class ObjectPositionExtractorNode
4545
double object_separation_distance_;
4646
int min_pixel_count_for_detection_;
4747
double sigma_coefficient_;
48+
double object_size_min_x_, object_size_max_x_;
49+
double object_size_min_y_, object_size_max_y_;
4850

4951
void colorCallback(const sensor_msgs::Image::ConstPtr& imageMsg, const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg);
5052
void depthCallback(const sensor_msgs::Image::ConstPtr& imageMsg, const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg);
@@ -56,11 +58,13 @@ class ObjectPositionExtractorNode
5658
{
5759
std::vector<Eigen::Vector3d> points;
5860
Eigen::Vector3d sum = {};
61+
int number_of_detected_pixels = 0;
5962

60-
void add(const Eigen::Vector3d& point)
63+
void add(const Eigen::Vector3d& point, int accumulated_pixel_count)
6164
{
6265
points.push_back(point);
6366
sum += point;
67+
number_of_detected_pixels += accumulated_pixel_count;
6468
}
6569

6670
Eigen::Vector3d mean() const
@@ -96,23 +100,6 @@ class ObjectPositionExtractorNode
96100
return sqrt(squared_diff_sum / points.size());
97101
}
98102

99-
int count() const
100-
{
101-
return points.size();
102-
}
103-
104-
int count(double threshold) const
105-
{
106-
auto m = mean();
107-
108-
int count = 0;
109-
for (const auto p : points)
110-
if ((p - m).norm() < threshold)
111-
++count;
112-
113-
return count;
114-
}
115-
116103
Eigen::Vector3d size(double threshold = std::numeric_limits<double>::infinity()) const
117104
{
118105
auto m = mean();

‎opengl_ros_lib/src/object_position_extractor.cpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,12 @@ ObjectPositionExtractor::Impl::Impl(
9797

9898
//Verticies setup
9999
vao_.mapVariable(vbo_, glGetAttribLocation(program_.get(), "inputPixel"), 3, GL_FLOAT, 0);
100+
101+
//Enable blending
102+
glEnable(GL_BLEND);
103+
glBlendEquation(GL_FUNC_ADD);
104+
glBlendFuncSeparatei(0, GL_ONE, GL_ONE , GL_ONE, GL_ONE); //accumulate position
105+
glBlendFuncSeparatei(1, GL_ONE, GL_ZERO, GL_ONE, GL_ZERO); //always overwrite color
100106
}
101107

102108
void ObjectPositionExtractor::Impl::updateProjectionMatrix(

0 commit comments

Comments
 (0)