publish tracked object visible points

CMakeLists.txt

@@ -160,6 +160,11 @@ set(PCL_BUILD_TYPE Release)
 #add_executable(kinect2_read_write_node src/kinect2_read_write_node.cpp)
 add_executable(kinect2_csl_tracker src/kinect2_freenect_tracker.cpp)
 add_executable(dian_csl_tracker src/dian_kinect2_tracker.cpp)
+
+add_executable(csl_tracker_new src/csl_new_kinect2_tracker.cpp)
+
+add_executable(pointcloud2pcd src/tools/pointcloud2_to_pcd.cpp)
+
 add_executable(openni2_grabber_test src/openni2_test.cpp)
 add_executable(pcd_cloud_viewer src/pcd_cloud_viewer.cpp)
 ## Add cmake target dependencies of the executable
@@ -249,10 +254,23 @@ target_link_libraries(pcd_cloud_viewer
 )
 
 
+target_link_libraries(pointcloud2pcd
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+#  ${freenect2_LIBRARY}
+#  -lfreenect2
+)
+
 target_link_libraries(dian_csl_tracker
   ${catkin_LIBRARIES}
   ${PCL_LIBRARIES}
   ${math_utils_LIBRARIES}
+)
+
+target_link_libraries(csl_tracker_new
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${math_utils_LIBRARIES}
 
 #  ${freenect2_LIBRARY}
 #  -lfreenect2

config/camera_calibration.yaml

@@ -0,0 +1,12 @@
+detection:
+    camera:
+        rgb:
+            fx: 1059.9465578241038
+            fy: 1053.9326808799726
+            cx: 954.88326677588441
+            cy: 523.73858291060583
+        depth:
+            fx: 366.94757270064110
+            fy: 364.79117165721783
+            cx: 242.98551682775121
+            cy: 207.69853575457685

data/kinect2_calibration_files/csl_kinect2/calib_color.yaml

@@ -0,0 +1,27 @@
+%YAML:1.0
+---
+cameraMatrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1.0264598052776691e+03, 0., 9.1772347666319945e+02, 0.,
+       1.0254214510585814e+03, 5.8634673850059335e+02, 0., 0., 1. ]
+distortionCoefficients: !!opencv-matrix
+   rows: 1
+   cols: 5
+   dt: d
+   data: [ 5.5008336565507458e-02, -1.0233881019355782e-01,
+       7.7251010585815041e-03, -3.2070707781992960e-03,
+       4.3583619594476944e-02 ]
+rotation: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+projection: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [ 1.0264598052776691e+03, 0., 9.1772347666319945e+02, 0., 0.,
+       1.0254214510585814e+03, 5.8634673850059335e+02, 0., 0., 0., 1.,
+       0., 0., 0., 0., 1. ]

data/kinect2_calibration_files/csl_kinect2/calib_color2.yaml

@@ -0,0 +1,27 @@
+%YAML:1.0
+---
+cameraMatrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1.0530049852947350e+03, 0., 9.6457685251628504e+02, 0.,
+       1.0502434494898575e+03, 4.7815744522591950e+02, 0., 0., 1. ]
+distortionCoefficients: !!opencv-matrix
+   rows: 1
+   cols: 5
+   dt: d
+   data: [ 7.2241836443579940e-02, -1.5890054983889132e-01,
+       -1.0606549896679238e-02, 1.7569701096418831e-03,
+       8.5291449236173628e-02 ]
+rotation: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+projection: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [ 1.0530049852947350e+03, 0., 9.6457685251628504e+02, 0., 0.,
+       1.0502434494898575e+03, 4.7815744522591950e+02, 0., 0., 0., 1.,
+       0., 0., 0., 0., 1. ]

data/kinect2_calibration_files/csl_kinect2/calib_ir.yaml

@@ -0,0 +1,27 @@
+%YAML:1.0
+---
+cameraMatrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 3.5002392528267427e+02, 0., 2.5098792650438548e+02, 0.,
+       3.5056476479313227e+02, 1.8303580307019357e+02, 0., 0., 1. ]
+distortionCoefficients: !!opencv-matrix
+   rows: 1
+   cols: 5
+   dt: d
+   data: [ 1.4696017264088765e-01, -4.4966826515557901e-01,
+       -4.7576886187111677e-03, -1.1559560677660397e-02,
+       4.8745194129575842e-01 ]
+rotation: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+projection: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [ 3.5002392528267427e+02, 0., 2.5098792650438548e+02, 0., 0.,
+       3.5056476479313227e+02, 1.8303580307019357e+02, 0., 0., 0., 1.,
+       0., 0., 0., 0., 1. ]