Skip to content

Commit ac929ce

Browse files
ravijoravijo
committed
added support for azure kinect camera
1 parent 583d09d commit ac929ce

5 files changed

+361
-20
lines changed

‎files/only_person_azurekinect.rviz

+109
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,109 @@
1+
Panels:
2+
- Class: rviz/Displays
3+
Help Height: 78
4+
Name: Displays
5+
Property Tree Widget:
6+
Expanded:
7+
- /Status1
8+
Splitter Ratio: 0.5
9+
Tree Height: 916
10+
- Class: rviz/Selection
11+
Name: Selection
12+
- Class: rviz/Tool Properties
13+
Expanded:
14+
- /2D Pose Estimate1
15+
- /2D Nav Goal1
16+
- /Publish Point1
17+
Name: Tool Properties
18+
Splitter Ratio: 0.5886790156364441
19+
- Class: rviz/Views
20+
Expanded:
21+
- /Current View1
22+
Name: Views
23+
Splitter Ratio: 0.5
24+
- Class: rviz/Time
25+
Experimental: false
26+
Name: Time
27+
SyncMode: 0
28+
SyncSource: ""
29+
Preferences:
30+
PromptSaveOnExit: true
31+
Toolbars:
32+
toolButtonStyle: 2
33+
Visualization Manager:
34+
Class: ""
35+
Displays:
36+
- Class: rviz/MarkerArray
37+
Enabled: true
38+
Marker Topic: /visualization
39+
Name: Human Skeleton
40+
Namespaces:
41+
visualization: true
42+
Queue Size: 100
43+
Value: true
44+
Enabled: true
45+
Global Options:
46+
Background Color: 48; 48; 48
47+
Default Light: true
48+
Fixed Frame: rgb_camera_link
49+
Frame Rate: 30
50+
Name: root
51+
Tools:
52+
- Class: rviz/Interact
53+
Hide Inactive Objects: true
54+
- Class: rviz/MoveCamera
55+
- Class: rviz/Select
56+
- Class: rviz/FocusCamera
57+
- Class: rviz/Measure
58+
- Class: rviz/SetInitialPose
59+
Theta std deviation: 0.2617993950843811
60+
Topic: /initialpose
61+
X std deviation: 0.5
62+
Y std deviation: 0.5
63+
- Class: rviz/SetGoal
64+
Topic: /move_base_simple/goal
65+
- Class: rviz/PublishPoint
66+
Single click: true
67+
Topic: /clicked_point
68+
Value: true
69+
Views:
70+
Current:
71+
Class: rviz/Orbit
72+
Distance: 2.9169700145721436
73+
Enable Stereo Rendering:
74+
Stereo Eye Separation: 0.05999999865889549
75+
Stereo Focal Distance: 1
76+
Swap Stereo Eyes: false
77+
Value: false
78+
Focal Point:
79+
X: 0.07911369949579239
80+
Y: 0.04142170026898384
81+
Z: 2.841330051422119
82+
Focal Shape Fixed Size: true
83+
Focal Shape Size: 0.05000000074505806
84+
Invert Z Axis: false
85+
Name: Current View
86+
Near Clip Distance: 0.009999999776482582
87+
Pitch: -1.4397964477539062
88+
Target Frame: <Fixed Frame>
89+
Value: Orbit (rviz)
90+
Yaw: 4.778579235076904
91+
Saved: ~
92+
Window Geometry:
93+
Displays:
94+
collapsed: false
95+
Height: 1145
96+
Hide Left Dock: false
97+
Hide Right Dock: true
98+
QMainWindow State: 000000ff00000000fd0000000400000000000002320000041ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000041f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000394fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000394000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000040fc0100000002fb0000000800540069006d006500000000000000073f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005030000041f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
99+
Selection:
100+
collapsed: false
101+
Time:
102+
collapsed: false
103+
Tool Properties:
104+
collapsed: false
105+
Views:
106+
collapsed: true
107+
Width: 1851
108+
X: 1989
109+
Y: 27

‎files/person_pointcloud_azurekinect.rviz

+140
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,140 @@
1+
Panels:
2+
- Class: rviz/Displays
3+
Help Height: 78
4+
Name: Displays
5+
Property Tree Widget:
6+
Expanded:
7+
- /Status1
8+
- /Point Cloud1
9+
Splitter Ratio: 0.5
10+
Tree Height: 827
11+
- Class: rviz/Selection
12+
Name: Selection
13+
- Class: rviz/Tool Properties
14+
Expanded:
15+
- /2D Pose Estimate1
16+
- /2D Nav Goal1
17+
- /Publish Point1
18+
Name: Tool Properties
19+
Splitter Ratio: 0.5886790156364441
20+
- Class: rviz/Views
21+
Expanded:
22+
- /Current View1
23+
Name: Views
24+
Splitter Ratio: 0.5
25+
- Class: rviz/Time
26+
Experimental: false
27+
Name: Time
28+
SyncMode: 0
29+
SyncSource: Point Cloud
30+
Preferences:
31+
PromptSaveOnExit: true
32+
Toolbars:
33+
toolButtonStyle: 2
34+
Visualization Manager:
35+
Class: ""
36+
Displays:
37+
- Class: rviz/MarkerArray
38+
Enabled: true
39+
Marker Topic: /visualization
40+
Name: Human Skeleton
41+
Namespaces:
42+
{}
43+
Queue Size: 100
44+
Value: true
45+
- Alpha: 1
46+
Autocompute Intensity Bounds: true
47+
Autocompute Value Bounds:
48+
Max Value: 10
49+
Min Value: -10
50+
Value: true
51+
Axis: Z
52+
Channel Name: intensity
53+
Class: rviz/PointCloud2
54+
Color: 255; 255; 255
55+
Color Transformer: RGB8
56+
Decay Time: 0
57+
Enabled: true
58+
Invert Rainbow: false
59+
Max Color: 255; 255; 255
60+
Max Intensity: 4096
61+
Min Color: 0; 0; 0
62+
Min Intensity: 0
63+
Name: Point Cloud
64+
Position Transformer: XYZ
65+
Queue Size: 10
66+
Selectable: true
67+
Size (Pixels): 3
68+
Size (m): 0.009999999776482582
69+
Style: Flat Squares
70+
Topic: /points2
71+
Unreliable: false
72+
Use Fixed Frame: true
73+
Use rainbow: true
74+
Value: true
75+
Enabled: true
76+
Global Options:
77+
Background Color: 48; 48; 48
78+
Default Light: true
79+
Fixed Frame: rgb_camera_link
80+
Frame Rate: 30
81+
Name: root
82+
Tools:
83+
- Class: rviz/Interact
84+
Hide Inactive Objects: true
85+
- Class: rviz/MoveCamera
86+
- Class: rviz/Select
87+
- Class: rviz/FocusCamera
88+
- Class: rviz/Measure
89+
- Class: rviz/SetInitialPose
90+
Theta std deviation: 0.2617993950843811
91+
Topic: /initialpose
92+
X std deviation: 0.5
93+
Y std deviation: 0.5
94+
- Class: rviz/SetGoal
95+
Topic: /move_base_simple/goal
96+
- Class: rviz/PublishPoint
97+
Single click: true
98+
Topic: /clicked_point
99+
Value: true
100+
Views:
101+
Current:
102+
Class: rviz/Orbit
103+
Distance: 2.9169700145721436
104+
Enable Stereo Rendering:
105+
Stereo Eye Separation: 0.05999999865889549
106+
Stereo Focal Distance: 1
107+
Swap Stereo Eyes: false
108+
Value: false
109+
Focal Point:
110+
X: 0.07911369949579239
111+
Y: 0.04142170026898384
112+
Z: 2.841330051422119
113+
Focal Shape Fixed Size: true
114+
Focal Shape Size: 0.05000000074505806
115+
Invert Z Axis: false
116+
Name: Current View
117+
Near Clip Distance: 0.009999999776482582
118+
Pitch: -1.4397964477539062
119+
Target Frame: <Fixed Frame>
120+
Value: Orbit (rviz)
121+
Yaw: 4.778579235076904
122+
Saved: ~
123+
Window Geometry:
124+
Displays:
125+
collapsed: false
126+
Height: 1056
127+
Hide Left Dock: false
128+
Hide Right Dock: true
129+
QMainWindow State: 000000ff00000000fd000000040000000000000232000003c6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003c6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000394fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000394000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000040fc0100000002fb0000000800540069006d006500000000000000073f000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000507000003c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
130+
Selection:
131+
collapsed: false
132+
Time:
133+
collapsed: false
134+
Tool Properties:
135+
collapsed: false
136+
Views:
137+
collapsed: true
138+
Width: 1855
139+
X: 1975
140+
Y: 14

‎include/ros_openpose/cameraReader.hpp

+26-19
Original file line numberDiff line numberDiff line change
@@ -107,33 +107,40 @@ namespace ros_openpose
107107
}
108108

109109
// compute the point in 3D space for a given pixel without considering distortion
110-
void compute3DPoint(const float pixel_x, const float pixel_y, float (&point)[3])
110+
void compute3DPoint(const float pixelX, const float pixelY, float (&point)[3])
111111
{
112-
/*
113-
* K.at(0) = intrinsic.fx
114-
* K.at(4) = intrinsic.fy
115-
* K.at(2) = intrinsic.ppx
116-
* K.at(5) = intrinsic.ppy
117-
*/
112+
// K.at(0) = intrinsic.fx
113+
// K.at(4) = intrinsic.fy
114+
// K.at(2) = intrinsic.ppx
115+
// K.at(5) = intrinsic.ppy
118116

119117
// our depth frame type is 16UC1 which has unsigned short as an underlying type
120-
auto depth = mDepthImageUsed.at<unsigned short>(static_cast<int>(pixel_y), static_cast<int>(pixel_x));
118+
auto depth = mDepthImageUsed.at<unsigned short>(static_cast<int>(pixelY), static_cast<int>(pixelX));
121119

122-
// no need to proceed further if the depth is zero
120+
// no need to proceed further if the depth is zero or less than zero
123121
// the depth represents the distance of an object placed infront of the camera
124-
// therefore depth must be always a positive number
122+
// therefore depth must always be a positive number
125123
if (depth <= 0)
126124
return;
127125

128-
// convert to meter (SI units)
129-
auto depthSI = depth * 0.001f;
130-
131-
auto x = (pixel_x - mSPtrCameraInfo->K.at(2)) / mSPtrCameraInfo->K.at(0);
132-
auto y = (pixel_y - mSPtrCameraInfo->K.at(5)) / mSPtrCameraInfo->K.at(4);
133-
134-
point[0] = depthSI * x;
135-
point[1] = depthSI * y;
136-
point[2] = depthSI;
126+
// the following calculation can also be done by image_geometry
127+
// for example:
128+
// image_geometry::PinholeCameraModel camModel;
129+
// camModel.fromCameraInfo(mSPtrCameraInfo);
130+
// cv::Point2d depthPixel(pixelX, pixelY);
131+
// auto point3d = camModel.projectPixelTo3dRay(depthPixel)
132+
// auto depth = mDepthImageUsed.at<unsigned short>(depthPixel);
133+
// point[0] = depth * point3d.x;
134+
// point[1] = depth * point3d.y;
135+
// point[2] = depth * point3d.z;
136+
// for more info., please see http://wiki.ros.org/image_geometry
137+
138+
auto x = (pixelX - mSPtrCameraInfo->K.at(2)) / mSPtrCameraInfo->K.at(0);
139+
auto y = (pixelY - mSPtrCameraInfo->K.at(5)) / mSPtrCameraInfo->K.at(4);
140+
141+
point[0] = depth * x;
142+
point[1] = depth * y;
143+
point[2] = depth;
137144
}
138145
};
139146
}

‎launch/config_azurekinect.launch

+76
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
<?xml version="1.0"?>
2+
<!--
3+
config_azurekinect.launch
4+
Author: Ravi Joshi
5+
Note: Do not run this file directly. Please call run.launch file instead.
6+
Date: 2020/08/10
7+
-->
8+
<launch>
9+
<!-- rostopic to subscribe for color images -->
10+
<arg name="color_topic" default="/rgb/image_raw" />
11+
12+
<!-- rostopic to subscribe for depth images -->
13+
<arg name="depth_topic" default="/depth_to_rgb/image_raw" />
14+
15+
<!-- rostopic to subscribe for camera calibration parameters -->
16+
<arg name="cam_info_topic" default="/rgb/camera_info" />
17+
18+
<!-- frame id for point cloud and 3D skeleton data -->
19+
<arg name="frame_id" default="rgb_camera_link" />
20+
21+
<!-- realsense camera provide depth information.
22+
hence unset the flag. -->
23+
<arg name="no_depth" default="false" />
24+
25+
<!-- note that the following parameters are going to assigned by other launch file upon call. -->
26+
<arg name="rviz" default="" />
27+
<arg name="print" default="" />
28+
<arg name="skeleton" default="" />
29+
<arg name="pub_topic" default="" />
30+
<arg name="pointcloud" default="" />
31+
<arg name="id_text_size" default="" />
32+
<arg name="openpose_args" default="" />
33+
<arg name="id_text_offset" default="" />
34+
<arg name="skeleton_hands" default="" />
35+
<arg name="skeleton_line_width" default="" />
36+
37+
<include file="$(find ros_openpose)/launch/core.launch" >
38+
<arg name="print" value="$(arg print)" />
39+
<arg name="no_depth" value="$(arg no_depth)" />
40+
<arg name="skeleton" value="$(arg skeleton)" />
41+
<arg name="frame_id" value="$(arg frame_id)" />
42+
<arg name="pub_topic" value="$(arg pub_topic)" />
43+
<arg name="color_topic" value="$(arg color_topic)" />
44+
<arg name="depth_topic" value="$(arg depth_topic)" />
45+
<arg name="id_text_size" value="$(arg id_text_size)" />
46+
<arg name="openpose_args" value="$(arg openpose_args)" />
47+
<arg name="id_text_offset" value="$(arg id_text_offset)" />
48+
<arg name="skeleton_hands" value="$(arg skeleton_hands)" />
49+
<arg name="cam_info_topic" value="$(arg cam_info_topic)" />
50+
<arg name="skeleton_line_width" value="$(arg skeleton_line_width)" />
51+
</include>
52+
53+
<!-- invoke azure kinect camera ros package -->
54+
<group if="$(arg pointcloud)" >
55+
<include file="$(find azure_kinect_ros_driver)/launch/driver.launch" >
56+
<arg name="required" value="true" />
57+
<arg name="point_cloud_in_depth_frame" value="true" />
58+
</include>
59+
<!-- invoke rviz -->
60+
<group if="$(arg rviz)" >
61+
<node pkg="rviz" type="rviz" respawn="false" required="true" name="rviz" args="-d $(find ros_openpose)/files/person_pointcloud_azurekinect.rviz" />
62+
</group>
63+
</group>
64+
65+
<!-- invoke azure kinect camera ros package -->
66+
<group unless="$(arg pointcloud)" >
67+
<include file="$(find azure_kinect_ros_driver)/launch/driver.launch" >
68+
<arg name="required" value="true" />
69+
<arg name="point_cloud_in_depth_frame" value="false" />
70+
</include>
71+
<!-- invoke rviz -->
72+
<group if="$(arg rviz)" >
73+
<node pkg="rviz" type="rviz" respawn="false" required="true" name="rviz" args="-d $(find ros_openpose)/files/only_person_azurekinect.rviz" />
74+
</group>
75+
</group>
76+
</launch>

0 commit comments

Comments
 (0)