Skip to content

Commit 15913f0

Browse files
mohammedarimohammedari
committed
removed color input, changed parameter names
1 parent 48d28d6 commit 15913f0

File tree

7 files changed

+64
-276
lines changed

7 files changed

+64
-276
lines changed

‎opengl_ros/launch/depth_image_projection.launch‎

Lines changed: 29 additions & 93 deletions
Original file line numberDiff line numberDiff line change
@@ -9,102 +9,38 @@
99
<arg name="depth_unknown_color" default="0"/> <!-- 0 to 255 -->
1010
<arg name="grid_map_width" default="200"/>
1111
<arg name="grid_map_height" default="200"/>
12-
<arg name="grid_map_resolution" default="0.025"/>
13-
<arg name="grid_map_layer_height" default="0.3"/>
14-
<arg name="grid_map_accumulation_weight" default="0.05"/> <!-- 0.004 for 1/255 -->
12+
<arg name="grid_map_resolution" default="0.1"/>
13+
<arg name="grid_map_layer_height" default="3.0"/>
14+
<arg name="grid_map_accumulation_weight" default="0.004"/> <!-- 0.004 for 1/255 -->
1515
<arg name="frame_rate" default="30"/>
1616

17-
<arg name="d435" default="true"/>
18-
<arg name="record" default="false"/>
19-
20-
<group ns="d435" if="$(arg d435)">
21-
<node pkg="nodelet" type="nodelet" name="realsense2_camera_manager" args="manager" output="screen"/>
22-
<node pkg="nodelet" type="nodelet" name="realsense2_camera" args="load realsense2_camera/RealSenseNodeFactory realsense2_camera_manager">
23-
<param name="enable_pointcloud" type="bool" value="false"/>
24-
<param name="enable_sync" type="bool" value="false"/>
25-
<param name="align_depth" type="bool" value="false"/>
26-
<param name="enable_fisheye" type="bool" value="false"/>
27-
<param name="enable_imu" type="bool" value="false"/>
28-
29-
<param name="enable_depth" type="bool" value="true"/>
30-
<param name="enable_infra1" type="bool" value="false"/>
31-
<param name="enable_infra2" type="bool" value="false"/>
32-
<param name="enable_color" type="bool" value="true"/>
33-
34-
<param name="depth_width" type="int" value="$(arg depth_width)"/>
35-
<param name="depth_height" type="int" value="$(arg depth_height)"/>
36-
<param name="depth_fps" type="int" value="$(arg frame_rate)"/>
37-
<param name="color_width" type="int" value="$(arg depth_width)"/>
38-
<param name="color_height" type="int" value="$(arg depth_height)"/>
39-
<param name="color_fps" type="int" value="$(arg frame_rate)"/>
40-
<param name="base_frame_id" type="str" value="d435_link"/>
41-
<param name="depth_frame_id" type="str" value="d435_depth_link"/>
42-
<param name="depth_optical_frame_id" type="str" value="d435_depth_optical"/>
43-
</node>
44-
</group>
45-
4617
<node pkg="opengl_ros" type="depth_image_projector"
4718
name="depth_image_projector" output="screen">
48-
<param name="color_frame_id" value="d435_color_optical"/>
49-
<param name="depth_frame_id" value="d435_depth_optical"/>
50-
<param name="map_frame_id" value="d435_depth_link"/>
51-
<param name="tf_wait_duration" value="0.1"/>
52-
53-
<param name="colorWidth" value="$(arg depth_width)"/>
54-
<param name="colorHeight" value="$(arg depth_height)"/>
55-
<param name="depthWidth" value="$(arg depth_width)"/>
56-
<param name="depthHeight" value="$(arg depth_height)"/>
57-
58-
<param name="minDepth" value="$(arg depth_min)"/>
59-
<param name="maxDepth" value="$(arg depth_max)"/>
60-
<param name="depthHitThreshold" value="$(arg depth_hit_threshold)"/>
61-
<param name="unknownDepthColor" value="$(arg depth_unknown_color)"/>
62-
63-
<param name="gridMapWidth" value="$(arg grid_map_width)"/>
64-
<param name="gridMapHeight" value="$(arg grid_map_height)"/>
65-
<param name="gridMapResolution" value="$(arg grid_map_resolution)"/>
66-
<param name="gridMapLayerHeight" value="$(arg grid_map_layer_height)"/>
67-
<param name="gridMapAccumulationWeight" value="$(arg grid_map_accumulation_weight)"/>
68-
69-
<param name="threshold_l" value="0"/>
70-
<param name="svm_coef_a" value="0.266602955271"/>
71-
<param name="svm_coef_b" value="0.0444656815907"/>
72-
<param name="svm_intercept" value="-44.3271851592"/>
73-
74-
<param name="vertex_shader" value="$(find opengl_ros_lib)/shader/vs_depth_image_projection.glsl"/>
75-
<param name="geometry_shader" value="$(find opengl_ros_lib)/shader/gs_depth_image_projection.glsl"/>
76-
<param name="fragment_shader" value="$(find opengl_ros_lib)/shader/fs_depth_image_projection.glsl"/>
77-
<param name="vertex_shader_scaling" value="$(find opengl_ros_lib)/shader/vs_passthrough.glsl"/>
78-
<param name="fragment_shader_scaling" value="$(find opengl_ros_lib)/shader/fs_rgb_to_occupancy_grid.glsl"/>
79-
80-
<remap from="~depth_to_color" to="/d435/extrinsics/depth_to_color"/>
81-
<remap from="color_in" to="/d435/color/image_raw"/>
82-
<remap from="depth_in" to="/d435/depth/image_rect_raw"/>
83-
<remap from="image_out" to="/occupancy_grid_image"/>
19+
<param name="depth_frame_id" value="d435_depth_optical"/>
20+
<param name="map_frame_id" value="base_link_hor"/>
21+
<param name="tf_wait_duration" value="0.1"/>
22+
23+
<param name="depth_width" value="$(arg depth_width)"/>
24+
<param name="depth_height" value="$(arg depth_height)"/>
25+
26+
<param name="min_depth" value="$(arg depth_min)"/>
27+
<param name="max_depth" value="$(arg depth_max)"/>
28+
<param name="depth_hit_threshold" value="$(arg depth_hit_threshold)"/>
29+
<param name="unknown_depth_color" value="$(arg depth_unknown_color)"/>
30+
31+
<param name="grid_map_width" value="$(arg grid_map_width)"/>
32+
<param name="grid_map_height" value="$(arg grid_map_height)"/>
33+
<param name="grid_map_resolution" value="$(arg grid_map_resolution)"/>
34+
<param name="grid_map_layer_height" value="$(arg grid_map_layer_height)"/>
35+
<param name="grid_map_accumulation_weight" value="$(arg grid_map_accumulation_weight)"/>
36+
37+
<param name="vertex_shader" value="$(find opengl_ros_lib)/shader/vs_depth_image_projection.glsl"/>
38+
<param name="geometry_shader" value="$(find opengl_ros_lib)/shader/gs_depth_image_projection.glsl"/>
39+
<param name="fragment_shader" value="$(find opengl_ros_lib)/shader/fs_depth_image_projection.glsl"/>
40+
<param name="vertex_shader_scaling" value="$(find opengl_ros_lib)/shader/vs_passthrough.glsl"/>
41+
<param name="fragment_shader_scaling" value="$(find opengl_ros_lib)/shader/fs_rgb_to_occupancy_grid.glsl"/>
42+
43+
<remap from="depth_in" to="/d435/depth/image_rect_raw"/>
44+
<remap from="~map" to="/occupancy_grid2"/>
8445
</node>
85-
86-
<node pkg="image_view" type="image_view" name="viewer_depth">
87-
<remap from="image" to="/d435/depth/image_rect_raw"/>
88-
</node>
89-
90-
<node pkg="image_view" type="image_view" name="viewer_color">
91-
<remap from="image" to="/d435/color/image_raw"/>
92-
</node>
93-
94-
<node pkg="image_view" type="image_view" name="viewer_out">
95-
<remap from="image" to="/occupancy_grid_image"/>
96-
</node>
97-
98-
<node pkg="rosbag" type="record" name="logger" if="$(arg d435)"
99-
args="-o $(arg bag_prefix)
100-
--lz4
101-
-a
102-
-x /(d435|occupancy_grid_image|viewer_depth|viewer_color|viewer_out)/(.*)
103-
/d435/color/camera_info
104-
/d435/color/image_raw
105-
/d435/depth/camera_info
106-
/d435/depth/image_rect_raw
107-
/d435/extrinsics/depth_to_color
108-
/occupancy_grid_image
109-
"/>
11046
</launch>

‎opengl_ros/src/depth_image_projector_nodecore.cpp‎

Lines changed: 13 additions & 97 deletions
Original file line numberDiff line numberDiff line change
@@ -9,37 +9,29 @@ DepthImageProjectorNode::DepthImageProjectorNode(const ros::NodeHandle& nh, cons
99
: nh_(nh_private), it_(nh)
1010
{
1111
mapPublisher_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 1);
12-
13-
//TODO synchronize two image topics
14-
colorSubscriber_ = it_.subscribeCamera("color_in" , 1, &DepthImageProjectorNode::colorCallback, this);
1512
depthSubscriber_ = it_.subscribeCamera("depth_in" , 1, &DepthImageProjectorNode::depthCallback, this);
1613

17-
// frame ids for tf which associates color frame with depth frame
18-
nh_.param<std::string>("color_frame_id", color_frame_id_, "d435_color_optical");
1914
nh_.param<std::string>("depth_frame_id", depth_frame_id_, "d435_depth_optical");
20-
nh_.param<std::string>("map_frame_id", map_frame_id_, "d435_depth_link");
15+
nh_.param<std::string>("map_frame_id", map_frame_id_, "base_link_hor");
2116
nh_.param<double>("tf_wait_duration", tf_wait_duration_, 0.1);
2217

23-
int colorWidth, colorHeight;
24-
nh_.param<int>("colorWidth" , colorWidth , 640);
25-
nh_.param<int>("colorHeight", colorHeight, 360);
2618
int depthWidth, depthHeight;
27-
nh_.param<int>("depthWidth" , depthWidth , 640);
28-
nh_.param<int>("depthHeight", depthHeight, 360);
19+
nh_.param<int>("depth_width" , depthWidth , 640);
20+
nh_.param<int>("depth_height", depthHeight, 360);
2921

3022
double gridMapLayerHeight, gridMapAccumulationWeight;
31-
nh_.param<int>("gridMapWidth" , gridMapWidth_ , 1000);
32-
nh_.param<int>("gridMapHeight", gridMapHeight_, 1000);
33-
nh_.param<double>("gridMapResolution", gridMapResolution_, 0.01);
34-
nh_.param<double>("gridMapLayerHeight", gridMapLayerHeight, 1);
35-
nh_.param<double>("gridMapAccumulationWeight", gridMapAccumulationWeight, 1);
23+
nh_.param<int>("grid_map_width" , gridMapWidth_ , 1000);
24+
nh_.param<int>("grid_map_height", gridMapHeight_, 1000);
25+
nh_.param<double>("grid_map_resolution", gridMapResolution_, 0.01);
26+
nh_.param<double>("grid_map_layer_height", gridMapLayerHeight, 1);
27+
nh_.param<double>("grid_map_accumulation_weight", gridMapAccumulationWeight, 1);
3628

3729
double minDepth, maxDepth, depthHitThreshold;
3830
int unknownDepthColor;
39-
nh_.param<double>("minDepth", minDepth, 0.105);
40-
nh_.param<double>("maxDepth", maxDepth, 10);
41-
nh_.param<double>("depthHitThreshold", depthHitThreshold, 0.95);
42-
nh_.param<int>("unknownDepthColor", unknownDepthColor, 255);
31+
nh_.param<double>("min_depth", minDepth, 0.105);
32+
nh_.param<double>("max_depth", maxDepth, 10);
33+
nh_.param<double>("depth_hit_threshold", depthHitThreshold, 0.95);
34+
nh_.param<int>("unknown_depth_color", unknownDepthColor, 255);
4335

4436
std::string vertexShader, geometryShader, fragmentShader, vertexShaderScaling, fragmentShaderScaling;
4537
nh_.param<std::string>("vertex_shader" , vertexShader , "");
@@ -49,7 +41,6 @@ DepthImageProjectorNode::DepthImageProjectorNode(const ros::NodeHandle& nh, cons
4941
nh_.param<std::string>("fragment_shader_scaling", fragmentShaderScaling, "");
5042

5143
projector_ = std::make_unique<cgs::DepthImageProjector>(
52-
colorWidth, colorHeight,
5344
depthWidth, depthHeight,
5445
gridMapWidth_, gridMapHeight_, gridMapResolution_,
5546
gridMapLayerHeight, gridMapAccumulationWeight,
@@ -58,36 +49,9 @@ DepthImageProjectorNode::DepthImageProjectorNode(const ros::NodeHandle& nh, cons
5849
vertexShaderScaling, fragmentShaderScaling
5950
);
6051

61-
float threshold_l, svm_coef_a, svm_coef_b, svm_intercept;
62-
nh_.param<float>("threshold_l" , threshold_l , 0);
63-
nh_.param<float>("svm_coef_a" , svm_coef_a , 0);
64-
nh_.param<float>("svm_coef_b" , svm_coef_b , 0);
65-
nh_.param<float>("svm_intercept", svm_intercept, 0);
66-
67-
//TODO Remove entire color image process as color image is not used anymore
68-
//projector_->uniform("threshold_l" , threshold_l);
69-
//projector_->uniform("svm_coef_a" , svm_coef_a);
70-
//projector_->uniform("svm_coef_b" , svm_coef_b);
71-
//projector_->uniform("svm_intercept", svm_intercept);
72-
7352
output_.create(gridMapHeight_, gridMapWidth_, CV_8UC1);
7453
}
7554

76-
void DepthImageProjectorNode::colorCallback(const sensor_msgs::Image::ConstPtr& imageMsg, const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg)
77-
{
78-
try
79-
{
80-
latestColorImagePtr = cv_bridge::toCvShare(imageMsg, sensor_msgs::image_encodings::TYPE_8UC3);
81-
}
82-
catch (cv_bridge::Exception& e)
83-
{
84-
ROS_ERROR_STREAM("cv_bridge exception: " << e.what());
85-
return;
86-
}
87-
88-
latestColorCameraInfo = *(cameraInfoMsg.get());
89-
}
90-
9155
void DepthImageProjectorNode::depthCallback(const sensor_msgs::Image::ConstPtr& imageMsg, const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg)
9256
{
9357
cv_bridge::CvImageConstPtr cv_ptr;
@@ -100,21 +64,6 @@ void DepthImageProjectorNode::depthCallback(const sensor_msgs::Image::ConstPtr&
10064
ROS_ERROR_STREAM("cv_bridge exception: " << e.what());
10165
return;
10266
}
103-
104-
if (!latestColorImagePtr)
105-
{
106-
ROS_WARN_STREAM("color stream not ready");
107-
return;
108-
}
109-
110-
//TODO Remove entire color image process as color image is not used anymore
111-
//// Update & check whether depthToColor is valid.
112-
//bool is_conversion_valid = updateDepthToColor();
113-
//if (!is_conversion_valid)
114-
//{
115-
// ROS_WARN_STREAM("extrinsics parameter not arrivied yet");
116-
// return;
117-
//}
11867

11968
std::array<float, 16> depthToMap;
12069
try
@@ -136,18 +85,13 @@ void DepthImageProjectorNode::depthCallback(const sensor_msgs::Image::ConstPtr&
13685

13786
//Update projector parameters with camera info
13887
projector_->updateProjectionMatrix(
139-
{static_cast<float>(latestColorCameraInfo.K[0]), static_cast<float>(latestColorCameraInfo.K[4])},
140-
{static_cast<float>(latestColorCameraInfo.K[2]), static_cast<float>(latestColorCameraInfo.K[5])},
14188
{static_cast<float>(cameraInfoMsg->K[0]) , static_cast<float>(cameraInfoMsg->K[4])},
14289
{static_cast<float>(cameraInfoMsg->K[2]) , static_cast<float>(cameraInfoMsg->K[5])},
143-
latestDepthToColor_,
14490
depthToMap
14591
);
14692

14793
//Perform projection
148-
const auto& color = latestColorImagePtr->image;
149-
const auto& depth = cv_ptr->image;
150-
projector_->project(output_, color, depth);
94+
projector_->project(output_, cv_ptr->image);
15195

15296
//Publish
15397
nav_msgs::MapMetaData mapInfo;
@@ -187,34 +131,6 @@ void DepthImageProjectorNode::getTransformMatrixArray(const tf::Transform& trans
187131
}; //column major order
188132
}
189133

190-
bool DepthImageProjectorNode::updateDepthToColor()
191-
{
192-
// Calculation is performed only once since depthToColor does not change.
193-
if (depthToColorArrived_)
194-
{
195-
return true;
196-
}
197-
198-
try
199-
{
200-
if (tfListener_.waitForTransform(color_frame_id_, depth_frame_id_, ros::Time(0), ros::Duration(tf_wait_duration_)))
201-
{
202-
tf::StampedTransform transform;
203-
tfListener_.lookupTransform(color_frame_id_, depth_frame_id_, ros::Time(0), transform);
204-
205-
getTransformMatrixArray(transform, latestDepthToColor_);
206-
207-
depthToColorArrived_ = true;
208-
}
209-
}
210-
catch (tf::TransformException& ex)
211-
{
212-
ROS_WARN("Transform Exception in updateDepthToColor(). %s", ex.what());
213-
}
214-
215-
return depthToColorArrived_;
216-
}
217-
218134
void DepthImageProjectorNode::run()
219135
{
220136
ros::spin();

‎opengl_ros/src/depth_image_projector_nodecore.h‎

Lines changed: 0 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,6 @@ class DepthImageProjectorNode
2222
image_transport::ImageTransport it_;
2323

2424
//tf
25-
std::string color_frame_id_;
2625
std::string depth_frame_id_;
2726
std::string map_frame_id_;
2827
tf::TransformListener tfListener_;
@@ -34,22 +33,14 @@ class DepthImageProjectorNode
3433

3534
//Publishers and Subscribers
3635
ros::Publisher mapPublisher_;
37-
image_transport::CameraSubscriber colorSubscriber_;
3836
image_transport::CameraSubscriber depthSubscriber_;
39-
ros::Subscriber depthToColorSubscriber_;
4037

4138
//Other members
4239
std::unique_ptr<cgs::DepthImageProjector> projector_;
4340
cv::Mat output_;
4441

45-
cv_bridge::CvImageConstPtr latestColorImagePtr;
46-
sensor_msgs::CameraInfo latestColorCameraInfo;
47-
void colorCallback(const sensor_msgs::Image::ConstPtr& imageMsg, const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg);
4842
void depthCallback(const sensor_msgs::Image::ConstPtr& imageMsg, const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg);
49-
bool depthToColorArrived_ = false;
5043
void getTransformMatrixArray(const tf::Transform& transform, std::array<float, 16>& matrix);
51-
std::array<float, 16> latestDepthToColor_;
52-
bool updateDepthToColor();
5344

5445
public:
5546
DepthImageProjectorNode(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private);

‎opengl_ros_lib/include/depth_image_projector.h‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,6 @@ class DepthImageProjector final
1717
{
1818
public:
1919
DepthImageProjector(
20-
int colorWidth, int colorHeight,
2120
int depthWidth, int depthHeight,
2221
int gridMapWidth, int gridMapHeight, float gridMapResolution,
2322
float gridMapLayerHeight, float gridMapAccumulationWeight,
@@ -27,10 +26,9 @@ class DepthImageProjector final
2726
~DepthImageProjector();
2827

2928
void updateProjectionMatrix(
30-
const std::array<float, 2> colorFocalLength, const std::array<float, 2> colorCenter,
3129
const std::array<float, 2> depthFocalLength, const std::array<float, 2> depthCenter,
32-
const std::array<float, 16> depthToColor, const std::array<float, 16> depthToMap);
33-
void project(cv::Mat& dest, const cv::Mat& color, const cv::Mat& depth);
30+
const std::array<float, 16> depthToMap);
31+
void project(cv::Mat& dest, const cv::Mat& depth);
3432

3533
DepthImageProjector(const DepthImageProjector&) = delete;
3634
DepthImageProjector& operator=(const DepthImageProjector&) = delete;

‎opengl_ros_lib/shader/fs_depth_image_projection.glsl‎

Lines changed: 2 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -9,16 +9,8 @@ out vec3 fragmentColor;
99
//y ... 1.0 if valid depth is measured in any ray
1010
//z ... 1.0 if any ray achieved at the pixel
1111

12-
//TODO Remove entire color image process as color image is not used anymore
13-
uniform sampler2D colorTexture;
14-
uniform float threshold_l;
15-
uniform float svm_coef_a;
16-
uniform float svm_coef_b;
17-
uniform float svm_intercept;
18-
//TODO Remove entire color image process as color image is not used anymore
19-
20-
in float depth;
21-
in float hitDepth;
12+
in float depth; //depth value of the pixel on the line
13+
in float hitDepth; //depth value of the end of the line
2214

2315
void main(void)
2416
{

‎opengl_ros_lib/shader/vs_depth_image_projection.glsl‎

Lines changed: 0 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -6,13 +6,6 @@ uniform vec2 depthFocalLength;
66
uniform vec2 depthCenter;
77
uniform vec2 validDepthInMeter;
88

9-
//TODO Remove entire color image process as color image is not used anymore
10-
uniform vec2 colorSize;
11-
uniform vec2 colorFocalLength;
12-
uniform vec2 colorCenter;
13-
uniform mat4 depthToColor;
14-
//TODO Remove entire color image process as color image is not used anymore
15-
169
uniform mat4 depthToMap;
1710

1811
uniform vec2 gridMapSize;

0 commit comments

Comments
 (0)