Skip to content

Commit cc89113

Browse files
mohammedarimohammedari
committed
added sigma parameter to reduce noise
1 parent ab53427 commit cc89113

File tree

5 files changed

+102
-37
lines changed

5 files changed

+102
-37
lines changed

‎opengl_ros/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -111,7 +111,7 @@ find_package(Eigen3 REQUIRED)
111111
catkin_package(
112112
# INCLUDE_DIRS include
113113
# LIBRARIES opengl_ros
114-
CATKIN_DEPENDS opengl_ros_lib cgs_nav_msgs
114+
CATKIN_DEPENDS roscpp std_msgs sensor_msgs cgs_nav_msgs tf image_transport cv_bridge opengl_ros_lib
115115
# DEPENDS system_lib
116116
)
117117

‎opengl_ros/launch/object_position_extraction.launch‎

Lines changed: 32 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,19 +1,30 @@
11
<?xml version="1.0"?>
22
<launch>
3-
<arg name="bag_prefix" default="/mnt/log/d435"/>
4-
<arg name="depth_width" default="640"/>
5-
<arg name="depth_height" default="360"/>
6-
<arg name="output_width" default="320"/>
7-
<arg name="output_height" default="180"/>
8-
<arg name="depth_min" default="0.105"/>
9-
<arg name="depth_max" default="10"/>
10-
<arg name="depth_hit_threshold" default="0.95"/>
11-
<arg name="depth_unknown_color" default="0"/> <!-- 0 to 255 -->
12-
<arg name="frame_rate" default="30"/>
13-
14-
<arg name="d435" default="false"/>
15-
<arg name="viewer" default="false"/>
16-
<arg name="record" default="false"/>
3+
<arg name="bag_prefix" default="/mnt/log/d435"/>
4+
<arg name="depth_width" default="640"/>
5+
<arg name="depth_height" default="360"/>
6+
<arg name="output_width" default="320"/>
7+
<arg name="output_height" default="180"/>
8+
<arg name="frame_rate" default="30"/>
9+
10+
<!-- valid depth range of D435 -->
11+
<arg name="depth_min" default="0.105"/>
12+
<arg name="depth_max" default="10"/>
13+
14+
<!-- color extraction parameter -->
15+
<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"/>
19+
20+
<!-- 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"/>
24+
25+
<arg name="d435" default="false"/>
26+
<arg name="viewer" default="false"/>
27+
<arg name="record" default="false"/>
1728

1829
<group ns="d435" if="$(arg d435)">
1930
<node pkg="nodelet" type="nodelet" name="realsense2_camera_manager" args="manager" output="screen"/>
@@ -57,13 +68,14 @@
5768
<param name="min_depth" value="$(arg depth_min)"/>
5869
<param name="max_depth" value="$(arg depth_max)"/>
5970

60-
<param name="threshold_l" value="50"/>
61-
<param name="svm_coef_a" value="0.266602955271"/>
62-
<param name="svm_coef_b" value="0.0444656815907"/>
63-
<param name="svm_intercept" value="-44.3271851592"/>
71+
<param name="threshold_l" value="$(arg threshold_l)"/>
72+
<param name="svm_coef_a" value="$(arg svm_coef_a)"/>
73+
<param name="svm_coef_b" value="$(arg svm_coef_b)"/>
74+
<param name="svm_intercept" value="$(arg svm_intercept)"/>
6475

65-
<param name="object_separation_distance" value="1"/>
66-
<param name="min_pixel_count_for_detection" value="15"/>
76+
<param name="object_separation_distance" value="$(arg object_separation_distance)"/>
77+
<param name="min_pixel_count_for_detection" value="$(arg min_pixel_count_for_detection)"/>
78+
<param name="sigma_coefficient" value="$(arg sigma_coefficient)"/>
6779

6880
<param name="vertex_shader" value="$(find opengl_ros_lib)/shader/vs_object_position_extraction.glsl"/>
6981
<param name="fragment_shader" value="$(find opengl_ros_lib)/shader/fs_object_position_extraction.glsl"/>

‎opengl_ros/package.xml‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -41,16 +41,21 @@
4141
<!-- <test_depend>gtest</test_depend> -->
4242
<buildtool_depend>catkin</buildtool_depend>
4343
<build_depend>roscpp</build_depend>
44+
<build_depend>std_msgs</build_depend>
4445
<build_depend>sensor_msgs</build_depend>
46+
<build_depend>cgs_nav_msgs</build_depend>
47+
<build_depend>tf</build_depend>
4548
<build_depend>image_transport</build_depend>
4649
<build_depend>cv_bridge</build_depend>
4750
<build_depend>opengl_ros_lib</build_depend>
48-
<build_depend>tf</build_depend>
49-
<build_depend>cgs_nav_msgs</build_depend>
50-
<run_depend>opengl_ros_lib</run_depend>
51-
<run_depend>tf</run_depend>
51+
<run_depend>roscpp</run_depend>
52+
<run_depend>std_msgs</run_depend>
53+
<run_depend>sensor_msgs</run_depend>
5254
<run_depend>cgs_nav_msgs</run_depend>
53-
55+
<run_depend>tf</run_depend>
56+
<run_depend>image_transport</run_depend>
57+
<run_depend>cv_bridge</run_depend>
58+
<run_depend>opengl_ros_lib</run_depend>
5459

5560
<!-- The export tag contains other, unspecified, tags -->
5661
<export>

‎opengl_ros/src/object_position_extractor_nodecore.cpp‎

Lines changed: 11 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@ ObjectPositionExtractorNode::ObjectPositionExtractorNode(const ros::NodeHandle&
2727
//Other parameters
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);
30+
nh_.param<double>("sigma_coefficient_", sigma_coefficient_, 2);
3031

3132
//OpenGL parameters
3233
int color_width, color_height;
@@ -79,7 +80,7 @@ void ObjectPositionExtractorNode::colorCallback(const sensor_msgs::Image::ConstP
7980
return;
8081
}
8182

82-
latestColorCameraInfoPtr = cameraInfoMsg;
83+
latestColorCameraInfo = *(cameraInfoMsg.get());
8384
}
8485

8586
static std::tuple<int, double> findNearest(const std::vector<ObjectPositionExtractorNode::ObjectCandidate>& candidates, const Eigen::Vector3d point)
@@ -130,8 +131,8 @@ void ObjectPositionExtractorNode::depthCallback(const sensor_msgs::Image::ConstP
130131

131132
//Update projector parameters with camera info
132133
extractor_->updateProjectionMatrix(
133-
{static_cast<float>(latestColorCameraInfoPtr->K[0]), static_cast<float>(latestColorCameraInfoPtr->K[4])},
134-
{static_cast<float>(latestColorCameraInfoPtr->K[2]), static_cast<float>(latestColorCameraInfoPtr->K[5])},
134+
{static_cast<float>(latestColorCameraInfo.K[0]), static_cast<float>(latestColorCameraInfo.K[4])},
135+
{static_cast<float>(latestColorCameraInfo.K[2]), static_cast<float>(latestColorCameraInfo.K[5])},
135136
{static_cast<float>(cameraInfoMsg->K[0]) , static_cast<float>(cameraInfoMsg->K[4])},
136137
{static_cast<float>(cameraInfoMsg->K[2]) , static_cast<float>(cameraInfoMsg->K[5])},
137138
latestDepthToColor_
@@ -192,14 +193,16 @@ void ObjectPositionExtractorNode::depthCallback(const sensor_msgs::Image::ConstP
192193
cgs_nav_msgs::ObjectArray objectArray;
193194
for (const auto& candidate : candidates)
194195
{
196+
auto sigma = candidate.variance();
195197

196-
if (candidate.count() < min_pixel_count_for_detection_)
198+
auto count = candidate.count(sigma_coefficient_ * sigma);
199+
if (count < min_pixel_count_for_detection_)
197200
continue;
198201

199-
ROS_INFO_STREAM("detected object (" << candidate.count() << "px)");
200-
auto candidate_pose = candidate.mean();
201-
auto candidate_size = candidate.size();
202+
ROS_INFO_STREAM("detected object (" << count << "/" << candidate.count() << "px)");
202203

204+
auto candidate_pose = candidate.mean(sigma_coefficient_ * sigma);
205+
auto candidate_size = candidate.size(sigma_coefficient_ * sigma);
203206
geometry_msgs::Pose pose;
204207
{
205208
pose.position.x = candidate_pose.x();
@@ -226,7 +229,7 @@ void ObjectPositionExtractorNode::depthCallback(const sensor_msgs::Image::ConstP
226229
o.pose = pose;
227230
o.twist = zero_twist;
228231
o.size = size;
229-
o.confidence = 1.0; //TODO ちゃんとconfidenceを設定する
232+
o.confidence = static_cast<double>(count) / candidate.count(); //set number of pixels in threshold as confidence
230233

231234
objectArray.objects.push_back(o);
232235
}

‎opengl_ros/src/object_position_extractor_nodecore.h‎

Lines changed: 48 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -40,10 +40,11 @@ class ObjectPositionExtractorNode
4040
cv::Mat positionOut_, colorOut_;
4141
bool depthToColorArrived_ = false;
4242
std::array<float, 16> latestDepthToColor_;
43-
cv_bridge::CvImageConstPtr latestColorImagePtr;
44-
sensor_msgs::CameraInfoConstPtr latestColorCameraInfoPtr;
43+
cv_bridge::CvImageConstPtr latestColorImagePtr;
44+
sensor_msgs::CameraInfo latestColorCameraInfo;
4545
double object_separation_distance_;
4646
int min_pixel_count_for_detection_;
47+
double sigma_coefficient_;
4748

4849
void colorCallback(const sensor_msgs::Image::ConstPtr& imageMsg, const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg);
4950
void depthCallback(const sensor_msgs::Image::ConstPtr& imageMsg, const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg);
@@ -67,13 +68,54 @@ class ObjectPositionExtractorNode
6768
return sum / points.size();
6869
}
6970

71+
Eigen::Vector3d mean(double threshold) const
72+
{
73+
auto m = mean();
74+
75+
int count = 0;
76+
Eigen::Vector3d sum_in_variance = {};
77+
for (const auto p : points)
78+
if ((p - m).norm() < threshold)
79+
{
80+
sum_in_variance += p;
81+
++count;
82+
}
83+
84+
return sum_in_variance / count;
85+
}
86+
87+
double variance() const
88+
{
89+
auto m = mean();
90+
double squared_diff_sum = 0;
91+
for (const auto p : points)
92+
{
93+
squared_diff_sum += (p - m).dot(p - m);
94+
}
95+
96+
return sqrt(squared_diff_sum / points.size());
97+
}
98+
7099
int count() const
71100
{
72101
return points.size();
73102
}
74103

75-
Eigen::Vector3d size() const
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+
116+
Eigen::Vector3d size(double threshold = std::numeric_limits<double>::infinity()) const
76117
{
118+
auto m = mean();
77119
double min_x = std::numeric_limits<double>::infinity();
78120
double min_y = std::numeric_limits<double>::infinity();
79121
double min_z = std::numeric_limits<double>::infinity();
@@ -82,6 +124,9 @@ class ObjectPositionExtractorNode
82124
double max_z = -std::numeric_limits<double>::infinity();
83125
for (const auto p : points)
84126
{
127+
if (threshold < (p - m).norm())
128+
continue;
129+
85130
if (p.x() < min_x) min_x = p.x();
86131
if (p.y() < min_y) min_y = p.y();
87132
if (p.z() < min_z) min_z = p.z();

0 commit comments

Comments
 (0)