Added cam calibration and made changes to use with opencv4

kinect2_bridge/data/026866743647/calib_color.yaml

@@ -0,0 +1,27 @@
+%YAML:1.0
+---
+cameraMatrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1.0551315070752844e+03, 0., 9.6084149597990870e+02, 0.,
+       1.0552244286514840e+03, 5.2607365219992914e+02, 0., 0., 1. ]
+distortionCoefficients: !!opencv-matrix
+   rows: 1
+   cols: 5
+   dt: d
+   data: [ 4.1512151977835615e-02, -5.3638742252239899e-02,
+       1.2037833875121360e-03, 1.6398998861241888e-03,
+       1.4649620958823243e-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.0551315070752844e+03, 0., 9.6084149597990870e+02, 0., 0.,
+       1.0552244286514840e+03, 5.2607365219992914e+02, 0., 0., 0., 1.,
+       0., 0., 0., 0., 1. ]

kinect2_bridge/data/026866743647/calib_depth.yaml

@@ -0,0 +1,3 @@
+%YAML:1.0
+---
+depthShift: -2.0190552153934949e+01

kinect2_bridge/data/026866743647/calib_ir.yaml

@@ -0,0 +1,27 @@
+%YAML:1.0
+---
+cameraMatrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 3.6408722690985758e+02, 0., 2.5321652194754324e+02, 0.,
+       3.6441218439462546e+02, 2.1121233784388843e+02, 0., 0., 1. ]
+distortionCoefficients: !!opencv-matrix
+   rows: 1
+   cols: 5
+   dt: d
+   data: [ 1.0098315227010919e-01, -2.8037375106147122e-01,
+       -2.7329384178549127e-04, -1.0353851549988523e-03,
+       1.0149158516883695e-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.6408722690985758e+02, 0., 2.5321652194754324e+02, 0., 0.,
+       3.6441218439462546e+02, 2.1121233784388843e+02, 0., 0., 0., 1.,
+       0., 0., 0., 0., 1. ]

kinect2_bridge/data/026866743647/calib_pose.yaml

@@ -0,0 +1,34 @@
+%YAML:1.0
+---
+rotation: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 9.9995368070728241e-01, 5.4592591576380885e-03,
+       -7.9267224883975071e-03, -5.4952497616097993e-03,
+       9.9997465956253362e-01, -4.5257555007433342e-03,
+       7.9018143496193058e-03, 4.5691051908142545e-03,
+       9.9995834143615170e-01 ]
+translation: !!opencv-matrix
+   rows: 3
+   cols: 1
+   dt: d
+   data: [ -5.2437310020516291e-02, 1.3500952799609413e-03,
+       -2.6124874376302542e-03 ]
+essential: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ -3.6880687122975893e-06, 2.6185899634074729e-03,
+       1.3382155575390419e-03, -2.1980165402843577e-03,
+       2.2532933943897988e-04, 5.2455834020411291e-02,
+       -1.0618766291127708e-03, -5.2443351756161619e-02,
+       2.4802027488668086e-04 ]
+fundamental: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ -2.0446880409373407e-09, 1.4504678454111094e-06,
+       -3.5717195853936663e-05, -1.2184866189394339e-06,
+       1.2480158619743641e-07, 1.0869574634637794e-02,
+       2.1810852356492393e-05, -3.2109829830322900e-02, 1. ]

kinect2_bridge/launch/kinect2_bridge.launch

@@ -2,22 +2,22 @@
 
   <arg name="base_name"         default="kinect2"/>
   <arg name="sensor"            default=""/>
-  <arg name="publish_tf"        default="false"/>
+  <arg name="publish_tf"        default="true"/>
   <arg name="base_name_tf"      default="$(arg base_name)"/>
   <arg name="fps_limit"         default="-1.0"/>
   <arg name="calib_path"        default="$(find kinect2_bridge)/data/"/>
   <arg name="use_png"           default="false"/>
   <arg name="jpeg_quality"      default="90"/>
   <arg name="png_level"         default="1"/>
-  <arg name="depth_method"      default="opengl"/>
+  <arg name="depth_method"      default="cpu"/>
   <arg name="depth_device"      default="-1"/>
   <arg name="reg_method"        default="cpu"/>
   <arg name="reg_device"        default="-1"/>
   <arg name="max_depth"         default="12.0"/>
   <arg name="min_depth"         default="0.1"/>
-  <arg name="queue_size"        default="5"/>
-  <arg name="bilateral_filter"  default="true"/>
-  <arg name="edge_aware_filter" default="true"/>
+  <arg name="queue_size"        default="1"/>
+  <arg name="bilateral_filter"  default="false"/>
+  <arg name="edge_aware_filter" default="false"/>
   <arg name="worker_threads"    default="4"/>
   <arg name="machine"           default="localhost"/>
   <arg name="nodelet_manager"   default="$(arg base_name)"/>

kinect2_calibration/src/kinect2_calibration.cpp

@@ -743,7 +743,7 @@ class CameraCalibration
 #if CV_MAJOR_VERSION == 2
     error = cv::stereoCalibrate(pointsBoard, pointsIr, pointsColor, cameraMatrixIr, distortionIr, cameraMatrixColor, distortionColor, sizeColor,
                                 rotation, translation, essential, fundamental, termCriteria, cv::CALIB_FIX_INTRINSIC);
-#elif CV_MAJOR_VERSION == 3
+#elif CV_MAJOR_VERSION == 3 || CV_MAJOR_VERSION == 4
     error = cv::stereoCalibrate(pointsBoard, pointsIr, pointsColor, cameraMatrixIr, distortionIr, cameraMatrixColor, distortionColor, sizeColor,
                                 rotation, translation, essential, fundamental, cv::CALIB_FIX_INTRINSIC, termCriteria);
 #endif
@@ -1091,7 +1091,7 @@ class DepthCalibration
     //cv::solvePnP(board, points[index], cameraMatrix, distortion, rvec, translation, false, cv::EPNP);
 #if CV_MAJOR_VERSION == 2
     cv::solvePnPRansac(board, points[index], cameraMatrix, distortion, rvec, translation, false, 300, 0.05, board.size(), cv::noArray(), cv::ITERATIVE);
-#elif CV_MAJOR_VERSION == 3
+#elif CV_MAJOR_VERSION == 3 || CV_MAJOR_VERSION == 4 
     cv::solvePnPRansac(board, points[index], cameraMatrix, distortion, rvec, translation, false, 300, 0.05, 0.99, cv::noArray(), cv::SOLVEPNP_ITERATIVE);
 #endif
     cv::Rodrigues(rvec, rotation);
@@ -1240,7 +1240,6 @@ int main(int argc, char **argv)
   std::string path = "./";
 
   ros::init(argc, argv, "kinect2_calib", ros::init_options::AnonymousName);
-
   if(!ros::ok())
   {
     return 0;
@@ -1349,6 +1348,8 @@ int main(int argc, char **argv)
   std::string topicColor = "/" + ns + K2_TOPIC_HD + K2_TOPIC_IMAGE_MONO;
   std::string topicIr = "/" + ns + K2_TOPIC_SD + K2_TOPIC_IMAGE_IR;
   std::string topicDepth = "/" + ns + K2_TOPIC_SD + K2_TOPIC_IMAGE_DEPTH;
+
+  OUT_INFO("CV_MAJOR_VERSION : " << CV_MAJOR_VERSION << std::endl);
   OUT_INFO("Start settings:" << std::endl
            << "       Mode: " FG_CYAN << (mode == RECORD ? "record" : "calibrate") << NO_COLOR << std::endl
            << "     Source: " FG_CYAN << (calibDepth ? "depth" : (source == COLOR ? "color" : (source == IR ? "ir" : "sync"))) << NO_COLOR << std::endl