From 1a3a198f55ded7986e1f532943d2465a311424bd Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 18 Jun 2018 17:35:26 +0200 Subject: [PATCH 01/33] launch and cfg files for RealSense and px4 --- cfg/imgui.ini | 40 +++++ cfg/imu.yaml | 10 ++ cfg/rovio.info | 68 ++++----- cfg/rovio_custom.info_new | 212 +++++++++++++++++++++++++++ cfg/rs_color_radtan.yaml | 14 ++ cfg/rs_color_radtan_intel.yaml | 13 ++ launch/rovio_mavros-realsense.launch | 7 + launch/rovio_node.launch | 7 +- 8 files changed, 335 insertions(+), 36 deletions(-) create mode 100644 cfg/imgui.ini create mode 100644 cfg/imu.yaml create mode 100644 cfg/rovio_custom.info_new create mode 100644 cfg/rs_color_radtan.yaml create mode 100644 cfg/rs_color_radtan_intel.yaml create mode 100644 launch/rovio_mavros-realsense.launch diff --git a/cfg/imgui.ini b/cfg/imgui.ini new file mode 100644 index 00000000..4599d85a --- /dev/null +++ b/cfg/imgui.ini @@ -0,0 +1,40 @@ +[Debug] +Pos=60,60 +Size=400,400 +Collapsed=0 + +[Splash Screen Banner] +Pos=746,574 +Size=1792,1008 +Collapsed=0 + +[nostreaming_popup] +Pos=916,454 +Size=1452,50 +Collapsed=0 + +[Notification parent window] +Pos=60,60 +Size=32,32 +Collapsed=0 + +[Stream of 0] +Pos=357,53 +Size=690,32 +Collapsed=0 + +[Stream of 1] +Pos=357,477 +Size=690,32 +Collapsed=0 + +[numbered_ruler] +Pos=1018,103 +Size=32,358 +Collapsed=0 + +[Stream of 3] +Pos=1083,53 +Size=690,32 +Collapsed=0 + diff --git a/cfg/imu.yaml b/cfg/imu.yaml new file mode 100644 index 00000000..2cfed98f --- /dev/null +++ b/cfg/imu.yaml @@ -0,0 +1,10 @@ +#Accelerometers +accelerometer_noise_density: 1.86e-03 #Noise density (continuous-time) +accelerometer_random_walk: 4.33e-04 #Bias random walk + +#Gyroscopes +gyroscope_noise_density: 1.87e-04 #Noise density (continuous-time) +gyroscope_random_walk: 2.66e-05 #Bias random walk + +rostopic: /mavros/imu/data #the IMU ROS topic +update_rate: 200.0 #Hz (for discretization of the values above) \ No newline at end of file diff --git a/cfg/rovio.info b/cfg/rovio.info index f237fe5d..ec11a378 100644 --- a/cfg/rovio.info +++ b/cfg/rovio.info @@ -8,14 +8,14 @@ Common } Camera0 { - CalibrationFile ; Camera-Calibration file for intrinsics - qCM_x 0.00666398307551; X-entry of IMU to Camera quaterion (Hamilton) - qCM_y -0.0079168224269; Y-entry of IMU to Camera quaterion (Hamilton) - qCM_z -0.701985972528; Z-entry of IMU to Camera quaterion (Hamilton) - qCM_w 0.712115587266; W-entry of IMU to Camera quaterion (Hamilton) - MrMC_x -0.0111674199187; X-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_y -0.0574640920022; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_z 0.0207586947896; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x 0.498529507038; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.496012173235; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z 0.511762063883; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.493497562897; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x 0.000218693879012; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y -0.00011610537335; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.000499672135095; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] } Camera1 { @@ -146,31 +146,33 @@ Prediction { PredictionNoise { - pos_0 1e-4; X-covariance parameter of position prediction [m^2/s] - pos_1 1e-4; Y-covariance parameter of position prediction [m^2/s] - pos_2 1e-4; Z-covariance parameter of position prediction [m^2/s] - vel_0 4e-6; X-covariance parameter of velocity prediction [m^2/s^3] - vel_1 4e-6; Y-covariance parameter of velocity prediction [m^2/s^3] - vel_2 4e-6; Z-covariance parameter of velocity prediction [m^2/s^3] - acb_0 1e-8; X-covariance parameter of accelerometer bias prediction [m^2/s^5] - acb_1 1e-8; Y-covariance parameter of accelerometer bias prediction [m^2/s^5] - acb_2 1e-8; Z-covariance parameter of accelerometer bias prediction [m^2/s^5] - gyb_0 3.8e-7; X-covariance parameter of gyroscope bias prediction [rad^2/s^3] - gyb_1 3.8e-7; Y-covariance parameter of gyroscope bias prediction [rad^2/s^3] - gyb_2 3.8e-7; Z-covariance parameter of gyroscope bias prediction [rad^2/s^3] - vep 1e-8; Covariance parameter of linear extrinsics prediction [m^2/s] - att_0 7.6e-7; X-Covariance parameter of attitude prediction [rad^2/s] - att_1 7.6e-7; Y-Covariance parameter of attitude prediction [rad^2/s] - att_2 7.6e-7; Z-Covariance parameter of attitude prediction [rad^2/s] - vea 1e-8; Covariance parameter of rotational extrinsics prediction [rad^2/s] - dep 0.0001; Covariance parameter of distance prediction [m^2/s] - nor 0.00001; Covariance parameter of bearing vector prediction [rad^2/s] + pos_0 1e-4; X-covariance parameter of position prediction [m^2/s] + pos_1 1e-4; Y-covariance parameter of position prediction [m^2/s] + pos_2 1e-4; Z-covariance parameter of position prediction [m^2/s] + vel_0 4e-5; 4e-6 X-covariance parameter of velocity prediction [m^2/s^3] + vel_1 4e-5; Y-covariance parameter of velocity prediction [m^2/s^3] + vel_2 4e-5; Z-covariance parameter of velocity prediction [m^2/s^3] + acb_0 9e-6; X-covariance parameter of accelerometer bias prediction [m^2/s^5] + acb_1 9e-6; Y-covariance parameter of accelerometer bias prediction [m^2/s^5] + acb_2 9e-6; Z-covariance parameter of accelerometer bias prediction [m^2/s^5] + gyb_0 2e-8; X-covariance parameter of gyroscope bias prediction [rad^2/s^3] + gyb_1 2e-8; Y-covariance parameter of gyroscope bias prediction [rad^2/s^3] + gyb_2 2e-8; Z-covariance parameter of gyroscope bias prediction [rad^2/s^3] + vep 1e-6; Covariance parameter of linear extrinsics prediction [m^2/s] + att_0 7.6e-6; 7.6e-7 X-Covariance parameter of attitude prediction [rad^2/s] + att_1 7.6e-6; Y-Covariance parameter of attitude prediction [rad^2/s] + att_2 7.6e-6; Z-Covariance parameter of attitude prediction [rad^2/s] + vea 1e-6; Covariance parameter of rotational extrinsics prediction [rad^2/s] + dep 0.0001; Covariance parameter of distance prediction [m^2/s] + nor 0.00001; Covariance parameter of bearing vector prediction [rad^2/s] + + } MotionDetection { - inertialMotionRorTh 0.1; Treshold on rotational rate for motion detection [rad/s] - inertialMotionAccTh 0.5; Treshold on acceleration for motion detection [m/s^2] - } + inertialMotionRorTh 0.1; Treshold on rotational rate for motion detection [rad/s] + inertialMotionAccTh 0.5; Treshold on acceleration for motion detection [m/s^2] + } } PoseUpdate { @@ -202,14 +204,14 @@ PoseUpdate qVM_x 0; X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) qVM_y 0; Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) qVM_z 0; Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) - qVM_w 1; W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_w -1; W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) MrMV_x 0; X-entry of vector representing IMU to reference body coordinate frame of pose measurement MrMV_y 0; Y-entry of vector representing IMU to reference body coordinate frame of pose measurement MrMV_z 0; Z-entry of vector representing IMU to reference body coordinate frame of pose measurement qWI_x 0; X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) qWI_y 0; Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) qWI_z 0; Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) - qWI_w 1; W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_w -1; W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) IrIW_x 0; X-entry of vector representing World to reference inertial coordinate frame of pose measurement IrIW_y 0; Y-entry of vector representing World to reference inertial coordinate frame of pose measurement IrIW_z 0; Z-entry of vector representing World to reference inertial coordinate frame of pose measurement @@ -227,4 +229,4 @@ VelocityUpdate qAM_y 0 qAM_z 0 qAM_w 1 -} +} \ No newline at end of file diff --git a/cfg/rovio_custom.info_new b/cfg/rovio_custom.info_new new file mode 100644 index 00000000..31023599 --- /dev/null +++ b/cfg/rovio_custom.info_new @@ -0,0 +1,212 @@ +Common +{ + doVECalibration true + verbose false + depthType 1 +} +PoseUpdate +{ + doVisualization true +} +Init +{ + Covariance + { + pos_0 1 + pos_1 1 + pos_2 1 + vel_0 1 + vel_1 1 + vel_2 1 + acb_0 1 + acb_1 1 + acb_2 1 + gyb_0 1 + gyb_1 1 + gyb_2 1 + att_0 1 + att_1 1 + att_2 1 + vep 1 + vea 1 + } + State + { + att_x 0 + att_y 0 + att_z 0 + att_w 1 + gyb_0 0 + gyb_1 0 + gyb_2 0 + acb_0 0 + acb_1 0 + acb_2 0 + vel_0 0 + vel_1 0 + vel_2 0 + pos_0 0 + pos_1 0 + pos_2 0 + } +} +Camera0 +{ + qCM_x 0 + qCM_y 0 + qCM_z 0 + qCM_w 1 + MrMC_x 0 + MrMC_y 0 + MrMC_z 0 + CalibrationFile "" +} +VelocityUpdate +{ + UpdateNoise + { + vel_0 0.0001 + vel_1 0.0001 + vel_2 0.0001 + } + MahalanobisTh0 7.689997599999999 + qAM_x 0 + qAM_y 0 + qAM_z 0 + qAM_w 1 +} +PoseUpdate +{ + enablePosition true + enableAttitude true + noFeedbackToRovio true + doInertialAlignmentAtStart true + useOdometryCov false + UpdateNoise + { + pos_0 0 + pos_1 0 + pos_2 0 + att_0 0 + att_1 0 + att_2 0 + } + MahalanobisTh0 12.6511204 + qVM_x 0 + qVM_y 0 + qVM_z 0 + qVM_w 1 + MrMV_x 0 + MrMV_y 0 + MrMV_z 0 + qWI_x 0 + qWI_y 0 + qWI_z 0 + qWI_w 1 + IrIW_x 0 + IrIW_y 0 + IrIW_z 0 + timeOffset 0 +} +ImgUpdate +{ + doPatchWarping false + MotionDetection + { + isEnabled false + minFeatureCountForNoMotionDetection 5 + rateOfMovingFeaturesTh 0.5 + pixelCoordinateMotionTh 1 + } + useDirectMethod true + doFrameVisualisation true + visualizePatches false + removeNegativeFeatureAfterUpdate true + useCrossCameraMeasurements true + doStereoInitialization true + useIntensityOffsetForAlignment true + useIntensitySqewForAlignment true + statLocalQualityRange 10 + statLocalVisibilityRange 100 + statMinGlobalQualityRange 100 + maxNumIteration 10 + startLevel 3 + endLevel 1 + nDetectionBuckets 100 + fastDetectionThreshold 10 + alignMaxUniSample 5 + updateVecNormTermination 9.9999999999999995e-07 + MahalanobisTh 5.8858356000000001 + initCovFeature_0 1 + initCovFeature_1 1 + initCovFeature_2 1 + initDepth 0.5 + startDetectionTh 0.90000000000000002 + scoreDetectionExponent 0.5 + penaltyDistance 20 + zeroDistancePenalty 100 + trackingUpperBound 0.90000000000000002 + trackingLowerBound 0.10000000000000001 + minTrackedAndFreeFeatures 0.5 + minRelativeSTScore 0.20000000000000001 + minAbsoluteSTScore 0.20000000000000001 + minTimeBetweenPatchUpdate 1 + removalFactor 1.1000000000000001 + patchRejectionTh 10 + maxUncertaintyToDepthRatioForDepthInitialization 0.29999999999999999 + UpdateNoise + { + pix 2 + int 10000 + } + noiseGainForOffCamera 4 + alignConvergencePixelRange 1 + alignCoverageRatio 2 + alignmentHuberNormThreshold 0 + alignmentGaussianWeightingSigma 2 + alignmentGradientExponent 0 + discriminativeSamplingDistance 0 + discriminativeSamplingGain 0 + ZeroVelocityUpdate + { + isEnabled false + UpdateNoise + { + vel_0 0.0001 + vel_1 0.0001 + vel_2 0.0001 + } + minNoMotionTime 1 + MahalanobisTh0 7.689997599999999 + } +} +Prediction +{ + PredictionNoise + { + pos_0 0.0001 + pos_1 0.0001 + pos_2 0.0001 + vel_0 0.0001 + vel_1 0.0001 + vel_2 0.0001 + acb_0 0.0001 + acb_1 0.0001 + acb_2 0.0001 + gyb_0 0.0001 + gyb_1 0.0001 + gyb_2 0.0001 + att_0 0.0001 + att_1 0.0001 + att_2 0.0001 + vep 0.0001 + vea 0.0001 + nor 0.0001 + dep 0.0001 + } + MotionDetection + { + inertialMotionRorTh 0.10000000000000001 + inertialMotionAccTh 0.10000000000000001 + } +} diff --git a/cfg/rs_color_radtan.yaml b/cfg/rs_color_radtan.yaml new file mode 100644 index 00000000..7eaaa1f0 --- /dev/null +++ b/cfg/rs_color_radtan.yaml @@ -0,0 +1,14 @@ +###### Camera Calibration File For Cam 0 of Euroc Datasets ###### +image_width: 640 +image_height: 480 +camera_name: camera ## RealSense D415 rgb module +camera_matrix: + rows: 3 + cols: 3 + data: [602.6682976548108, 0.0 , 323.09018740329157, 0.0, 603.0133127786867, 228.72219305730522, 0.0, 0.0, 1.0] +distortion_model: equidistant +distortion_coefficients: + rows: 1 + cols: 4 + data: [0.1043930758106389, -0.2434771945266034, -0.0013695830857340307, + 0.0017089625472838964] \ No newline at end of file diff --git a/cfg/rs_color_radtan_intel.yaml b/cfg/rs_color_radtan_intel.yaml new file mode 100644 index 00000000..ab05e856 --- /dev/null +++ b/cfg/rs_color_radtan_intel.yaml @@ -0,0 +1,13 @@ +###### Camera Calibration File For Cam 0 of Euroc Datasets ###### +image_width: 640 +image_height: 480 +camera_name: camera ## RealSense D415 rgb module +camera_matrix: + rows: 3 + cols: 3 + data: [616.1016235351562, 0.0 , 319.61151123046875, 0.0, 615.8966064453125, 232.68994140625, 0.0, 0.0, 1.0] +distortion_model: equidistant +distortion_coefficients: + rows: 1 + cols: 4 + data: [0, 0, 0, 0] \ No newline at end of file diff --git a/launch/rovio_mavros-realsense.launch b/launch/rovio_mavros-realsense.launch new file mode 100644 index 00000000..5a0d05e8 --- /dev/null +++ b/launch/rovio_mavros-realsense.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/launch/rovio_node.launch b/launch/rovio_node.launch index c4bdd653..8c4bff05 100644 --- a/launch/rovio_node.launch +++ b/launch/rovio_node.launch @@ -1,8 +1,9 @@ - - - + + + + From 4073537770932f095df2543af94e7a2e821af17b Mon Sep 17 00:00:00 2001 From: trleonie Date: Wed, 20 Jun 2018 17:52:49 +0200 Subject: [PATCH 02/33] calibration file for RealSense D435 --- cfg/rs_color_D435_intel.yaml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 cfg/rs_color_D435_intel.yaml diff --git a/cfg/rs_color_D435_intel.yaml b/cfg/rs_color_D435_intel.yaml new file mode 100644 index 00000000..46a1769c --- /dev/null +++ b/cfg/rs_color_D435_intel.yaml @@ -0,0 +1,13 @@ +###### Camera Calibration File For the RealSense D435 given by Intel ###### +image_width: 640 +image_height: 480 +camera_name: camera ## RealSense D435 rgb module +camera_matrix: + rows: 3 + cols: 3 + data: [618.293701171875, 0.0 , 323.7475280761719, 0.0, 618.570556640625, 239.60833740234375, 0.0, 0.0, 1.0] +distortion_model: equidistant +distortion_coefficients: + rows: 1 + cols: 4 + data: [0, 0, 0, 0] \ No newline at end of file From b5fe5bdbdbfe4f87017b0f4740f176e780529692 Mon Sep 17 00:00:00 2001 From: trleonie Date: Thu, 21 Jun 2018 11:25:44 +0200 Subject: [PATCH 03/33] launch cleanup --- launch/rovio_mavros-realsense.launch | 7 ------- launch/rovio_mavros_rs.launch | 21 +++++++++++++++++++++ launch/rovio_node.launch | 15 +++++++++++---- 3 files changed, 32 insertions(+), 11 deletions(-) delete mode 100644 launch/rovio_mavros-realsense.launch create mode 100644 launch/rovio_mavros_rs.launch diff --git a/launch/rovio_mavros-realsense.launch b/launch/rovio_mavros-realsense.launch deleted file mode 100644 index 5a0d05e8..00000000 --- a/launch/rovio_mavros-realsense.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/launch/rovio_mavros_rs.launch b/launch/rovio_mavros_rs.launch new file mode 100644 index 00000000..1f802d87 --- /dev/null +++ b/launch/rovio_mavros_rs.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/rovio_node.launch b/launch/rovio_node.launch index 8c4bff05..65cdde85 100644 --- a/launch/rovio_node.launch +++ b/launch/rovio_node.launch @@ -1,9 +1,16 @@ + + + + + + - - - - + + + + + From d5146713a777e90b29ec05697b5d6f588f37da44 Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 25 Jun 2018 10:02:38 +0200 Subject: [PATCH 04/33] config file for downward IMU-cam configuration --- launch/rovio_mavros_rs.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/rovio_mavros_rs.launch b/launch/rovio_mavros_rs.launch index 1f802d87..94e7d89d 100644 --- a/launch/rovio_mavros_rs.launch +++ b/launch/rovio_mavros_rs.launch @@ -1,7 +1,7 @@ - + From 863a40a28630ff65b198ed20a309b5b026c2c95b Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 25 Jun 2018 10:04:07 +0200 Subject: [PATCH 05/33] info file for downward configuration --- cfg/rovio_down.info | 232 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 232 insertions(+) create mode 100644 cfg/rovio_down.info diff --git a/cfg/rovio_down.info b/cfg/rovio_down.info new file mode 100644 index 00000000..c97b141d --- /dev/null +++ b/cfg/rovio_down.info @@ -0,0 +1,232 @@ +; You can partially override parameter set in this file by creating your own subset of parameter in a separate info-file and include it with: +; #include "/home/user/workspace/rovio/cfg/rovio_custom.info" +Common +{ + doVECalibration true; Should the camera-IMU extrinsics be calibrated online + depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic) + verbose false; Is the verbose active +} +Camera0 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x 0.708709126204; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.704791804906; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z 0.00387651358845; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.031382460299; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x -0.054286735468; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0755568214257; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z 0.0951657050294; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} +Camera1 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x 0.00151329637706; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.0123329249764; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z -0.702657352863; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.711419885414; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x -0.0091929160801; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0540646643676; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z 0.0190387660614; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} +Init +{ + State + { + pos_0 0; X-entry of initial position (world to IMU in world) [m] + pos_1 0; Y-entry of initial position (world to IMU in world) [m] + pos_2 0; Z-entry of initial position (world to IMU in world) [m] + vel_0 0; X-entry of initial velocity (robocentric, IMU) [m/s] + vel_1 0; Y-entry of initial velocity (robocentric, IMU) [m/s] + vel_2 0; Z-entry of initial velocity (robocentric, IMU) [m/s] + acb_0 0; X-entry of accelerometer bias [m/s^2] + acb_1 0; Y-entry of accelerometer bias [m/s^2] + acb_2 0; Z-entry of accelerometer bias [m/s^2] + gyb_0 0; X-entry of gyroscope bias [rad/s] + gyb_1 0; Y-entry of gyroscope bias [rad/s] + gyb_2 0; Z-entry of gyroscope bias [rad/s] + att_x 0; X-entry of initial attitude (IMU to world, Hamilton) + att_y 0; Y-entry of initial attitude (IMU to world, Hamilton) + att_z 0; Z-entry of initial attitude (IMU to world, Hamilton) + att_w 1; W-entry of initial attitude (IMU to world, Hamilton) + } + Covariance + { + pos_0 0.0001; X-Covariance of initial position [m^2] + pos_1 0.0001; Y-Covariance of initial position [m^2] + pos_2 0.0001; Z-Covariance of initial position [m^2] + vel_0 1.0; X-Covariance of initial velocity [m^2/s^2] + vel_1 1.0; Y-Covariance of initial velocity [m^2/s^2] + vel_2 1.0; Z-Covariance of initial velocity [m^2/s^2] + acb_0 4e-4; X-Covariance of initial accelerometer bias [m^2/s^4] + acb_1 4e-4; Y-Covariance of initial accelerometer bias [m^2/s^4] + acb_2 4e-4; Z-Covariance of initial accelerometer bias [m^2/s^4] + gyb_0 3e-4; X-Covariance of initial gyroscope bias [rad^2/s^2] + gyb_1 3e-4; Y-Covariance of initial gyroscope bias [rad^2/s^2] + gyb_2 3e-4; Z-Covariance of initial gyroscope bias [rad^2/s^2] + vep 0.0001; Covariance of initial linear camera-IMU extrinsics, same for all entries [m^2] + att_0 0.1; X-Covariance of initial attitude [rad^2] + att_1 0.1; Y-Covariance of initial attitude [rad^2] + att_2 0.1; Z-Covariance of initial attitude [rad^2] + vea 0.01; Covariance of initial rotational camera-IMU extrinsics, same for all entries [rad^2] + } +} +ImgUpdate +{ + updateVecNormTermination 1e-4; + maxNumIteration 20; + doPatchWarping true; Should the patches be warped + doFrameVisualisation true; Should the frames be visualized + visualizePatches false; Should the patches be visualized + useDirectMethod true; Should the EKF-innovation be based on direct intensity error (o.w. reprojection error) + startLevel 2; Highest patch level which is being employed (must be smaller than the hardcoded template parameter) + endLevel 1; Lowest patch level which is being employed + nDetectionBuckets 100; Number of discretization buckets used during the candidates selection + MahalanobisTh 9.21; Mahalanobis treshold for the update, 5.8858356 + UpdateNoise + { + pix 2; Covariance used for the reprojection error, 1/focal_length is roughly 1 pixel std [rad] (~1/f ~ 1/400^2 = 1/160000) + int 400; Covariance used for the photometric error [intensity^2] + } + initCovFeature_0 0.5; Covariance for the initial distance (Relative to initialization depth [m^2/m^2]) + initCovFeature_1 1e-5; Covariance for the initial bearing vector, x-component [rad^2] + initCovFeature_2 1e-5; Covariance for the initial bearing vector, y-component [rad^2] + initDepth 0.5; Initial value for the initial distance parameter + startDetectionTh 0.8; Threshold for detecting new features (min: 0, max: 1) + scoreDetectionExponent 0.25; Exponent used for weighting the distance between candidates + penaltyDistance 100; Maximal distance which gets penalized during bucketing [pix] + zeroDistancePenalty 100; Penalty for zero distance (max: nDetectionBuckets) + statLocalQualityRange 10; Number of frames for local quality evaluation + statLocalVisibilityRange 100; Number of frames for local visibility evaluation + statMinGlobalQualityRange 100; Minimum number of frames for obtaining maximal global quality + trackingUpperBound 0.9; Threshold for local quality for min overall global quality + trackingLowerBound 0.8; Threshold for local quality for max overall global quality + minTrackedAndFreeFeatures 0.75; Minimum of amount of feature which are either tracked or free + removalFactor 1.1; Factor for enforcing feature removal if not enough free + minRelativeSTScore 0.75; Minimum relative ST-score for extracting new feature patch + minAbsoluteSTScore 5.0; Minimum absolute ST-score for extracting new feature patch + minTimeBetweenPatchUpdate 1.0; Minimum time between new multilevel patch extrection [s] + fastDetectionThreshold 5; Fast corner detector treshold while adding new feature + alignConvergencePixelRange 10; Assumed convergence range for image alignment (gets scaled with the level) [pixels] + alignCoverageRatio 2; How many sigma of the uncertainty should be covered in the adaptive alignement + alignMaxUniSample 1; Maximal number of alignment seeds on one side -> total number of sample = 2n+1. Carefull can get very costly if diverging! + patchRejectionTh 50.0; If the average itensity error is larger than this the feauture is rejected [intensity], if smaller 0 the no check is performed + alignmentHuberNormThreshold 10; Intensity error threshold for Huber norm (enabled if > 0) + alignmentGaussianWeightingSigma -1; Width of Gaussian which is used for pixel error weighting (enabled if > 0) + alignmentGradientExponent 0.0; Exponent used for gradient based weighting of residuals + useIntensityOffsetForAlignment true; Should an intensity offset between the patches be considered + useIntensitySqewForAlignment true; Should an intensity sqew between the patches be considered + removeNegativeFeatureAfterUpdate true; Should feature with negative distance get removed + maxUncertaintyToDepthRatioForDepthInitialization 0.3; If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame + useCrossCameraMeasurements true; Should cross measurements between frame be used. Might be turned of in calibration phase. + doStereoInitialization true; Should a stereo match be used for feature initialization. + noiseGainForOffCamera 10.0; Factor added on update noise if not main camera + discriminativeSamplingDistance 0.02; Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed). + discriminativeSamplingGain 1.1; Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used). + MotionDetection + { + isEnabled 0; Is the motion detection enabled + rateOfMovingFeaturesTh 0.5; Amount of feature with motion for overall motion detection + pixelCoordinateMotionTh 1.0; Threshold for motion detection for patched [pixels] + minFeatureCountForNoMotionDetection 5; Min feature count in frame for motion detection + } + ZeroVelocityUpdate + { + UpdateNoise + { + vel_0 0.01; X-Covariance of zero velocity update [m^2/s^2] + vel_1 0.01; Y-Covariance of zero velocity update [m^2/s^2] + vel_2 0.01; Z-Covariance of zero velocity update [m^2/s^2] + } + MahalanobisTh0 7.689997599999999; Mahalanobid distance for zero velocity updates + minNoMotionTime 1.0; Min duration of no-motion + isEnabled 0; Should zero velocity update be applied, only works if MotionDetection.isEnabled is true + } +} +Prediction +{ + PredictionNoise + { + pos_0 1e-4; X-covariance parameter of position prediction [m^2/s] + pos_1 1e-4; Y-covariance parameter of position prediction [m^2/s] + pos_2 1e-4; Z-covariance parameter of position prediction [m^2/s] + vel_0 4e-5; 4e-6 X-covariance parameter of velocity prediction [m^2/s^3] + vel_1 4e-5; Y-covariance parameter of velocity prediction [m^2/s^3] + vel_2 4e-5; Z-covariance parameter of velocity prediction [m^2/s^3] + acb_0 9e-6; X-covariance parameter of accelerometer bias prediction [m^2/s^5] + acb_1 9e-6; Y-covariance parameter of accelerometer bias prediction [m^2/s^5] + acb_2 9e-6; Z-covariance parameter of accelerometer bias prediction [m^2/s^5] + gyb_0 2e-8; X-covariance parameter of gyroscope bias prediction [rad^2/s^3] + gyb_1 2e-8; Y-covariance parameter of gyroscope bias prediction [rad^2/s^3] + gyb_2 2e-8; Z-covariance parameter of gyroscope bias prediction [rad^2/s^3] + vep 1e-6; Covariance parameter of linear extrinsics prediction [m^2/s] + att_0 7.6e-6; 7.6e-7 X-Covariance parameter of attitude prediction [rad^2/s] + att_1 7.6e-6; Y-Covariance parameter of attitude prediction [rad^2/s] + att_2 7.6e-6; Z-Covariance parameter of attitude prediction [rad^2/s] + vea 1e-6; Covariance parameter of rotational extrinsics prediction [rad^2/s] + dep 0.0001; Covariance parameter of distance prediction [m^2/s] + nor 0.00001; Covariance parameter of bearing vector prediction [rad^2/s] + + + } + MotionDetection + { + inertialMotionRorTh 0.1; Treshold on rotational rate for motion detection [rad/s] + inertialMotionAccTh 0.5; Treshold on acceleration for motion detection [m/s^2] + } +} +PoseUpdate +{ + UpdateNoise + { + pos_0 0.01; X-Covariance of linear pose measurement update [m^2] + pos_1 0.01; Y-Covariance of linear pose measurement update [m^2] + pos_2 0.01; Z-Covariance of linear pose measurement update [m^2] + att_0 0.01; X-Covariance of rotational pose measurement update [rad^2] + att_1 0.01; Y-Covariance of rotational pose measurement update [rad^2] + att_2 0.01; Z-Covariance of rotational pose measurement update [rad^2] + } + init_cov_IrIW 1; Covariance of initial pose between inertial frames, linear part [m^2] + init_cov_qWI 1; Covariance of initial pose between inertial frames, rotational part [rad^2] + init_cov_MrMV 1; Covariance of initial pose between inertial frames, linear part [m^2] + init_cov_qVM 1; Covariance of initial pose between inertial frames, rotational part [rad^2] + pre_cov_IrIW 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s] + pre_cov_qWI 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s] + pre_cov_MrMV 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s] + pre_cov_qVM 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s] + MahalanobisTh0 12.6511204; Mahalonobis distance treshold of pose measurement + doVisualization false; Should the measured poses be vizualized + enablePosition true; Should the linear part be used during the update + enableAttitude true; Should the rotation part be used during the update (e.g. set to fals eif feeding GPS-measurement) + noFeedbackToRovio true; By default the pose update is only used for aligning coordinate frame. Set to false if ROVIO should benefit frome the posed measurements. + doInertialAlignmentAtStart true; Should the transformation between I and W be explicitly computed and set with the first pose measurement. + timeOffset 0.0; Time offset added to the pose measurement timestamps + useOdometryCov false; Should the UpdateNoise position covariance be scaled using the covariance in the Odometry message + qVM_x 0; X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_y 0; Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_z 0; Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_w -1; W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + MrMV_x 0; X-entry of vector representing IMU to reference body coordinate frame of pose measurement + MrMV_y 0; Y-entry of vector representing IMU to reference body coordinate frame of pose measurement + MrMV_z 0; Z-entry of vector representing IMU to reference body coordinate frame of pose measurement + qWI_x 0; X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_y 0; Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_z 0; Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_w -1; W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + IrIW_x 0; X-entry of vector representing World to reference inertial coordinate frame of pose measurement + IrIW_y 0; Y-entry of vector representing World to reference inertial coordinate frame of pose measurement + IrIW_z 0; Z-entry of vector representing World to reference inertial coordinate frame of pose measurement +} +VelocityUpdate +{ + UpdateNoise + { + vel_0 0.0001 + vel_1 0.0001 + vel_2 0.0001 + } + MahalanobisTh0 7.689997599999999 + qAM_x 0 + qAM_y 0 + qAM_z 0 + qAM_w 1 +} \ No newline at end of file From 8b191a4bb3c1a73728230fdebb52b6ea1519ed36 Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 25 Jun 2018 10:08:47 +0200 Subject: [PATCH 06/33] name cleanup --- cfg/rs_color_D415_intel.yaml | 13 +++++++++++++ cfg/rs_color_D415_kalibr.yaml | 14 ++++++++++++++ 2 files changed, 27 insertions(+) create mode 100644 cfg/rs_color_D415_intel.yaml create mode 100644 cfg/rs_color_D415_kalibr.yaml diff --git a/cfg/rs_color_D415_intel.yaml b/cfg/rs_color_D415_intel.yaml new file mode 100644 index 00000000..ab05e856 --- /dev/null +++ b/cfg/rs_color_D415_intel.yaml @@ -0,0 +1,13 @@ +###### Camera Calibration File For Cam 0 of Euroc Datasets ###### +image_width: 640 +image_height: 480 +camera_name: camera ## RealSense D415 rgb module +camera_matrix: + rows: 3 + cols: 3 + data: [616.1016235351562, 0.0 , 319.61151123046875, 0.0, 615.8966064453125, 232.68994140625, 0.0, 0.0, 1.0] +distortion_model: equidistant +distortion_coefficients: + rows: 1 + cols: 4 + data: [0, 0, 0, 0] \ No newline at end of file diff --git a/cfg/rs_color_D415_kalibr.yaml b/cfg/rs_color_D415_kalibr.yaml new file mode 100644 index 00000000..7eaaa1f0 --- /dev/null +++ b/cfg/rs_color_D415_kalibr.yaml @@ -0,0 +1,14 @@ +###### Camera Calibration File For Cam 0 of Euroc Datasets ###### +image_width: 640 +image_height: 480 +camera_name: camera ## RealSense D415 rgb module +camera_matrix: + rows: 3 + cols: 3 + data: [602.6682976548108, 0.0 , 323.09018740329157, 0.0, 603.0133127786867, 228.72219305730522, 0.0, 0.0, 1.0] +distortion_model: equidistant +distortion_coefficients: + rows: 1 + cols: 4 + data: [0.1043930758106389, -0.2434771945266034, -0.0013695830857340307, + 0.0017089625472838964] \ No newline at end of file From 747350a6ed7fec4eec121b64e44b972ffb1acf64 Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 25 Jun 2018 10:11:16 +0200 Subject: [PATCH 07/33] cfg folder cleanup --- cfg/rovio_custom.info_new | 212 --------------------------------- cfg/rs_color_radtan.yaml | 14 --- cfg/rs_color_radtan_intel.yaml | 13 -- 3 files changed, 239 deletions(-) delete mode 100644 cfg/rovio_custom.info_new delete mode 100644 cfg/rs_color_radtan.yaml delete mode 100644 cfg/rs_color_radtan_intel.yaml diff --git a/cfg/rovio_custom.info_new b/cfg/rovio_custom.info_new deleted file mode 100644 index 31023599..00000000 --- a/cfg/rovio_custom.info_new +++ /dev/null @@ -1,212 +0,0 @@ -Common -{ - doVECalibration true - verbose false - depthType 1 -} -PoseUpdate -{ - doVisualization true -} -Init -{ - Covariance - { - pos_0 1 - pos_1 1 - pos_2 1 - vel_0 1 - vel_1 1 - vel_2 1 - acb_0 1 - acb_1 1 - acb_2 1 - gyb_0 1 - gyb_1 1 - gyb_2 1 - att_0 1 - att_1 1 - att_2 1 - vep 1 - vea 1 - } - State - { - att_x 0 - att_y 0 - att_z 0 - att_w 1 - gyb_0 0 - gyb_1 0 - gyb_2 0 - acb_0 0 - acb_1 0 - acb_2 0 - vel_0 0 - vel_1 0 - vel_2 0 - pos_0 0 - pos_1 0 - pos_2 0 - } -} -Camera0 -{ - qCM_x 0 - qCM_y 0 - qCM_z 0 - qCM_w 1 - MrMC_x 0 - MrMC_y 0 - MrMC_z 0 - CalibrationFile "" -} -VelocityUpdate -{ - UpdateNoise - { - vel_0 0.0001 - vel_1 0.0001 - vel_2 0.0001 - } - MahalanobisTh0 7.689997599999999 - qAM_x 0 - qAM_y 0 - qAM_z 0 - qAM_w 1 -} -PoseUpdate -{ - enablePosition true - enableAttitude true - noFeedbackToRovio true - doInertialAlignmentAtStart true - useOdometryCov false - UpdateNoise - { - pos_0 0 - pos_1 0 - pos_2 0 - att_0 0 - att_1 0 - att_2 0 - } - MahalanobisTh0 12.6511204 - qVM_x 0 - qVM_y 0 - qVM_z 0 - qVM_w 1 - MrMV_x 0 - MrMV_y 0 - MrMV_z 0 - qWI_x 0 - qWI_y 0 - qWI_z 0 - qWI_w 1 - IrIW_x 0 - IrIW_y 0 - IrIW_z 0 - timeOffset 0 -} -ImgUpdate -{ - doPatchWarping false - MotionDetection - { - isEnabled false - minFeatureCountForNoMotionDetection 5 - rateOfMovingFeaturesTh 0.5 - pixelCoordinateMotionTh 1 - } - useDirectMethod true - doFrameVisualisation true - visualizePatches false - removeNegativeFeatureAfterUpdate true - useCrossCameraMeasurements true - doStereoInitialization true - useIntensityOffsetForAlignment true - useIntensitySqewForAlignment true - statLocalQualityRange 10 - statLocalVisibilityRange 100 - statMinGlobalQualityRange 100 - maxNumIteration 10 - startLevel 3 - endLevel 1 - nDetectionBuckets 100 - fastDetectionThreshold 10 - alignMaxUniSample 5 - updateVecNormTermination 9.9999999999999995e-07 - MahalanobisTh 5.8858356000000001 - initCovFeature_0 1 - initCovFeature_1 1 - initCovFeature_2 1 - initDepth 0.5 - startDetectionTh 0.90000000000000002 - scoreDetectionExponent 0.5 - penaltyDistance 20 - zeroDistancePenalty 100 - trackingUpperBound 0.90000000000000002 - trackingLowerBound 0.10000000000000001 - minTrackedAndFreeFeatures 0.5 - minRelativeSTScore 0.20000000000000001 - minAbsoluteSTScore 0.20000000000000001 - minTimeBetweenPatchUpdate 1 - removalFactor 1.1000000000000001 - patchRejectionTh 10 - maxUncertaintyToDepthRatioForDepthInitialization 0.29999999999999999 - UpdateNoise - { - pix 2 - int 10000 - } - noiseGainForOffCamera 4 - alignConvergencePixelRange 1 - alignCoverageRatio 2 - alignmentHuberNormThreshold 0 - alignmentGaussianWeightingSigma 2 - alignmentGradientExponent 0 - discriminativeSamplingDistance 0 - discriminativeSamplingGain 0 - ZeroVelocityUpdate - { - isEnabled false - UpdateNoise - { - vel_0 0.0001 - vel_1 0.0001 - vel_2 0.0001 - } - minNoMotionTime 1 - MahalanobisTh0 7.689997599999999 - } -} -Prediction -{ - PredictionNoise - { - pos_0 0.0001 - pos_1 0.0001 - pos_2 0.0001 - vel_0 0.0001 - vel_1 0.0001 - vel_2 0.0001 - acb_0 0.0001 - acb_1 0.0001 - acb_2 0.0001 - gyb_0 0.0001 - gyb_1 0.0001 - gyb_2 0.0001 - att_0 0.0001 - att_1 0.0001 - att_2 0.0001 - vep 0.0001 - vea 0.0001 - nor 0.0001 - dep 0.0001 - } - MotionDetection - { - inertialMotionRorTh 0.10000000000000001 - inertialMotionAccTh 0.10000000000000001 - } -} diff --git a/cfg/rs_color_radtan.yaml b/cfg/rs_color_radtan.yaml deleted file mode 100644 index 7eaaa1f0..00000000 --- a/cfg/rs_color_radtan.yaml +++ /dev/null @@ -1,14 +0,0 @@ -###### Camera Calibration File For Cam 0 of Euroc Datasets ###### -image_width: 640 -image_height: 480 -camera_name: camera ## RealSense D415 rgb module -camera_matrix: - rows: 3 - cols: 3 - data: [602.6682976548108, 0.0 , 323.09018740329157, 0.0, 603.0133127786867, 228.72219305730522, 0.0, 0.0, 1.0] -distortion_model: equidistant -distortion_coefficients: - rows: 1 - cols: 4 - data: [0.1043930758106389, -0.2434771945266034, -0.0013695830857340307, - 0.0017089625472838964] \ No newline at end of file diff --git a/cfg/rs_color_radtan_intel.yaml b/cfg/rs_color_radtan_intel.yaml deleted file mode 100644 index ab05e856..00000000 --- a/cfg/rs_color_radtan_intel.yaml +++ /dev/null @@ -1,13 +0,0 @@ -###### Camera Calibration File For Cam 0 of Euroc Datasets ###### -image_width: 640 -image_height: 480 -camera_name: camera ## RealSense D415 rgb module -camera_matrix: - rows: 3 - cols: 3 - data: [616.1016235351562, 0.0 , 319.61151123046875, 0.0, 615.8966064453125, 232.68994140625, 0.0, 0.0, 1.0] -distortion_model: equidistant -distortion_coefficients: - rows: 1 - cols: 4 - data: [0, 0, 0, 0] \ No newline at end of file From b5f7c6a6151c258616be56714d12166e835425eb Mon Sep 17 00:00:00 2001 From: trleonie Date: Thu, 28 Jun 2018 14:06:23 +0200 Subject: [PATCH 08/33] setup for M600 --- launch/rovio_mavros_rs.launch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/rovio_mavros_rs.launch b/launch/rovio_mavros_rs.launch index 94e7d89d..bc8988e6 100644 --- a/launch/rovio_mavros_rs.launch +++ b/launch/rovio_mavros_rs.launch @@ -1,14 +1,14 @@ - + - + - + x From 19d148cb3dac96ed80da94641e25849fbac9cdab Mon Sep 17 00:00:00 2001 From: trleonie Date: Thu, 28 Jun 2018 14:08:20 +0200 Subject: [PATCH 09/33] M600 calibration files --- cfg/M600-rs_D435_down_intel.yaml | 13 ++ cfg/rovio_M600.info | 232 +++++++++++++++++++++++++++++++ 2 files changed, 245 insertions(+) create mode 100644 cfg/M600-rs_D435_down_intel.yaml create mode 100644 cfg/rovio_M600.info diff --git a/cfg/M600-rs_D435_down_intel.yaml b/cfg/M600-rs_D435_down_intel.yaml new file mode 100644 index 00000000..c0160be8 --- /dev/null +++ b/cfg/M600-rs_D435_down_intel.yaml @@ -0,0 +1,13 @@ +image_width: 640 +image_height: 480 +camera_name: camr +camera_matrix: + rows: 3 + cols: 3 + data: [618.293701172, 0.0, 323.747528076, 0.0, 618.570556641, 239.608337402, 0.0, 0.0, 1.0] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0.0, 0.0, 0.0, 0.0, 0.0] + diff --git a/cfg/rovio_M600.info b/cfg/rovio_M600.info new file mode 100644 index 00000000..4f7220f1 --- /dev/null +++ b/cfg/rovio_M600.info @@ -0,0 +1,232 @@ +; You can partially override parameter set in this file by creating your own subset of parameter in a separate info-file and include it with: +; #include "/home/user/workspace/rovio/cfg/rovio_custom.info" +Common +{ + doVECalibration true; Should the camera-IMU extrinsics be calibrated online + depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic) + verbose false; Is the verbose active +} +Camera0 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x -0.712595441024; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y 0.701298215193; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z 0.0196987073794; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.000715350070233; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x 0.135011088808; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0301838079241; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.149016900074; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} +Camera1 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x 0.00151329637706; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.0123329249764; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z -0.702657352863; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.711419885414; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x -0.0091929160801; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0540646643676; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z 0.0190387660614; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} +Init +{ + State + { + pos_0 0; X-entry of initial position (world to IMU in world) [m] + pos_1 0; Y-entry of initial position (world to IMU in world) [m] + pos_2 0; Z-entry of initial position (world to IMU in world) [m] + vel_0 0; X-entry of initial velocity (robocentric, IMU) [m/s] + vel_1 0; Y-entry of initial velocity (robocentric, IMU) [m/s] + vel_2 0; Z-entry of initial velocity (robocentric, IMU) [m/s] + acb_0 0; X-entry of accelerometer bias [m/s^2] + acb_1 0; Y-entry of accelerometer bias [m/s^2] + acb_2 0; Z-entry of accelerometer bias [m/s^2] + gyb_0 0; X-entry of gyroscope bias [rad/s] + gyb_1 0; Y-entry of gyroscope bias [rad/s] + gyb_2 0; Z-entry of gyroscope bias [rad/s] + att_x 0; X-entry of initial attitude (IMU to world, Hamilton) + att_y 0; Y-entry of initial attitude (IMU to world, Hamilton) + att_z 0; Z-entry of initial attitude (IMU to world, Hamilton) + att_w 1; W-entry of initial attitude (IMU to world, Hamilton) + } + Covariance + { + pos_0 0.0001; X-Covariance of initial position [m^2] + pos_1 0.0001; Y-Covariance of initial position [m^2] + pos_2 0.0001; Z-Covariance of initial position [m^2] + vel_0 1.0; X-Covariance of initial velocity [m^2/s^2] + vel_1 1.0; Y-Covariance of initial velocity [m^2/s^2] + vel_2 1.0; Z-Covariance of initial velocity [m^2/s^2] + acb_0 4e-4; X-Covariance of initial accelerometer bias [m^2/s^4] + acb_1 4e-4; Y-Covariance of initial accelerometer bias [m^2/s^4] + acb_2 4e-4; Z-Covariance of initial accelerometer bias [m^2/s^4] + gyb_0 3e-4; X-Covariance of initial gyroscope bias [rad^2/s^2] + gyb_1 3e-4; Y-Covariance of initial gyroscope bias [rad^2/s^2] + gyb_2 3e-4; Z-Covariance of initial gyroscope bias [rad^2/s^2] + vep 0.0001; Covariance of initial linear camera-IMU extrinsics, same for all entries [m^2] + att_0 0.1; X-Covariance of initial attitude [rad^2] + att_1 0.1; Y-Covariance of initial attitude [rad^2] + att_2 0.1; Z-Covariance of initial attitude [rad^2] + vea 0.01; Covariance of initial rotational camera-IMU extrinsics, same for all entries [rad^2] + } +} +ImgUpdate +{ + updateVecNormTermination 1e-4; + maxNumIteration 20; + doPatchWarping true; Should the patches be warped + doFrameVisualisation true; Should the frames be visualized + visualizePatches false; Should the patches be visualized + useDirectMethod true; Should the EKF-innovation be based on direct intensity error (o.w. reprojection error) + startLevel 2; Highest patch level which is being employed (must be smaller than the hardcoded template parameter) + endLevel 1; Lowest patch level which is being employed + nDetectionBuckets 100; Number of discretization buckets used during the candidates selection + MahalanobisTh 9.21; Mahalanobis treshold for the update, 5.8858356 + UpdateNoise + { + pix 2; Covariance used for the reprojection error, 1/focal_length is roughly 1 pixel std [rad] (~1/f ~ 1/400^2 = 1/160000) + int 400; Covariance used for the photometric error [intensity^2] + } + initCovFeature_0 0.5; Covariance for the initial distance (Relative to initialization depth [m^2/m^2]) + initCovFeature_1 1e-5; Covariance for the initial bearing vector, x-component [rad^2] + initCovFeature_2 1e-5; Covariance for the initial bearing vector, y-component [rad^2] + initDepth 0.5; Initial value for the initial distance parameter + startDetectionTh 0.8; Threshold for detecting new features (min: 0, max: 1) + scoreDetectionExponent 0.25; Exponent used for weighting the distance between candidates + penaltyDistance 100; Maximal distance which gets penalized during bucketing [pix] + zeroDistancePenalty 100; Penalty for zero distance (max: nDetectionBuckets) + statLocalQualityRange 10; Number of frames for local quality evaluation + statLocalVisibilityRange 100; Number of frames for local visibility evaluation + statMinGlobalQualityRange 100; Minimum number of frames for obtaining maximal global quality + trackingUpperBound 0.9; Threshold for local quality for min overall global quality + trackingLowerBound 0.8; Threshold for local quality for max overall global quality + minTrackedAndFreeFeatures 0.75; Minimum of amount of feature which are either tracked or free + removalFactor 1.1; Factor for enforcing feature removal if not enough free + minRelativeSTScore 0.75; Minimum relative ST-score for extracting new feature patch + minAbsoluteSTScore 5.0; Minimum absolute ST-score for extracting new feature patch + minTimeBetweenPatchUpdate 1.0; Minimum time between new multilevel patch extrection [s] + fastDetectionThreshold 5; Fast corner detector treshold while adding new feature + alignConvergencePixelRange 10; Assumed convergence range for image alignment (gets scaled with the level) [pixels] + alignCoverageRatio 2; How many sigma of the uncertainty should be covered in the adaptive alignement + alignMaxUniSample 1; Maximal number of alignment seeds on one side -> total number of sample = 2n+1. Carefull can get very costly if diverging! + patchRejectionTh 50.0; If the average itensity error is larger than this the feauture is rejected [intensity], if smaller 0 the no check is performed + alignmentHuberNormThreshold 10; Intensity error threshold for Huber norm (enabled if > 0) + alignmentGaussianWeightingSigma -1; Width of Gaussian which is used for pixel error weighting (enabled if > 0) + alignmentGradientExponent 0.0; Exponent used for gradient based weighting of residuals + useIntensityOffsetForAlignment true; Should an intensity offset between the patches be considered + useIntensitySqewForAlignment true; Should an intensity sqew between the patches be considered + removeNegativeFeatureAfterUpdate true; Should feature with negative distance get removed + maxUncertaintyToDepthRatioForDepthInitialization 0.3; If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame + useCrossCameraMeasurements true; Should cross measurements between frame be used. Might be turned of in calibration phase. + doStereoInitialization true; Should a stereo match be used for feature initialization. + noiseGainForOffCamera 10.0; Factor added on update noise if not main camera + discriminativeSamplingDistance 0.02; Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed). + discriminativeSamplingGain 1.1; Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used). + MotionDetection + { + isEnabled 0; Is the motion detection enabled + rateOfMovingFeaturesTh 0.5; Amount of feature with motion for overall motion detection + pixelCoordinateMotionTh 1.0; Threshold for motion detection for patched [pixels] + minFeatureCountForNoMotionDetection 5; Min feature count in frame for motion detection + } + ZeroVelocityUpdate + { + UpdateNoise + { + vel_0 0.01; X-Covariance of zero velocity update [m^2/s^2] + vel_1 0.01; Y-Covariance of zero velocity update [m^2/s^2] + vel_2 0.01; Z-Covariance of zero velocity update [m^2/s^2] + } + MahalanobisTh0 7.689997599999999; Mahalanobid distance for zero velocity updates + minNoMotionTime 1.0; Min duration of no-motion + isEnabled 0; Should zero velocity update be applied, only works if MotionDetection.isEnabled is true + } +} +Prediction +{ + PredictionNoise + { + pos_0 1e-4; X-covariance parameter of position prediction [m^2/s] + pos_1 1e-4; Y-covariance parameter of position prediction [m^2/s] + pos_2 1e-4; Z-covariance parameter of position prediction [m^2/s] + vel_0 4e-5; 4e-6 X-covariance parameter of velocity prediction [m^2/s^3] + vel_1 4e-5; Y-covariance parameter of velocity prediction [m^2/s^3] + vel_2 4e-5; Z-covariance parameter of velocity prediction [m^2/s^3] + acb_0 9e-6; X-covariance parameter of accelerometer bias prediction [m^2/s^5] + acb_1 9e-6; Y-covariance parameter of accelerometer bias prediction [m^2/s^5] + acb_2 9e-6; Z-covariance parameter of accelerometer bias prediction [m^2/s^5] + gyb_0 2e-8; X-covariance parameter of gyroscope bias prediction [rad^2/s^3] + gyb_1 2e-8; Y-covariance parameter of gyroscope bias prediction [rad^2/s^3] + gyb_2 2e-8; Z-covariance parameter of gyroscope bias prediction [rad^2/s^3] + vep 1e-6; Covariance parameter of linear extrinsics prediction [m^2/s] + att_0 7.6e-6; 7.6e-7 X-Covariance parameter of attitude prediction [rad^2/s] + att_1 7.6e-6; Y-Covariance parameter of attitude prediction [rad^2/s] + att_2 7.6e-6; Z-Covariance parameter of attitude prediction [rad^2/s] + vea 1e-6; Covariance parameter of rotational extrinsics prediction [rad^2/s] + dep 0.0001; Covariance parameter of distance prediction [m^2/s] + nor 0.00001; Covariance parameter of bearing vector prediction [rad^2/s] + + + } + MotionDetection + { + inertialMotionRorTh 0.1; Treshold on rotational rate for motion detection [rad/s] + inertialMotionAccTh 0.5; Treshold on acceleration for motion detection [m/s^2] + } +} +PoseUpdate +{ + UpdateNoise + { + pos_0 0.01; X-Covariance of linear pose measurement update [m^2] + pos_1 0.01; Y-Covariance of linear pose measurement update [m^2] + pos_2 0.01; Z-Covariance of linear pose measurement update [m^2] + att_0 0.01; X-Covariance of rotational pose measurement update [rad^2] + att_1 0.01; Y-Covariance of rotational pose measurement update [rad^2] + att_2 0.01; Z-Covariance of rotational pose measurement update [rad^2] + } + init_cov_IrIW 1; Covariance of initial pose between inertial frames, linear part [m^2] + init_cov_qWI 1; Covariance of initial pose between inertial frames, rotational part [rad^2] + init_cov_MrMV 1; Covariance of initial pose between inertial frames, linear part [m^2] + init_cov_qVM 1; Covariance of initial pose between inertial frames, rotational part [rad^2] + pre_cov_IrIW 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s] + pre_cov_qWI 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s] + pre_cov_MrMV 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s] + pre_cov_qVM 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s] + MahalanobisTh0 12.6511204; Mahalonobis distance treshold of pose measurement + doVisualization false; Should the measured poses be vizualized + enablePosition true; Should the linear part be used during the update + enableAttitude true; Should the rotation part be used during the update (e.g. set to fals eif feeding GPS-measurement) + noFeedbackToRovio true; By default the pose update is only used for aligning coordinate frame. Set to false if ROVIO should benefit frome the posed measurements. + doInertialAlignmentAtStart true; Should the transformation between I and W be explicitly computed and set with the first pose measurement. + timeOffset 0.0; Time offset added to the pose measurement timestamps + useOdometryCov false; Should the UpdateNoise position covariance be scaled using the covariance in the Odometry message + qVM_x 0; X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_y 0; Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_z 0; Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_w -1; W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + MrMV_x 0; X-entry of vector representing IMU to reference body coordinate frame of pose measurement + MrMV_y 0; Y-entry of vector representing IMU to reference body coordinate frame of pose measurement + MrMV_z 0; Z-entry of vector representing IMU to reference body coordinate frame of pose measurement + qWI_x 0; X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_y 0; Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_z 0; Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_w -1; W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + IrIW_x 0; X-entry of vector representing World to reference inertial coordinate frame of pose measurement + IrIW_y 0; Y-entry of vector representing World to reference inertial coordinate frame of pose measurement + IrIW_z 0; Z-entry of vector representing World to reference inertial coordinate frame of pose measurement +} +VelocityUpdate +{ + UpdateNoise + { + vel_0 0.0001 + vel_1 0.0001 + vel_2 0.0001 + } + MahalanobisTh0 7.689997599999999 + qAM_x 0 + qAM_y 0 + qAM_z 0 + qAM_w 1 +} \ No newline at end of file From e343156beb447f119ca3012fc946ce7e507d00c4 Mon Sep 17 00:00:00 2001 From: trleonie Date: Tue, 23 Oct 2018 09:13:14 +0200 Subject: [PATCH 10/33] update for domi sensor --- cfg/calibration.yaml | 72 ++++++++++ cfg/domi_cam0.yaml | 13 ++ cfg/domi_cam1.yaml | 13 ++ cfg/domi_cam2.yaml | 13 ++ cfg/domi_cam3.yaml | 13 ++ cfg/domi_cam4.yaml | 13 ++ cfg/euroc_cam0.yaml | 23 ---- cfg/euroc_cam0_new.yaml | 13 ++ cfg/rovio_domi.info | 230 +++++++++++++++++++++++++++++++ cfg/rovio_domi.info_new | 234 ++++++++++++++++++++++++++++++++ cfg/rovio_orig.info | 230 +++++++++++++++++++++++++++++++ launch/rovio_M600.launch | 18 +++ launch/rovio_domi.launch | 17 +++ launch/rovio_node.launch | 2 +- launch/rovio_node_orig.launch | 8 ++ launch/rovio_rosbag_node.launch | 4 +- 16 files changed, 890 insertions(+), 26 deletions(-) create mode 100644 cfg/calibration.yaml create mode 100644 cfg/domi_cam0.yaml create mode 100644 cfg/domi_cam1.yaml create mode 100644 cfg/domi_cam2.yaml create mode 100644 cfg/domi_cam3.yaml create mode 100644 cfg/domi_cam4.yaml create mode 100644 cfg/euroc_cam0_new.yaml create mode 100644 cfg/rovio_domi.info create mode 100644 cfg/rovio_domi.info_new create mode 100644 cfg/rovio_orig.info create mode 100644 launch/rovio_M600.launch create mode 100644 launch/rovio_domi.launch create mode 100644 launch/rovio_node_orig.launch diff --git a/cfg/calibration.yaml b/cfg/calibration.yaml new file mode 100644 index 00000000..074d2148 --- /dev/null +++ b/cfg/calibration.yaml @@ -0,0 +1,72 @@ +cam0: + T_cam_imu: + - [-0.08408111890742742, -0.9960984788103229, -0.02679899167946961, 0.04479369286657575] + - [0.9963498922995513, -0.0844393301122407, 0.012525639499406719, 0.017429964015559494] + - [-0.014739659356583449, -0.025648002689434925, 0.9995623654380427, -0.008515976150586862] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [] + camera_model: pinhole + distortion_coeffs: [-0.05242378016082595, -0.005452250979470613, -0.002750824731857844, + 0.001176879627814984] + distortion_model: equidistant + intrinsics: [393.01620151949186, 392.35754010306675, 360.133022189131, 239.82750356152488] + resolution: [752, 480] + rostopic: /uvc_camera/cam_0/image_raw +cam1: + T_cam_imu: + - [0.9996318198325032, -0.008930485458513018, -0.025621694085903285, -0.03360089532815607] + - [0.025343025375257167, -0.030032942880376667, 0.9992275783858109, 0.030203489020421376] + - [-0.009693082233500086, -0.9995090138520102, -0.029795559826848173, -0.04864993999452485] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [] + camera_model: pinhole + distortion_coeffs: [-0.05775660779258553, -0.016321098687079805, 0.010851033712369944, + -0.003727782103089848] + distortion_model: equidistant + intrinsics: [399.3480570111828, 398.3491316285275, 386.46344514949317, 249.82118871166142] + resolution: [752, 480] + rostopic: /uvc_camera/cam_1/image_raw +cam2: + T_cam_imu: + - [-0.014335233107286627, -0.9998565560354433, 0.009020445926071874, 0.045133886593555224] + - [0.0014261615362032298, -0.009041809365503639, -0.9999581049957411, -0.008250822703834645] + - [0.9998962281932238, -0.014321767919615314, 0.0015555734069045424, -0.026038027580066486] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [] + camera_model: pinhole + distortion_coeffs: [-0.06456227781767035, -0.013988324416174668, 0.016058120497332448, + -0.007586776661876683] + distortion_model: equidistant + intrinsics: [401.6472159834467, 400.20768312959996, 364.5763065095566, 237.33091443088992] + resolution: [752, 480] + rostopic: /uvc_camera/cam_2/image_raw +cam3: + T_cam_imu: + - [-0.005018093185921224, 0.9992645940012366, 0.03801433829911055, -0.04238657606191231] + - [0.004042825346933565, 0.03803477912356312, -0.9992682378321819, -0.0075742965840363006] + - [-0.9999792369364424, -0.00486073580475066, -0.0042307142948694865, -0.06461910888733008] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [] + camera_model: pinhole + distortion_coeffs: [-0.05606253878969147, -0.014037695708003596, 0.006384058502673369, + -0.001519392296560513] + distortion_model: equidistant + intrinsics: [398.9944782000244, 398.0805787425024, 364.4159902096, 275.47777743169365] + resolution: [752, 480] + rostopic: /uvc_camera/cam_3/image_raw +cam4: + T_cam_imu: + - [-0.9997974682265937, 0.017982575329980252, 0.00903601196300318, 0.03350202332038546] + - [0.008860893124254645, -0.009794219283195392, 0.9999127751167467, 0.02522916124620373] + - [0.01806950748455773, 0.9997903281454241, 0.009632893964497026, -0.04929900189157067] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [] + camera_model: pinhole + distortion_coeffs: [-0.058690582862360736, -0.004194002012911134, -0.0014205680884277923, + 0.00032045972552648583] + distortion_model: equidistant + intrinsics: [400.1030703837698, 399.06815731220064, 371.4006422531301, 248.04717800715062] + resolution: [752, 480] + rostopic: /uvc_camera/cam_4/image_raw + + diff --git a/cfg/domi_cam0.yaml b/cfg/domi_cam0.yaml new file mode 100644 index 00000000..d2698318 --- /dev/null +++ b/cfg/domi_cam0.yaml @@ -0,0 +1,13 @@ +image_width: 752 +image_height: 480 +camera_name: cam0 +camera_matrix: + rows: 3 + cols: 3 + data: [393.016201519, 0.0, 360.133022189, 0.0, 392.357540103, 239.827503562, 0.0, 0.0, 1.0] +distortion_model: equidistant +distortion_coefficients: + rows: 1 + cols: 4 + data: [-0.0524237801608, -0.00545225097947, -0.00275082473186, 0.00117687962781] + diff --git a/cfg/domi_cam1.yaml b/cfg/domi_cam1.yaml new file mode 100644 index 00000000..8be9b5cb --- /dev/null +++ b/cfg/domi_cam1.yaml @@ -0,0 +1,13 @@ +image_width: 752 +image_height: 480 +camera_name: cam1 +camera_matrix: + rows: 3 + cols: 3 + data: [399.348057011, 0.0, 386.463445149, 0.0, 398.349131629, 249.821188712, 0.0, 0.0, 1.0] +distortion_model: equidistant +distortion_coefficients: + rows: 1 + cols: 4 + data: [-0.0577566077926, -0.0163210986871, 0.0108510337124, -0.00372778210309] + diff --git a/cfg/domi_cam2.yaml b/cfg/domi_cam2.yaml new file mode 100644 index 00000000..f5d5f166 --- /dev/null +++ b/cfg/domi_cam2.yaml @@ -0,0 +1,13 @@ +image_width: 752 +image_height: 480 +camera_name: cam2 +camera_matrix: + rows: 3 + cols: 3 + data: [401.647215983, 0.0, 364.57630651, 0.0, 400.20768313, 237.330914431, 0.0, 0.0, 1.0] +distortion_model: equidistant +distortion_coefficients: + rows: 1 + cols: 4 + data: [-0.0645622778177, -0.0139883244162, 0.0160581204973, -0.00758677666188] + diff --git a/cfg/domi_cam3.yaml b/cfg/domi_cam3.yaml new file mode 100644 index 00000000..104dfaa6 --- /dev/null +++ b/cfg/domi_cam3.yaml @@ -0,0 +1,13 @@ +image_width: 752 +image_height: 480 +camera_name: cam3 +camera_matrix: + rows: 3 + cols: 3 + data: [398.9944782, 0.0, 364.41599021, 0.0, 398.080578743, 275.477777432, 0.0, 0.0, 1.0] +distortion_model: equidistant +distortion_coefficients: + rows: 1 + cols: 4 + data: [-0.0560625387897, -0.014037695708, 0.00638405850267, -0.00151939229656] + diff --git a/cfg/domi_cam4.yaml b/cfg/domi_cam4.yaml new file mode 100644 index 00000000..a084e054 --- /dev/null +++ b/cfg/domi_cam4.yaml @@ -0,0 +1,13 @@ +image_width: 752 +image_height: 480 +camera_name: cam4 +camera_matrix: + rows: 3 + cols: 3 + data: [400.103070384, 0.0, 371.400642253, 0.0, 399.068157312, 248.047178007, 0.0, 0.0, 1.0] +distortion_model: equidistant +distortion_coefficients: + rows: 1 + cols: 4 + data: [-0.0586905828624, -0.00419400201291, -0.00142056808843, 0.000320459725526] + diff --git a/cfg/euroc_cam0.yaml b/cfg/euroc_cam0.yaml index 193db731..a29033fa 100644 --- a/cfg/euroc_cam0.yaml +++ b/cfg/euroc_cam0.yaml @@ -12,26 +12,3 @@ distortion_coefficients: cols: 5 data: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] -###### Original Calibration File ###### -## General sensor definitions. -#sensor_type: camera -#comment: VI-Sensor cam0 (MT9M034) -# -## Sensor extrinsics wrt. the body-frame. -#T_BS: -# cols: 4 -# rows: 4 -# data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, -# 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, -# -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, -# 0.0, 0.0, 0.0, 1.0] -# -## Camera specific definitions. -#rate_hz: 20 -#resolution: [752, 480] -#camera_model: pinhole -#intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv -#distortion_model: radial-tangential -#distortion_coefficients: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] - - diff --git a/cfg/euroc_cam0_new.yaml b/cfg/euroc_cam0_new.yaml new file mode 100644 index 00000000..5eff8c8f --- /dev/null +++ b/cfg/euroc_cam0_new.yaml @@ -0,0 +1,13 @@ +###### Camera Calibration File For Cam 0 of Euroc Datasets ###### +image_width: 752 +image_height: 480 +camera_name: cam0 +camera_matrix: + rows: 3 + cols: 3 + data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 +data: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] diff --git a/cfg/rovio_domi.info b/cfg/rovio_domi.info new file mode 100644 index 00000000..2b98b1b8 --- /dev/null +++ b/cfg/rovio_domi.info @@ -0,0 +1,230 @@ +; You can partially override parameter set in this file by creating your own subset of parameter in a separate info-file and include it with: +; #include "/home/user/workspace/rovio/cfg/rovio_custom.info" +Common +{ + doVECalibration true; Should the camera-IMU extrinsics be calibrated online + depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic) + verbose false; Is the verbose active +} +Camera0 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x -0.0141053692505; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.00445598914788; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z 0.736220553648; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.676579987219; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x -0.013725561541; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0458722860308; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z 0.00949435362177; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} +Camera1 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x 0.498284808832; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.500933590992; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z 0.506194778687; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.494514542489; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x 0.0266940973582; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0446798994558; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.00861710075576; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} +Init +{ + State + { + pos_0 0; X-entry of initial position (world to IMU in world) [m] + pos_1 0; Y-entry of initial position (world to IMU in world) [m] + pos_2 0; Z-entry of initial position (world to IMU in world) [m] + vel_0 0; X-entry of initial velocity (robocentric, IMU) [m/s] + vel_1 0; Y-entry of initial velocity (robocentric, IMU) [m/s] + vel_2 0; Z-entry of initial velocity (robocentric, IMU) [m/s] + acb_0 0; X-entry of accelerometer bias [m/s^2] + acb_1 0; Y-entry of accelerometer bias [m/s^2] + acb_2 0; Z-entry of accelerometer bias [m/s^2] + gyb_0 0; X-entry of gyroscope bias [rad/s] + gyb_1 0; Y-entry of gyroscope bias [rad/s] + gyb_2 0; Z-entry of gyroscope bias [rad/s] + att_x 0; X-entry of initial attitude (IMU to world, Hamilton) + att_y 0; Y-entry of initial attitude (IMU to world, Hamilton) + att_z 0; Z-entry of initial attitude (IMU to world, Hamilton) + att_w 1; W-entry of initial attitude (IMU to world, Hamilton) + } + Covariance + { + pos_0 0.0001; X-Covariance of initial position [m^2] + pos_1 0.0001; Y-Covariance of initial position [m^2] + pos_2 0.0001; Z-Covariance of initial position [m^2] + vel_0 1.0; X-Covariance of initial velocity [m^2/s^2] + vel_1 1.0; Y-Covariance of initial velocity [m^2/s^2] + vel_2 1.0; Z-Covariance of initial velocity [m^2/s^2] + acb_0 4e-4; X-Covariance of initial accelerometer bias [m^2/s^4] + acb_1 4e-4; Y-Covariance of initial accelerometer bias [m^2/s^4] + acb_2 4e-4; Z-Covariance of initial accelerometer bias [m^2/s^4] + gyb_0 3e-4; X-Covariance of initial gyroscope bias [rad^2/s^2] + gyb_1 3e-4; Y-Covariance of initial gyroscope bias [rad^2/s^2] + gyb_2 3e-4; Z-Covariance of initial gyroscope bias [rad^2/s^2] + vep 0.0001; Covariance of initial linear camera-IMU extrinsics, same for all entries [m^2] + att_0 0.1; X-Covariance of initial attitude [rad^2] + att_1 0.1; Y-Covariance of initial attitude [rad^2] + att_2 0.1; Z-Covariance of initial attitude [rad^2] + vea 0.01; Covariance of initial rotational camera-IMU extrinsics, same for all entries [rad^2] + } +} +ImgUpdate +{ + updateVecNormTermination 1e-4; + maxNumIteration 20; + doPatchWarping true; Should the patches be warped + doFrameVisualisation true; Should the frames be visualized + visualizePatches false; Should the patches be visualized + useDirectMethod true; Should the EKF-innovation be based on direct intensity error (o.w. reprojection error) + startLevel 2; Highest patch level which is being employed (must be smaller than the hardcoded template parameter) + endLevel 1; Lowest patch level which is being employed + nDetectionBuckets 100; Number of discretization buckets used during the candidates selection + MahalanobisTh 9.21; Mahalanobis treshold for the update, 5.8858356 + UpdateNoise + { + pix 2; Covariance used for the reprojection error, 1/focal_length is roughly 1 pixel std [rad] (~1/f ~ 1/400^2 = 1/160000) + int 400; Covariance used for the photometric error [intensity^2] + } + initCovFeature_0 0.5; Covariance for the initial distance (Relative to initialization depth [m^2/m^2]) + initCovFeature_1 1e-5; Covariance for the initial bearing vector, x-component [rad^2] + initCovFeature_2 1e-5; Covariance for the initial bearing vector, y-component [rad^2] + initDepth 0.5; Initial value for the initial distance parameter + startDetectionTh 0.8; Threshold for detecting new features (min: 0, max: 1) + scoreDetectionExponent 0.25; Exponent used for weighting the distance between candidates + penaltyDistance 100; Maximal distance which gets penalized during bucketing [pix] + zeroDistancePenalty 100; Penalty for zero distance (max: nDetectionBuckets) + statLocalQualityRange 10; Number of frames for local quality evaluation + statLocalVisibilityRange 100; Number of frames for local visibility evaluation + statMinGlobalQualityRange 100; Minimum number of frames for obtaining maximal global quality + trackingUpperBound 0.9; Threshold for local quality for min overall global quality + trackingLowerBound 0.8; Threshold for local quality for max overall global quality + minTrackedAndFreeFeatures 0.75; Minimum of amount of feature which are either tracked or free + removalFactor 1.1; Factor for enforcing feature removal if not enough free + minRelativeSTScore 0.75; Minimum relative ST-score for extracting new feature patch + minAbsoluteSTScore 5.0; Minimum absolute ST-score for extracting new feature patch + minTimeBetweenPatchUpdate 1.0; Minimum time between new multilevel patch extrection [s] + fastDetectionThreshold 5; Fast corner detector treshold while adding new feature + alignConvergencePixelRange 10; Assumed convergence range for image alignment (gets scaled with the level) [pixels] + alignCoverageRatio 2; How many sigma of the uncertainty should be covered in the adaptive alignement + alignMaxUniSample 1; Maximal number of alignment seeds on one side -> total number of sample = 2n+1. Carefull can get very costly if diverging! + patchRejectionTh 50.0; If the average itensity error is larger than this the feauture is rejected [intensity], if smaller 0 the no check is performed + alignmentHuberNormThreshold 10; Intensity error threshold for Huber norm (enabled if > 0) + alignmentGaussianWeightingSigma -1; Width of Gaussian which is used for pixel error weighting (enabled if > 0) + alignmentGradientExponent 0.0; Exponent used for gradient based weighting of residuals + useIntensityOffsetForAlignment true; Should an intensity offset between the patches be considered + useIntensitySqewForAlignment true; Should an intensity sqew between the patches be considered + removeNegativeFeatureAfterUpdate true; Should feature with negative distance get removed + maxUncertaintyToDepthRatioForDepthInitialization 0.3; If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame + useCrossCameraMeasurements false; Should cross measurements between frame be used. Might be turned of in calibration phase. + doStereoInitialization true; Should a stereo match be used for feature initialization. + noiseGainForOffCamera 10.0; Factor added on update noise if not main camera + discriminativeSamplingDistance 0.02; Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed). + discriminativeSamplingGain 1.1; Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used). + MotionDetection + { + isEnabled 0; Is the motion detection enabled + rateOfMovingFeaturesTh 0.5; Amount of feature with motion for overall motion detection + pixelCoordinateMotionTh 1.0; Threshold for motion detection for patched [pixels] + minFeatureCountForNoMotionDetection 5; Min feature count in frame for motion detection + } + ZeroVelocityUpdate + { + UpdateNoise + { + vel_0 0.01; X-Covariance of zero velocity update [m^2/s^2] + vel_1 0.01; Y-Covariance of zero velocity update [m^2/s^2] + vel_2 0.01; Z-Covariance of zero velocity update [m^2/s^2] + } + MahalanobisTh0 7.689997599999999; Mahalanobid distance for zero velocity updates + minNoMotionTime 1.0; Min duration of no-motion + isEnabled 0; Should zero velocity update be applied, only works if MotionDetection.isEnabled is true + } +} +Prediction +{ + PredictionNoise + { + pos_0 1e-4; X-covariance parameter of position prediction [m^2/s] + pos_1 1e-4; Y-covariance parameter of position prediction [m^2/s] + pos_2 1e-4; Z-covariance parameter of position prediction [m^2/s] + vel_0 4e-6; X-covariance parameter of velocity prediction [m^2/s^3] + vel_1 4e-6; Y-covariance parameter of velocity prediction [m^2/s^3] + vel_2 4e-6; Z-covariance parameter of velocity prediction [m^2/s^3] + acb_0 1e-8; X-covariance parameter of accelerometer bias prediction [m^2/s^5] + acb_1 1e-8; Y-covariance parameter of accelerometer bias prediction [m^2/s^5] + acb_2 1e-8; Z-covariance parameter of accelerometer bias prediction [m^2/s^5] + gyb_0 3.8e-7; X-covariance parameter of gyroscope bias prediction [rad^2/s^3] + gyb_1 3.8e-7; Y-covariance parameter of gyroscope bias prediction [rad^2/s^3] + gyb_2 3.8e-7; Z-covariance parameter of gyroscope bias prediction [rad^2/s^3] + vep 1e-8; Covariance parameter of linear extrinsics prediction [m^2/s] + att_0 7.6e-7; X-Covariance parameter of attitude prediction [rad^2/s] + att_1 7.6e-7; Y-Covariance parameter of attitude prediction [rad^2/s] + att_2 7.6e-7; Z-Covariance parameter of attitude prediction [rad^2/s] + vea 1e-8; Covariance parameter of rotational extrinsics prediction [rad^2/s] + dep 0.0001; Covariance parameter of distance prediction [m^2/s] + nor 0.00001; Covariance parameter of bearing vector prediction [rad^2/s] + } + MotionDetection + { + inertialMotionRorTh 0.1; Treshold on rotational rate for motion detection [rad/s] + inertialMotionAccTh 0.5; Treshold on acceleration for motion detection [m/s^2] + } +} +PoseUpdate +{ + UpdateNoise + { + pos_0 0.01; X-Covariance of linear pose measurement update [m^2] + pos_1 0.01; Y-Covariance of linear pose measurement update [m^2] + pos_2 0.01; Z-Covariance of linear pose measurement update [m^2] + att_0 0.01; X-Covariance of rotational pose measurement update [rad^2] + att_1 0.01; Y-Covariance of rotational pose measurement update [rad^2] + att_2 0.01; Z-Covariance of rotational pose measurement update [rad^2] + } + init_cov_IrIW 1; Covariance of initial pose between inertial frames, linear part [m^2] + init_cov_qWI 1; Covariance of initial pose between inertial frames, rotational part [rad^2] + init_cov_MrMV 1; Covariance of initial pose between inertial frames, linear part [m^2] + init_cov_qVM 1; Covariance of initial pose between inertial frames, rotational part [rad^2] + pre_cov_IrIW 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s] + pre_cov_qWI 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s] + pre_cov_MrMV 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s] + pre_cov_qVM 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s] + MahalanobisTh0 12.6511204; Mahalonobis distance treshold of pose measurement + doVisualization false; Should the measured poses be vizualized + enablePosition true; Should the linear part be used during the update + enableAttitude true; Should the rotation part be used during the update (e.g. set to fals eif feeding GPS-measurement) + noFeedbackToRovio true; By default the pose update is only used for aligning coordinate frame. Set to false if ROVIO should benefit frome the posed measurements. + doInertialAlignmentAtStart true; Should the transformation between I and W be explicitly computed and set with the first pose measurement. + timeOffset 0.0; Time offset added to the pose measurement timestamps + useOdometryCov false; Should the UpdateNoise position covariance be scaled using the covariance in the Odometry message + qVM_x 0; X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_y 0; Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_z 0; Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_w 1; W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + MrMV_x 0; X-entry of vector representing IMU to reference body coordinate frame of pose measurement + MrMV_y 0; Y-entry of vector representing IMU to reference body coordinate frame of pose measurement + MrMV_z 0; Z-entry of vector representing IMU to reference body coordinate frame of pose measurement + qWI_x 0; X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_y 0; Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_z 0; Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_w 1; W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + IrIW_x 0; X-entry of vector representing World to reference inertial coordinate frame of pose measurement + IrIW_y 0; Y-entry of vector representing World to reference inertial coordinate frame of pose measurement + IrIW_z 0; Z-entry of vector representing World to reference inertial coordinate frame of pose measurement +} +VelocityUpdate +{ + UpdateNoise + { + vel_0 0.0001 + vel_1 0.0001 + vel_2 0.0001 + } + MahalanobisTh0 7.689997599999999 + qAM_x 0 + qAM_y 0 + qAM_z 0 + qAM_w 1 +} diff --git a/cfg/rovio_domi.info_new b/cfg/rovio_domi.info_new new file mode 100644 index 00000000..b06399be --- /dev/null +++ b/cfg/rovio_domi.info_new @@ -0,0 +1,234 @@ +Common +{ + doVECalibration true + verbose false + depthType 1 +} +PoseUpdate +{ + doVisualization true +} +Init +{ + Covariance + { + pos_0 1 + pos_1 1 + pos_2 1 + vel_0 1 + vel_1 1 + vel_2 1 + acb_0 1 + acb_1 1 + acb_2 1 + gyb_0 1 + gyb_1 1 + gyb_2 1 + att_0 1 + att_1 1 + att_2 1 + vep 1 + vea 1 + } + State + { + att_x 0 + att_y 0 + att_z 0 + att_w 1 + gyb_0 0 + gyb_1 0 + gyb_2 0 + acb_0 0 + acb_1 0 + acb_2 0 + vel_0 0 + vel_1 0 + vel_2 0 + pos_0 0 + pos_1 0 + pos_2 0 + } +} +Camera0 +{ + qCM_x 0 + qCM_y 0 + qCM_z 0 + qCM_w 1 + MrMC_x 0 + MrMC_y 0 + MrMC_z 0 + CalibrationFile "" +} +Camera1 +{ + qCM_x 0 + qCM_y 0 + qCM_z 0 + qCM_w 1 + MrMC_x 0 + MrMC_y 0 + MrMC_z 0 + CalibrationFile "" +} +Camera2 +{ + qCM_x 0 + qCM_y 0 + qCM_z 0 + qCM_w 1 + MrMC_x 0 + MrMC_y 0 + MrMC_z 0 + CalibrationFile "" +} +VelocityUpdate +{ + UpdateNoise + { + vel_0 0.0001 + vel_1 0.0001 + vel_2 0.0001 + } + MahalanobisTh0 7.689997599999999 + qAM_x 0 + qAM_y 0 + qAM_z 0 + qAM_w 1 +} +PoseUpdate +{ + enablePosition true + enableAttitude true + noFeedbackToRovio true + doInertialAlignmentAtStart true + useOdometryCov false + UpdateNoise + { + pos_0 0 + pos_1 0 + pos_2 0 + att_0 0 + att_1 0 + att_2 0 + } + MahalanobisTh0 12.6511204 + qVM_x 0 + qVM_y 0 + qVM_z 0 + qVM_w 1 + MrMV_x 0 + MrMV_y 0 + MrMV_z 0 + qWI_x 0 + qWI_y 0 + qWI_z 0 + qWI_w 1 + IrIW_x 0 + IrIW_y 0 + IrIW_z 0 + timeOffset 0 +} +ImgUpdate +{ + doPatchWarping false + MotionDetection + { + isEnabled false + minFeatureCountForNoMotionDetection 5 + rateOfMovingFeaturesTh 0.5 + pixelCoordinateMotionTh 1 + } + useDirectMethod true + doFrameVisualisation true + visualizePatches false + removeNegativeFeatureAfterUpdate true + useCrossCameraMeasurements true + doStereoInitialization true + useIntensityOffsetForAlignment true + useIntensitySqewForAlignment true + statLocalQualityRange 10 + statLocalVisibilityRange 100 + statMinGlobalQualityRange 100 + maxNumIteration 10 + startLevel 3 + endLevel 1 + nDetectionBuckets 100 + fastDetectionThreshold 10 + alignMaxUniSample 5 + updateVecNormTermination 9.9999999999999995e-07 + MahalanobisTh 5.8858356000000001 + initCovFeature_0 1 + initCovFeature_1 1 + initCovFeature_2 1 + initDepth 0.5 + startDetectionTh 0.90000000000000002 + scoreDetectionExponent 0.5 + penaltyDistance 20 + zeroDistancePenalty 100 + trackingUpperBound 0.90000000000000002 + trackingLowerBound 0.10000000000000001 + minTrackedAndFreeFeatures 0.5 + minRelativeSTScore 0.20000000000000001 + minAbsoluteSTScore 0.20000000000000001 + minTimeBetweenPatchUpdate 1 + removalFactor 1.1000000000000001 + patchRejectionTh 10 + maxUncertaintyToDepthRatioForDepthInitialization 0.29999999999999999 + UpdateNoise + { + pix 2 + int 10000 + } + noiseGainForOffCamera 4 + alignConvergencePixelRange 1 + alignCoverageRatio 2 + alignmentHuberNormThreshold 0 + alignmentGaussianWeightingSigma 2 + alignmentGradientExponent 0 + discriminativeSamplingDistance 0 + discriminativeSamplingGain 0 + ZeroVelocityUpdate + { + isEnabled false + UpdateNoise + { + vel_0 0.0001 + vel_1 0.0001 + vel_2 0.0001 + } + minNoMotionTime 1 + MahalanobisTh0 7.689997599999999 + } +} +Prediction +{ + PredictionNoise + { + pos_0 0.0001 + pos_1 0.0001 + pos_2 0.0001 + vel_0 0.0001 + vel_1 0.0001 + vel_2 0.0001 + acb_0 0.0001 + acb_1 0.0001 + acb_2 0.0001 + gyb_0 0.0001 + gyb_1 0.0001 + gyb_2 0.0001 + att_0 0.0001 + att_1 0.0001 + att_2 0.0001 + vep 0.0001 + vea 0.0001 + nor 0.0001 + dep 0.0001 + } + MotionDetection + { + inertialMotionRorTh 0.10000000000000001 + inertialMotionAccTh 0.10000000000000001 + } +} diff --git a/cfg/rovio_orig.info b/cfg/rovio_orig.info new file mode 100644 index 00000000..f237fe5d --- /dev/null +++ b/cfg/rovio_orig.info @@ -0,0 +1,230 @@ +; You can partially override parameter set in this file by creating your own subset of parameter in a separate info-file and include it with: +; #include "/home/user/workspace/rovio/cfg/rovio_custom.info" +Common +{ + doVECalibration true; Should the camera-IMU extrinsics be calibrated online + depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic) + verbose false; Is the verbose active +} +Camera0 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x 0.00666398307551; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.0079168224269; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z -0.701985972528; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.712115587266; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x -0.0111674199187; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y -0.0574640920022; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z 0.0207586947896; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} +Camera1 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x 0.00151329637706; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.0123329249764; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z -0.702657352863; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.711419885414; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x -0.0091929160801; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0540646643676; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z 0.0190387660614; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} +Init +{ + State + { + pos_0 0; X-entry of initial position (world to IMU in world) [m] + pos_1 0; Y-entry of initial position (world to IMU in world) [m] + pos_2 0; Z-entry of initial position (world to IMU in world) [m] + vel_0 0; X-entry of initial velocity (robocentric, IMU) [m/s] + vel_1 0; Y-entry of initial velocity (robocentric, IMU) [m/s] + vel_2 0; Z-entry of initial velocity (robocentric, IMU) [m/s] + acb_0 0; X-entry of accelerometer bias [m/s^2] + acb_1 0; Y-entry of accelerometer bias [m/s^2] + acb_2 0; Z-entry of accelerometer bias [m/s^2] + gyb_0 0; X-entry of gyroscope bias [rad/s] + gyb_1 0; Y-entry of gyroscope bias [rad/s] + gyb_2 0; Z-entry of gyroscope bias [rad/s] + att_x 0; X-entry of initial attitude (IMU to world, Hamilton) + att_y 0; Y-entry of initial attitude (IMU to world, Hamilton) + att_z 0; Z-entry of initial attitude (IMU to world, Hamilton) + att_w 1; W-entry of initial attitude (IMU to world, Hamilton) + } + Covariance + { + pos_0 0.0001; X-Covariance of initial position [m^2] + pos_1 0.0001; Y-Covariance of initial position [m^2] + pos_2 0.0001; Z-Covariance of initial position [m^2] + vel_0 1.0; X-Covariance of initial velocity [m^2/s^2] + vel_1 1.0; Y-Covariance of initial velocity [m^2/s^2] + vel_2 1.0; Z-Covariance of initial velocity [m^2/s^2] + acb_0 4e-4; X-Covariance of initial accelerometer bias [m^2/s^4] + acb_1 4e-4; Y-Covariance of initial accelerometer bias [m^2/s^4] + acb_2 4e-4; Z-Covariance of initial accelerometer bias [m^2/s^4] + gyb_0 3e-4; X-Covariance of initial gyroscope bias [rad^2/s^2] + gyb_1 3e-4; Y-Covariance of initial gyroscope bias [rad^2/s^2] + gyb_2 3e-4; Z-Covariance of initial gyroscope bias [rad^2/s^2] + vep 0.0001; Covariance of initial linear camera-IMU extrinsics, same for all entries [m^2] + att_0 0.1; X-Covariance of initial attitude [rad^2] + att_1 0.1; Y-Covariance of initial attitude [rad^2] + att_2 0.1; Z-Covariance of initial attitude [rad^2] + vea 0.01; Covariance of initial rotational camera-IMU extrinsics, same for all entries [rad^2] + } +} +ImgUpdate +{ + updateVecNormTermination 1e-4; + maxNumIteration 20; + doPatchWarping true; Should the patches be warped + doFrameVisualisation true; Should the frames be visualized + visualizePatches false; Should the patches be visualized + useDirectMethod true; Should the EKF-innovation be based on direct intensity error (o.w. reprojection error) + startLevel 2; Highest patch level which is being employed (must be smaller than the hardcoded template parameter) + endLevel 1; Lowest patch level which is being employed + nDetectionBuckets 100; Number of discretization buckets used during the candidates selection + MahalanobisTh 9.21; Mahalanobis treshold for the update, 5.8858356 + UpdateNoise + { + pix 2; Covariance used for the reprojection error, 1/focal_length is roughly 1 pixel std [rad] (~1/f ~ 1/400^2 = 1/160000) + int 400; Covariance used for the photometric error [intensity^2] + } + initCovFeature_0 0.5; Covariance for the initial distance (Relative to initialization depth [m^2/m^2]) + initCovFeature_1 1e-5; Covariance for the initial bearing vector, x-component [rad^2] + initCovFeature_2 1e-5; Covariance for the initial bearing vector, y-component [rad^2] + initDepth 0.5; Initial value for the initial distance parameter + startDetectionTh 0.8; Threshold for detecting new features (min: 0, max: 1) + scoreDetectionExponent 0.25; Exponent used for weighting the distance between candidates + penaltyDistance 100; Maximal distance which gets penalized during bucketing [pix] + zeroDistancePenalty 100; Penalty for zero distance (max: nDetectionBuckets) + statLocalQualityRange 10; Number of frames for local quality evaluation + statLocalVisibilityRange 100; Number of frames for local visibility evaluation + statMinGlobalQualityRange 100; Minimum number of frames for obtaining maximal global quality + trackingUpperBound 0.9; Threshold for local quality for min overall global quality + trackingLowerBound 0.8; Threshold for local quality for max overall global quality + minTrackedAndFreeFeatures 0.75; Minimum of amount of feature which are either tracked or free + removalFactor 1.1; Factor for enforcing feature removal if not enough free + minRelativeSTScore 0.75; Minimum relative ST-score for extracting new feature patch + minAbsoluteSTScore 5.0; Minimum absolute ST-score for extracting new feature patch + minTimeBetweenPatchUpdate 1.0; Minimum time between new multilevel patch extrection [s] + fastDetectionThreshold 5; Fast corner detector treshold while adding new feature + alignConvergencePixelRange 10; Assumed convergence range for image alignment (gets scaled with the level) [pixels] + alignCoverageRatio 2; How many sigma of the uncertainty should be covered in the adaptive alignement + alignMaxUniSample 1; Maximal number of alignment seeds on one side -> total number of sample = 2n+1. Carefull can get very costly if diverging! + patchRejectionTh 50.0; If the average itensity error is larger than this the feauture is rejected [intensity], if smaller 0 the no check is performed + alignmentHuberNormThreshold 10; Intensity error threshold for Huber norm (enabled if > 0) + alignmentGaussianWeightingSigma -1; Width of Gaussian which is used for pixel error weighting (enabled if > 0) + alignmentGradientExponent 0.0; Exponent used for gradient based weighting of residuals + useIntensityOffsetForAlignment true; Should an intensity offset between the patches be considered + useIntensitySqewForAlignment true; Should an intensity sqew between the patches be considered + removeNegativeFeatureAfterUpdate true; Should feature with negative distance get removed + maxUncertaintyToDepthRatioForDepthInitialization 0.3; If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame + useCrossCameraMeasurements true; Should cross measurements between frame be used. Might be turned of in calibration phase. + doStereoInitialization true; Should a stereo match be used for feature initialization. + noiseGainForOffCamera 10.0; Factor added on update noise if not main camera + discriminativeSamplingDistance 0.02; Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed). + discriminativeSamplingGain 1.1; Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used). + MotionDetection + { + isEnabled 0; Is the motion detection enabled + rateOfMovingFeaturesTh 0.5; Amount of feature with motion for overall motion detection + pixelCoordinateMotionTh 1.0; Threshold for motion detection for patched [pixels] + minFeatureCountForNoMotionDetection 5; Min feature count in frame for motion detection + } + ZeroVelocityUpdate + { + UpdateNoise + { + vel_0 0.01; X-Covariance of zero velocity update [m^2/s^2] + vel_1 0.01; Y-Covariance of zero velocity update [m^2/s^2] + vel_2 0.01; Z-Covariance of zero velocity update [m^2/s^2] + } + MahalanobisTh0 7.689997599999999; Mahalanobid distance for zero velocity updates + minNoMotionTime 1.0; Min duration of no-motion + isEnabled 0; Should zero velocity update be applied, only works if MotionDetection.isEnabled is true + } +} +Prediction +{ + PredictionNoise + { + pos_0 1e-4; X-covariance parameter of position prediction [m^2/s] + pos_1 1e-4; Y-covariance parameter of position prediction [m^2/s] + pos_2 1e-4; Z-covariance parameter of position prediction [m^2/s] + vel_0 4e-6; X-covariance parameter of velocity prediction [m^2/s^3] + vel_1 4e-6; Y-covariance parameter of velocity prediction [m^2/s^3] + vel_2 4e-6; Z-covariance parameter of velocity prediction [m^2/s^3] + acb_0 1e-8; X-covariance parameter of accelerometer bias prediction [m^2/s^5] + acb_1 1e-8; Y-covariance parameter of accelerometer bias prediction [m^2/s^5] + acb_2 1e-8; Z-covariance parameter of accelerometer bias prediction [m^2/s^5] + gyb_0 3.8e-7; X-covariance parameter of gyroscope bias prediction [rad^2/s^3] + gyb_1 3.8e-7; Y-covariance parameter of gyroscope bias prediction [rad^2/s^3] + gyb_2 3.8e-7; Z-covariance parameter of gyroscope bias prediction [rad^2/s^3] + vep 1e-8; Covariance parameter of linear extrinsics prediction [m^2/s] + att_0 7.6e-7; X-Covariance parameter of attitude prediction [rad^2/s] + att_1 7.6e-7; Y-Covariance parameter of attitude prediction [rad^2/s] + att_2 7.6e-7; Z-Covariance parameter of attitude prediction [rad^2/s] + vea 1e-8; Covariance parameter of rotational extrinsics prediction [rad^2/s] + dep 0.0001; Covariance parameter of distance prediction [m^2/s] + nor 0.00001; Covariance parameter of bearing vector prediction [rad^2/s] + } + MotionDetection + { + inertialMotionRorTh 0.1; Treshold on rotational rate for motion detection [rad/s] + inertialMotionAccTh 0.5; Treshold on acceleration for motion detection [m/s^2] + } +} +PoseUpdate +{ + UpdateNoise + { + pos_0 0.01; X-Covariance of linear pose measurement update [m^2] + pos_1 0.01; Y-Covariance of linear pose measurement update [m^2] + pos_2 0.01; Z-Covariance of linear pose measurement update [m^2] + att_0 0.01; X-Covariance of rotational pose measurement update [rad^2] + att_1 0.01; Y-Covariance of rotational pose measurement update [rad^2] + att_2 0.01; Z-Covariance of rotational pose measurement update [rad^2] + } + init_cov_IrIW 1; Covariance of initial pose between inertial frames, linear part [m^2] + init_cov_qWI 1; Covariance of initial pose between inertial frames, rotational part [rad^2] + init_cov_MrMV 1; Covariance of initial pose between inertial frames, linear part [m^2] + init_cov_qVM 1; Covariance of initial pose between inertial frames, rotational part [rad^2] + pre_cov_IrIW 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s] + pre_cov_qWI 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s] + pre_cov_MrMV 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s] + pre_cov_qVM 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s] + MahalanobisTh0 12.6511204; Mahalonobis distance treshold of pose measurement + doVisualization false; Should the measured poses be vizualized + enablePosition true; Should the linear part be used during the update + enableAttitude true; Should the rotation part be used during the update (e.g. set to fals eif feeding GPS-measurement) + noFeedbackToRovio true; By default the pose update is only used for aligning coordinate frame. Set to false if ROVIO should benefit frome the posed measurements. + doInertialAlignmentAtStart true; Should the transformation between I and W be explicitly computed and set with the first pose measurement. + timeOffset 0.0; Time offset added to the pose measurement timestamps + useOdometryCov false; Should the UpdateNoise position covariance be scaled using the covariance in the Odometry message + qVM_x 0; X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_y 0; Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_z 0; Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + qVM_w 1; W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) + MrMV_x 0; X-entry of vector representing IMU to reference body coordinate frame of pose measurement + MrMV_y 0; Y-entry of vector representing IMU to reference body coordinate frame of pose measurement + MrMV_z 0; Z-entry of vector representing IMU to reference body coordinate frame of pose measurement + qWI_x 0; X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_y 0; Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_z 0; Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + qWI_w 1; W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) + IrIW_x 0; X-entry of vector representing World to reference inertial coordinate frame of pose measurement + IrIW_y 0; Y-entry of vector representing World to reference inertial coordinate frame of pose measurement + IrIW_z 0; Z-entry of vector representing World to reference inertial coordinate frame of pose measurement +} +VelocityUpdate +{ + UpdateNoise + { + vel_0 0.0001 + vel_1 0.0001 + vel_2 0.0001 + } + MahalanobisTh0 7.689997599999999 + qAM_x 0 + qAM_y 0 + qAM_z 0 + qAM_w 1 +} diff --git a/launch/rovio_M600.launch b/launch/rovio_M600.launch new file mode 100644 index 00000000..7cad7de1 --- /dev/null +++ b/launch/rovio_M600.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/rovio_domi.launch b/launch/rovio_domi.launch new file mode 100644 index 00000000..92cbb060 --- /dev/null +++ b/launch/rovio_domi.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/launch/rovio_node.launch b/launch/rovio_node.launch index 65cdde85..b7ecb5ed 100644 --- a/launch/rovio_node.launch +++ b/launch/rovio_node.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/rovio_node_orig.launch b/launch/rovio_node_orig.launch new file mode 100644 index 00000000..b2870740 --- /dev/null +++ b/launch/rovio_node_orig.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/launch/rovio_rosbag_node.launch b/launch/rovio_rosbag_node.launch index d5556c5f..e9cfb34b 100644 --- a/launch/rovio_rosbag_node.launch +++ b/launch/rovio_rosbag_node.launch @@ -4,9 +4,9 @@ - + - \ No newline at end of file + From 513c94c220b207c02b29322cc4f64c5ffaaeb422 Mon Sep 17 00:00:00 2001 From: trleonie Date: Tue, 23 Oct 2018 09:59:59 +0200 Subject: [PATCH 11/33] clean up --- cfg/imgui.ini | 40 ---------------------------------------- 1 file changed, 40 deletions(-) delete mode 100644 cfg/imgui.ini diff --git a/cfg/imgui.ini b/cfg/imgui.ini deleted file mode 100644 index 4599d85a..00000000 --- a/cfg/imgui.ini +++ /dev/null @@ -1,40 +0,0 @@ -[Debug] -Pos=60,60 -Size=400,400 -Collapsed=0 - -[Splash Screen Banner] -Pos=746,574 -Size=1792,1008 -Collapsed=0 - -[nostreaming_popup] -Pos=916,454 -Size=1452,50 -Collapsed=0 - -[Notification parent window] -Pos=60,60 -Size=32,32 -Collapsed=0 - -[Stream of 0] -Pos=357,53 -Size=690,32 -Collapsed=0 - -[Stream of 1] -Pos=357,477 -Size=690,32 -Collapsed=0 - -[numbered_ruler] -Pos=1018,103 -Size=32,358 -Collapsed=0 - -[Stream of 3] -Pos=1083,53 -Size=690,32 -Collapsed=0 - From bc0b2b0f907df7de29abfa43fc091fee8a3c5d31 Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 19 Nov 2018 16:23:55 +0100 Subject: [PATCH 12/33] rovio with three cameras working --- cfg/rovio_domi.info | 13 ++++++++++++- include/rovio/RovioNode.hpp | 14 +++++++++++++- launch/rovio_domi.launch | 9 +++++++-- src/FeatureCoordinates.cpp | 6 +++--- 4 files changed, 35 insertions(+), 7 deletions(-) diff --git a/cfg/rovio_domi.info b/cfg/rovio_domi.info index 2b98b1b8..88922a28 100644 --- a/cfg/rovio_domi.info +++ b/cfg/rovio_domi.info @@ -18,6 +18,17 @@ Camera0 MrMC_z 0.00949435362177; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] } Camera1 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x -0.00953751922809; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.703628160853; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z -0.710497111443; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.00320961269892; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x 0.0341624938784; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0489333125579; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.0250547932613; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} +Camera2 { CalibrationFile ; Camera-Calibration file for intrinsics qCM_x 0.498284808832; X-entry of IMU to Camera quaterion (Hamilton) @@ -118,7 +129,7 @@ ImgUpdate removeNegativeFeatureAfterUpdate true; Should feature with negative distance get removed maxUncertaintyToDepthRatioForDepthInitialization 0.3; If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame useCrossCameraMeasurements false; Should cross measurements between frame be used. Might be turned of in calibration phase. - doStereoInitialization true; Should a stereo match be used for feature initialization. + doStereoInitialization false; Should a stereo match be used for feature initialization. noiseGainForOffCamera 10.0; Factor added on update noise if not main camera discriminativeSamplingDistance 0.02; Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed). discriminativeSamplingGain 1.1; Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used). diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index 92c0030f..28ae50ec 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -131,6 +131,7 @@ class RovioNode{ ros::Subscriber subImu_; ros::Subscriber subImg0_; ros::Subscriber subImg1_; + ros::Subscriber subImg2_; ros::Subscriber subGroundtruth_; ros::Subscriber subGroundtruthOdometry_; ros::Subscriber subVelocity_; @@ -208,6 +209,7 @@ class RovioNode{ subImu_ = nh_.subscribe("imu0", 1000, &RovioNode::imuCallback,this); subImg0_ = nh_.subscribe("cam0/image_raw", 1000, &RovioNode::imgCallback0,this); subImg1_ = nh_.subscribe("cam1/image_raw", 1000, &RovioNode::imgCallback1,this); + subImg2_ = nh_.subscribe("cam2/image_raw", 1000, &RovioNode::imgCallback2,this); subGroundtruth_ = nh_.subscribe("pose", 1000, &RovioNode::groundtruthCallback,this); subGroundtruthOdometry_ = nh_.subscribe("odometry", 1000, &RovioNode::groundtruthOdometryCallback, this); subVelocity_ = nh_.subscribe("abss/twist", 1000, &RovioNode::velocityCallback,this); @@ -490,6 +492,16 @@ class RovioNode{ if(mtState::nCam_ > 1) imgCallback(img,1); } + /** \brief Image callback for the camera with ID 2 + * + * @param img - Image message. + * @todo generalize + */ + void imgCallback2(const sensor_msgs::ImageConstPtr & img) { + std::lock_guard lock(m_filter_); + if(mtState::nCam_ > 2) imgCallback(img,2); + } + /** \brief Image callback. Adds images (as update measurements) to the filter. * * @param img - Image message. @@ -664,7 +676,7 @@ class RovioNode{ // Obtain the save filter state. mtFilterState& filterState = mpFilter_->safe_; - mtState& state = mpFilter_->safe_.state_; + mtState& state = mpFilter_->safe_.state_; state.updateMultiCameraExtrinsics(&mpFilter_->multiCamera_); MXD& cov = mpFilter_->safe_.cov_; imuOutputCT_.transformState(state,imuOutput_); diff --git a/launch/rovio_domi.launch b/launch/rovio_domi.launch index 92cbb060..f14745ed 100644 --- a/launch/rovio_domi.launch +++ b/launch/rovio_domi.launch @@ -2,16 +2,21 @@ - + + - + + + + + diff --git a/src/FeatureCoordinates.cpp b/src/FeatureCoordinates.cpp index bf5238e5..5d963ae4 100644 --- a/src/FeatureCoordinates.cpp +++ b/src/FeatureCoordinates.cpp @@ -50,7 +50,7 @@ namespace rovio{ const cv::Point2f& FeatureCoordinates::get_c() const{ if(!com_c()){ - std::cout << " \033[31mERROR: No valid coordinate data!\033[0m" << std::endl; + std::cout << " \033[31mERROR: No valid coordinate data [1]!\033[0m" << std::endl; } return c_; } @@ -67,7 +67,7 @@ namespace rovio{ const LWF::NormalVectorElement& FeatureCoordinates::get_nor() const{ if(!com_nor()){ - std::cout << " \033[31mERROR: No valid coordinate data!\033[0m" << std::endl; + std::cout << " \033[31mERROR: No valid coordinate data [2]!\033[0m" << std::endl; } return nor_; } @@ -76,7 +76,7 @@ namespace rovio{ assert(mpCamera_ != nullptr); if(!mpCamera_->bearingToPixel(get_nor(),c_,matrix2dTemp_)){ matrix2dTemp_.setZero(); - std::cout << " \033[31mERROR: No valid coordinate data!\033[0m" << std::endl; + std::cout << " \033[31mERROR: No valid coordinate data [3]!\033[0m" << std::endl; } return matrix2dTemp_; } From f55ff7a9ca8c2e23b078ce5dc4fd178bacc19c6a Mon Sep 17 00:00:00 2001 From: trleonie Date: Tue, 20 Nov 2018 11:34:41 +0100 Subject: [PATCH 13/33] up to five cam setup --- cfg/rovio_domi.info | 23 +++++++++++++++++++ include/rovio/RovioNode.hpp | 29 +++++++++++++++++++++++- launch/rovio_domi.launch | 44 +++++++++++++++++++++++-------------- 3 files changed, 79 insertions(+), 17 deletions(-) diff --git a/cfg/rovio_domi.info b/cfg/rovio_domi.info index 88922a28..bd983f04 100644 --- a/cfg/rovio_domi.info +++ b/cfg/rovio_domi.info @@ -39,6 +39,29 @@ Camera2 MrMC_y 0.0446798994558; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] MrMC_z -0.00861710075576; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] } +Camera3 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x -0.717540995787; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.00571832829522; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z 0.0123040970923; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.696384110446; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x 0.0323515084872; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y -0.0480190261945; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.0324906232515; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} +Camera4 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x 0.490198388917; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y 0.5116843721; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z -0.490599785917; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.50714543566; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x -0.064799845427; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.042329395001; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.00623084134628; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} + Init { State diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index 28ae50ec..bfd16364 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -33,6 +33,8 @@ #include #include +#include + #include #include #include @@ -132,6 +134,8 @@ class RovioNode{ ros::Subscriber subImg0_; ros::Subscriber subImg1_; ros::Subscriber subImg2_; + ros::Subscriber subImg3_; + ros::Subscriber subImg4_; ros::Subscriber subGroundtruth_; ros::Subscriber subGroundtruthOdometry_; ros::Subscriber subVelocity_; @@ -207,9 +211,12 @@ class RovioNode{ // Subscribe topics subImu_ = nh_.subscribe("imu0", 1000, &RovioNode::imuCallback,this); + // subImg0_ = nh_.subscribe("cam0/image_raw", 1000, std::bind(&RovioNode::imgCallback, std::placeholders::_1, 0),this); subImg0_ = nh_.subscribe("cam0/image_raw", 1000, &RovioNode::imgCallback0,this); subImg1_ = nh_.subscribe("cam1/image_raw", 1000, &RovioNode::imgCallback1,this); subImg2_ = nh_.subscribe("cam2/image_raw", 1000, &RovioNode::imgCallback2,this); + subImg3_ = nh_.subscribe("cam3/image_raw", 1000, &RovioNode::imgCallback3,this); + subImg4_ = nh_.subscribe("cam4/image_raw", 1000, &RovioNode::imgCallback4,this); subGroundtruth_ = nh_.subscribe("pose", 1000, &RovioNode::groundtruthCallback,this); subGroundtruthOdometry_ = nh_.subscribe("odometry", 1000, &RovioNode::groundtruthOdometryCallback, this); subVelocity_ = nh_.subscribe("abss/twist", 1000, &RovioNode::velocityCallback,this); @@ -502,6 +509,26 @@ class RovioNode{ if(mtState::nCam_ > 2) imgCallback(img,2); } + /** \brief Image callback for the camera with ID 3 + * + * @param img - Image message. + * @todo generalize + */ + void imgCallback3(const sensor_msgs::ImageConstPtr & img) { + std::lock_guard lock(m_filter_); + if(mtState::nCam_ > 3) imgCallback(img,3); + } + + /** \brief Image callback for the camera with ID 4 + * + * @param img - Image message. + * @todo generalize + */ + void imgCallback4(const sensor_msgs::ImageConstPtr & img) { + std::lock_guard lock(m_filter_); + if(mtState::nCam_ > 4) imgCallback(img,4); + } + /** \brief Image callback. Adds images (as update measurements) to the filter. * * @param img - Image message. @@ -523,7 +550,7 @@ class RovioNode{ if(msgTime != imgUpdateMeas_.template get().imgTime_){ for(int i=0;i().isValidPyr_[i]){ - std::cout << " \033[31mFailed Synchronization of Camera Frames, t = " << msgTime << "\033[0m" << std::endl; + std::cout << " \033[31mFailed Synchronization of Camera Frames, t = " << msgTime << " " << camID << "\033[0m" << std::endl; } } imgUpdateMeas_.template get().reset(msgTime); diff --git a/launch/rovio_domi.launch b/launch/rovio_domi.launch index f14745ed..7925e092 100644 --- a/launch/rovio_domi.launch +++ b/launch/rovio_domi.launch @@ -1,22 +1,34 @@ - - - - - - - - + + + + + + + + + + + + + + + - - - - - - - - + + + + + + + + + + + + + From 53e394de0c277f10e2a989c3dc6e80bb7a5e7930 Mon Sep 17 00:00:00 2001 From: trleonie Date: Wed, 28 Nov 2018 11:24:41 +0100 Subject: [PATCH 14/33] added histogram equalization --- include/rovio/RovioNode.hpp | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index bfd16364..d9165686 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -545,6 +545,36 @@ class RovioNode{ } cv::Mat cv_img; cv_ptr->image.copyTo(cv_img); + + // Histogram equalization + //if(histogram_equalize_){ + constexpr size_t hist_bins = 256; + cv::Mat hist; + std::vector img_vec = {cv_img}; + cv::calcHist(img_vec, {0}, cv::Mat(), hist, {hist_bins}, + {0, hist_bins-1}, false); + cv::Mat lut(1, hist_bins, CV_8UC1); + double sum = 0.0; + //prevents an image full of noise if in total darkness + float max_per_bin = cv_img.cols * cv_img.rows * 0.02; + float min_per_bin = cv_img.cols * cv_img.rows * 0.002; + float total_pixels = 0; + for(size_t i = 0; i < hist_bins; ++i){ + float& bin = hist.at(i); + if(bin > max_per_bin){ + bin = max_per_bin; + } else if(bin < min_per_bin){ + bin = min_per_bin; + } + total_pixels += bin; + } + for(size_t i = 0; i < hist_bins; ++i){ + sum += hist.at(i) / total_pixels; + lut.at(i) = (hist_bins-1)*sum; + } + cv::LUT(cv_img, lut, cv_img); + //} + if(init_state_.isInitialized() && !cv_img.empty()){ double msgTime = img->header.stamp.toSec(); if(msgTime != imgUpdateMeas_.template get().imgTime_){ From 8de80e40843435253c950079d6793a345a3100c4 Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 3 Dec 2018 09:14:41 +0100 Subject: [PATCH 15/33] added histogram equalization --- include/rovio/RovioNode.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index d9165686..11e2f2d6 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -125,6 +125,7 @@ class RovioNode{ bool forceMarkersPublishing_; bool forcePatchPublishing_; bool gotFirstMessages_; + bool histogram_equalize_ = true; std::mutex m_filter_; // Nodes, Subscriber, Publishers @@ -547,7 +548,7 @@ class RovioNode{ cv_ptr->image.copyTo(cv_img); // Histogram equalization - //if(histogram_equalize_){ + if(histogram_equalize_){ constexpr size_t hist_bins = 256; cv::Mat hist; std::vector img_vec = {cv_img}; @@ -573,7 +574,7 @@ class RovioNode{ lut.at(i) = (hist_bins-1)*sum; } cv::LUT(cv_img, lut, cv_img); - //} + } if(init_state_.isInitialized() && !cv_img.empty()){ double msgTime = img->header.stamp.toSec(); From 0367a4021be622f8ca8ba18bc4028d9ce337e6ec Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 3 Dec 2018 09:16:13 +0100 Subject: [PATCH 16/33] visualization of candidates --- include/rovio/ImgUpdate.hpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/include/rovio/ImgUpdate.hpp b/include/rovio/ImgUpdate.hpp index 0f440925..ccca4c7c 100644 --- a/include/rovio/ImgUpdate.hpp +++ b/include/rovio/ImgUpdate.hpp @@ -987,6 +987,14 @@ ImgOutlierDetection,false>{ for(int l=endLevel_;l<=startLevel_;l++){ meas.aux().pyr_[camID].detectFastCorners(candidates_,l,fastDetectionThreshold_); } + + // visualization of detected candidates + if(doFrameVisualisation_){ + for(int i=0;i newSet = filterState.fsm_.addBestCandidates(candidates_,meas.aux().pyr_[camID],camID,filterState.t_, @@ -1005,8 +1013,8 @@ ImgOutlierDetection,false>{ filterState.resetFeatureCovariance(*it,initCovFeature_); initCovFeature_(0,0) = initRelDepthCovTemp_; if(doFrameVisualisation_){ - f.mpCoordinates_->drawPoint(filterState.img_[camID], cv::Scalar(255,0,0)); - f.mpCoordinates_->drawText(filterState.img_[camID],std::to_string(f.idx_),cv::Scalar(255,0,0)); + f.mpCoordinates_->drawPoint(filterState.img_[camID], cv::Scalar(255,255,0)); //changed color from blue to aquamarin + f.mpCoordinates_->drawText(filterState.img_[camID],std::to_string(f.idx_),cv::Scalar(255,255,0)); //changed color from blue to aquamarin } if(mtState::nCam_>1 && doStereoInitialization_){ From 300208822457115e2ac646484cff5724185de62c Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 10 Dec 2018 09:21:25 +0100 Subject: [PATCH 17/33] health checker added --- include/rovio/HealthMonitor.hpp | 90 +++++++++++++++++++++++++++++++++ include/rovio/RovioNode.hpp | 23 +++++++++ 2 files changed, 113 insertions(+) create mode 100644 include/rovio/HealthMonitor.hpp diff --git a/include/rovio/HealthMonitor.hpp b/include/rovio/HealthMonitor.hpp new file mode 100644 index 00000000..8b6f245f --- /dev/null +++ b/include/rovio/HealthMonitor.hpp @@ -0,0 +1,90 @@ +// This file was stolen straight out of maplab + +#ifndef ROVIO_HEALTH_MONITOR_H_ +#define ROVIO_HEALTH_MONITOR_H_ + +#include +#include + +#include "rovio/CoordinateTransform/RovioOutput.hpp" +#include "rovio/RovioFilter.hpp" + +namespace rovio { +class RovioHealthMonitor { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + RovioHealthMonitor() : num_subsequent_unhealthy_updates_(0) {} + + // Returns true if healthy; false if unhealthy and reset was triggered. + bool shouldResetEstimator(const std::vector& distance_covs_in, const StandardOutput& imu_output) { + float feature_distance_covariance_median = 0; + std::vector distance_covs = distance_covs_in; + if (!distance_covs.empty()) { + const size_t middle_index = distance_covs.size() / 2; + std::nth_element(distance_covs.begin(), + distance_covs.begin() + middle_index, + distance_covs.end()); + feature_distance_covariance_median = distance_covs[middle_index]; + } + const float BvB_norm = imu_output.BvB().norm(); + if ((BvB_norm > kVelocityToConsiderStatic) && ((BvB_norm > kUnhealthyVelocity) || + (feature_distance_covariance_median > kUnhealthyFeatureDistanceCov))) { + ++num_subsequent_unhealthy_updates_; + std::cout << "Estimator fault counter: " + << num_subsequent_unhealthy_updates_ << "/" + << kMaxSubsequentUnhealthyUpdates << ". Might reset soon."; + if (num_subsequent_unhealthy_updates_ > kMaxSubsequentUnhealthyUpdates) { + std::cout << "Will reset ROVIOLI. Velocity norm: " << BvB_norm + << " (limit: " << kUnhealthyVelocity + << "), median of feature distance covariances: " + << feature_distance_covariance_median + << " (limit: " << kUnhealthyFeatureDistanceCov << ")."; + return true; + } + } else { + if (feature_distance_covariance_median < kHealthyFeatureDistanceCov) { + if (std::abs(feature_distance_covariance_median - + last_safe_pose_.feature_distance_covariance_median) < + kHealthyFeatureDistanceCovIncrement) { + last_safe_pose_.failsafe_WrWB = imu_output.WrWB(); + last_safe_pose_.failsafe_qBW = imu_output.qBW(); + last_safe_pose_.feature_distance_covariance_median = feature_distance_covariance_median; + } + } + num_subsequent_unhealthy_updates_ = 0; + } + return false; + } + + Eigen::Vector3d failsafe_WrWB() { return last_safe_pose_.failsafe_WrWB; } + + kindr::RotationQuaternionPD failsafe_qBW() { + return last_safe_pose_.failsafe_qBW; + } + + private: + struct RovioFailsafePose { + RovioFailsafePose() + : failsafe_WrWB(Eigen::Vector3d::Zero()), feature_distance_covariance_median(0.0) { + failsafe_qBW.setIdentity(); + } + Eigen::Vector3d failsafe_WrWB; + kindr::RotationQuaternionPD failsafe_qBW; + float feature_distance_covariance_median; + }; + + RovioFailsafePose last_safe_pose_; + int num_subsequent_unhealthy_updates_; + + // The landmark covariance is not a good measure for divergence if we are static. + static constexpr float kVelocityToConsiderStatic = 0.1f; + static constexpr int kMaxSubsequentUnhealthyUpdates = 1; + static constexpr float kHealthyFeatureDistanceCov = 0.5f; + static constexpr float kHealthyFeatureDistanceCovIncrement = 0.3f; + static constexpr float kUnhealthyFeatureDistanceCov = 1.0f; + static constexpr float kUnhealthyVelocity = 5.0f; +}; + +} // namespace rovio + #endif // ROVIO_HEALTH_MONITOR_H_ \ No newline at end of file diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index 11e2f2d6..76b70ead 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -52,6 +52,7 @@ #include #include "rovio/RovioFilter.hpp" +#include "rovio/HealthMonitor.hpp" #include "rovio/CoordinateTransform/RovioOutput.hpp" #include "rovio/CoordinateTransform/FeatureOutput.hpp" #include "rovio/CoordinateTransform/FeatureOutputReadable.hpp" @@ -86,6 +87,8 @@ class RovioNode{ typedef typename mtVelocityUpdate::mtMeas mtVelocityMeas; mtVelocityMeas velocityUpdateMeas_; + RovioHealthMonitor healthMonitor_; + struct FilterInitializationState { FilterInitializationState() : WrWM_(V3D::Zero()), @@ -125,7 +128,10 @@ class RovioNode{ bool forceMarkersPublishing_; bool forcePatchPublishing_; bool gotFirstMessages_; + bool histogram_equalize_ = true; + bool healthCheck_ = false; + std::mutex m_filter_; // Nodes, Subscriber, Publishers @@ -934,6 +940,8 @@ class RovioNode{ pubImuBias_.publish(imuBiasMsg_); } + std::vector featureDistanceCov; + // PointCloud message. if(pubPcl_.getNumSubscribers() > 0 || pubMarkers_.getNumSubscribers() > 0 || forcePclPublishing_ || forceMarkersPublishing_){ pclMsg_.header.seq = msgSeq_; @@ -976,6 +984,8 @@ class RovioNode{ featureOutputReadableCT_.transformState(featureOutput_,featureOutputReadable_); featureOutputReadableCT_.transformCovMat(featureOutput_,featureOutputCov_,featureOutputReadableCov_); + featureDistanceCov.push_back(static_cast(featureOutputReadableCov_(3, 3))); // for health checker + // Get landmark output landmarkOutputImuCT_.setFeatureID(i); landmarkOutputImuCT_.transformState(state,landmarkOutput_); @@ -1071,6 +1081,19 @@ class RovioNode{ pubPatch_.publish(patchMsg_); } gotFirstMessages_ = true; + + if (healthCheck_) { + if(healthMonitor_.shouldResetEstimator(featureDistanceCov, imuOutput_)) { + if(!init_state_.isInitialized()) { + std::cout << "Reinitioalization already triggered. Ignoring request..."; + return; + } + + init_state_.WrWM_ = healthMonitor_.failsafe_WrWB(); + init_state_.qMW_ = healthMonitor_.failsafe_qBW(); + init_state_.state_ = FilterInitializationState::State::WaitForInitExternalPose; + } + } } } } From e4c2001284c2e2070235d7ecf7afe7c039f5f553 Mon Sep 17 00:00:00 2001 From: trleonie Date: Wed, 12 Dec 2018 14:26:59 +0100 Subject: [PATCH 18/33] add best global features --- include/rovio/FeatureManager.hpp | 175 ++++++++++++++++++++++++++++++- include/rovio/ImgUpdate.hpp | 125 ++++++++++++++-------- 2 files changed, 254 insertions(+), 46 deletions(-) diff --git a/include/rovio/FeatureManager.hpp b/include/rovio/FeatureManager.hpp index 2fe0917b..dd425dd2 100644 --- a/include/rovio/FeatureManager.hpp +++ b/include/rovio/FeatureManager.hpp @@ -265,7 +265,7 @@ class FeatureSetManager{ * @param initTime - Current time (time at which the MultilevelPatchFeature%s are created from the candidates list). * @param l1 - Start pyramid level for the Shi-Tomasi Score computation of MultilevelPatchFeature%s extracted from the candidates list. * @param l2 - End pyramid level for the Shi-Tomasi Score computation of MultilevelPatchFeature%s extracted from the candidates list. - * @param maxAddedFeature - Maximal number of features which should be added to the mlpSet. + * @param maxAddedFeature - Maximal number of features which should be added to the mlpSet. * @param nDetectionBuckets - Number of buckets. * @param scoreDetectionExponent - Choose it between [0 1]. 1 : Candidate features are sorted linearly into the buckets, depending on their Shi-Tomasi score. * 0 : All candidate features are filled into the highest bucket. @@ -389,6 +389,179 @@ class FeatureSetManager{ return newFeatureIDs; } +//////////////////////////////////////////////////////////////////////////// FUNCTION FOR ADDING BEST GLOBAL FEATURES +//////////////////////////////////////////////////////////////////////////// added by Leonie Traffelet + + /** \brief Adds the best MultilevelPatchFeature%s from a candidates list to an existing MultilevelPatchSet of ALL CAMERAS. + * + * This function takes a given feature candidate list of all cameras and builds, in a first step, + * MultilevelPatchFeature%s out of it using a given image pyramid. For each MultilevelPatchFeature the + * corresponding Shi-Tomasi Score is computed. + * In a second step, the candidate MultilevelPatchFeature%s are sorted and + * placed into buckets of their detection camera, depending on their individual Shi-Tomasi Score. MultilevelPatchFeature%s in a high bucket + * (high bucket index) have higher Shi-Tomasi Scores than MultilevelPatchFeature%s which have been placed into a + * low bucket. + * In a third step, the MultilevelPatchFeature%s in the buckets are reordered, depending on their distance + * to already existing features in the given MultilevelPatchSet. A small distance to an existing feature is punished, + * by moving the concerned candidate MultilevelPatchFeature into a lower bucket. + * Finally the best MultilevelPatchFeatures of each camera bucket are compared to each other and the best ones are added to the existing MultilevelPatchSet. + * + * @param candidates[nCam] - Array containing the list of candidate feature coordinates per camera. + * @param pyr[nCam] - Image pyramids of all the cameras used to extract the MultilevelPatchFeature%s from the candidates list. + * @param initTime - Current time (time at which the MultilevelPatchFeature%s are created from the candidates list). + * @param l1 - Start pyramid level for the Shi-Tomasi Score computation of MultilevelPatchFeature%s extracted from the candidates list. + * @param l2 - End pyramid level for the Shi-Tomasi Score computation of MultilevelPatchFeature%s extracted from the candidates list. + * @param maxAddedFeature - Maximal number of features which should be added to the mlpSet. + * @param nDetectionBuckets - Number of buckets. + * @param scoreDetectionExponent - Choose it between [0 1]. 1 : Candidate features are sorted linearly into the buckets, depending on their Shi-Tomasi score. + * 0 : All candidate features are filled into the highest bucket. + * A small scoreDetectionExponent forces more candidate features into high buckets. + * @param penaltyDistance - If a candidate feature has a smaller distance to an existing feature in the mlpSet, it is punished (shifted in an lower bucket) dependent of its actual distance to the existing feature. + * @param zeroDistancePenalty - A candidate feature in a specific bucket is shifted zeroDistancePenalty-buckets back to a lower bucket if it has zero distance to an existing feature in the mlpSet. + * @param requireMax - Should the adding of maxAddedFeature be enforced? + * @param minScore - Shi-Tomasi Score threshold for the best (highest Shi-Tomasi Score) MultilevelPatchFeature extracted from the candidates list. + * If the best MultilevelPatchFeature has a Shi-Tomasi Score less than or equal this threshold, the function aborts and returns an empty map. + * + * @return an unordered_set, holding the indizes of the MultilevelPatchSet, at which the new MultilevelPatchFeature%s have been added (from the candidates list). + */ + + std::unordered_set addBestGlobalCandidates(const FeatureCoordinatesVec (&candidates)[nCam], const ImagePyramid (&pyr)[nCam], const double initTime, + const int l1, const int l2, const int maxAddedFeature, const int nDetectionBuckets, const double scoreDetectionExponent, + const double penaltyDistance, const double zeroDistancePenalty, const bool requireMax, const float minScore){ + std::unordered_set newFeatureIDs; + std::vector> multilevelPatches[nCam]; + std::vector>> buckets(nCam, std::vector>(nDetectionBuckets)); + + double d2; + double t2 = pow(penaltyDistance,2); + bool doDelete; + unsigned int newBucketID; + + for(int camID = 0;camID maxScore) maxScore = multilevelPatches[camID].back().s_; + } else { + multilevelPatches[camID].back().s_ = -1; + } + } + if(maxScore <= minScore){ + break; + } + + // Make buckets and fill based on score + float relScore; + for(int i=0;i 0.0){ + newBucketID = std::ceil((nDetectionBuckets-1)*(pow(relScore,static_cast(scoreDetectionExponent)))); + if(newBucketID>nDetectionBuckets-1) newBucketID = nDetectionBuckets-1; + buckets[camID][newBucketID].insert(i); + } + } + + // Move buckets based on current features + FeatureCoordinates featureCoordinates; + FeatureDistance featureDistance; + for(unsigned int i=0;itransformFeature(camID,*(features_[i].mpCoordinates_),*(features_[i].mpDistance_),featureCoordinates,featureDistance); + if(featureCoordinates.isInFront()){ + for (unsigned int bucketID = 1;bucketID < nDetectionBuckets;bucketID++) { + for (auto it_cand = buckets[camID][bucketID].begin();it_cand != buckets[camID][bucketID].end();) { + doDelete = false; + d2 = std::pow(featureCoordinates.get_c().x - candidates[camID][*it_cand].get_c().x,2) + std::pow(featureCoordinates.get_c().y - candidates[camID][*it_cand].get_c().y,2); // Squared distance between the existing feature and the candidate feature. + if(d2 > nfCandidates(2, std::vector(nCam)); + std::vector bestBucketID(nCam, nDetectionBuckets-1); + + + while(addedCount < maxAddedFeature && getValidCount() != nMax) { + // get best features in each camera + for (int camID = 0; camID < nCam; camID++){ + for (int bucketID = bestBucketID[camID];bucketID >= 0+static_cast(!requireMax);bucketID--) { + //std::cout << "in bucket for loop. camera: " << camID << " Bucket: " << bucketID << std::endl; + if(!buckets[camID][bucketID].empty()){ + nfCandidates[0][camID] = bucketID; + nfCandidates[1][camID] = *(buckets[camID][bucketID].begin()); + bestBucketID[camID] = bucketID; + break; + } + } + } + + // get best of these + const int nfCam = std::max_element(nfCandidates[1].begin(), nfCandidates[1].end()) - nfCandidates[1].begin(); + const int nfBucket = nfCandidates[0][nfCam]; + const int nf = nfCandidates[1][nfCam]; + + buckets[nfCam][nfBucket].erase(nf); + const int ind = makeNewFeature(nfCam); + features_[ind].mpCoordinates_->set_c(candidates[nfCam][nf].get_c()); + features_[ind].mpCoordinates_->camID_ = nfCam; + features_[ind].mpCoordinates_->set_warp_identity(); + features_[ind].mpCoordinates_->mpCamera_ = &mpMultiCamera_->cameras_[nfCam]; + *(features_[ind].mpMultilevelPatch_) = multilevelPatches[nfCam][nf]; + if(ind >= 0){ + newFeatureIDs.insert(ind); + } + addedCount++; + for (unsigned int bucketID2 = 1;bucketID2 <= nfBucket;bucketID2++) { + for (auto it_cand = buckets[nfCam][bucketID2].begin();it_cand != buckets[nfCam][bucketID2].end();) { + doDelete = false; + d2 = std::pow(candidates[nfCam][nf].get_c().x - candidates[nfCam][*it_cand].get_c().x,2) + std::pow(candidates[nfCam][nf].get_c().y - candidates[nfCam][*it_cand].get_c().y,2); + if(d2,int>& mp1, const std::tuple,int>&mp2){ return std::get<1>(mp1).s_ > std::get<1>(mp2).s_; } diff --git a/include/rovio/ImgUpdate.hpp b/include/rovio/ImgUpdate.hpp index ccca4c7c..85e32936 100644 --- a/include/rovio/ImgUpdate.hpp +++ b/include/rovio/ImgUpdate.hpp @@ -224,6 +224,7 @@ ImgOutlierDetection,false>{ int alignMaxUniSample_; bool useCrossCameraMeasurements_; /**,false>{ mutable MultilevelPatch mlpTemp2_; mutable FeatureCoordinates alignedCoordinates_; mutable FeatureCoordinates tempCoordinates_; - mutable FeatureCoordinatesVec candidates_; + mutable FeatureCoordinatesVec candidates_[mtState::nCam_]; + //mutable std::vector candidates_; mutable cv::Point2f c_temp_; mutable Eigen::Matrix2d c_J_; mutable Eigen::Matrix2d A_red_; @@ -305,6 +307,7 @@ ImgOutlierDetection,false>{ alignMaxUniSample_ = 5; useCrossCameraMeasurements_ = true; doStereoInitialization_ = true; + addGlobalBest_ = false; removalFactor_ = 1.1; alignmentGaussianWeightingSigma_ = 2.0; discriminativeSamplingDistance_ = 0.0; @@ -343,6 +346,7 @@ ImgOutlierDetection,false>{ boolRegister_.registerScalar("removeNegativeFeatureAfterUpdate",removeNegativeFeatureAfterUpdate_); boolRegister_.registerScalar("useCrossCameraMeasurements",useCrossCameraMeasurements_); boolRegister_.registerScalar("doStereoInitialization",doStereoInitialization_); + boolRegister_.registerScalar("addGlobalBest",addGlobalBest_); boolRegister_.registerScalar("useIntensityOffsetForAlignment",alignment_.useIntensityOffset_); boolRegister_.registerScalar("useIntensitySqewForAlignment",alignment_.useIntensitySqew_); doubleRegister_.removeScalarByVar(updnoiP_(0,0)); @@ -979,78 +983,109 @@ ImgOutlierDetection,false>{ } else { medianDepthParameters.fill(initDepth_); } + + if(verbose_) std::cout << "Adding keypoints" << std::endl; + + // Get Candidates for(int camID = 0;camID newSet = filterState.fsm_.addBestCandidates(candidates_,meas.aux().pyr_[camID],camID,filterState.t_, - endLevel_,startLevel_,(mtState::nMax_-filterState.fsm_.getValidCount())/(mtState::nCam_-camID),nDetectionBuckets_, scoreDetectionExponent_, - penaltyDistance_, zeroDistancePenalty_,false,minAbsoluteSTScore_); + std::unordered_set newSet = filterState.fsm_.addBestGlobalCandidates(candidates_,meas.aux().pyr_,filterState.t_, + endLevel_,startLevel_,(mtState::nMax_-filterState.fsm_.getValidCount()),nDetectionBuckets_, scoreDetectionExponent_, + penaltyDistance_, zeroDistancePenalty_,false,minAbsoluteSTScore_); const double t3 = (double) cv::getTickCount(); - if(verbose_) std::cout << "== Got " << filterState.fsm_.getValidCount() << " after adding " << newSet.size() << " features in camera " << camID << " (" << (t3-t2)/cv::getTickFrequency()*1000 << " ms)" << std::endl; + if(verbose_) std::cout << "== Got " << filterState.fsm_.getValidCount() << " after adding " << newSet.size() << " features in" << " " << (t3-t2)/cv::getTickFrequency()*1000 << " ms" << std::endl; for(auto it = newSet.begin();it != newSet.end();++it){ FeatureManager& f = filterState.fsm_.features_[*it]; f.mpStatistics_->resetStatistics(filterState.t_); - f.mpStatistics_->status_[camID] = TRACKED; + f.mpStatistics_->status_[f.mpCoordinates_->camID_] = TRACKED; f.mpStatistics_->lastPatchUpdate_ = filterState.t_; - f.mpDistance_->p_ = medianDepthParameters[camID]; + f.mpDistance_->p_ = medianDepthParameters[f.mpCoordinates_->camID_]; const float initRelDepthCovTemp_ = initCovFeature_(0,0); initCovFeature_(0,0) = initRelDepthCovTemp_*pow(f.mpDistance_->getParameterDerivative()*f.mpDistance_->getDistance(),2); filterState.resetFeatureCovariance(*it,initCovFeature_); initCovFeature_(0,0) = initRelDepthCovTemp_; if(doFrameVisualisation_){ - f.mpCoordinates_->drawPoint(filterState.img_[camID], cv::Scalar(255,255,0)); //changed color from blue to aquamarin - f.mpCoordinates_->drawText(filterState.img_[camID],std::to_string(f.idx_),cv::Scalar(255,255,0)); //changed color from blue to aquamarin + f.mpCoordinates_->drawPoint(filterState.img_[f.mpCoordinates_->camID_], cv::Scalar(255,255,0)); //changed color from blue to aquamarin + f.mpCoordinates_->drawText(filterState.img_[f.mpCoordinates_->camID_],std::to_string(f.idx_),cv::Scalar(255,255,0)); //changed color from blue to aquamarin } + } + } else { + for(int camID = 0;camID newSet = filterState.fsm_.addBestCandidates(candidates_[camID],meas.aux().pyr_[camID],camID,filterState.t_, + endLevel_,startLevel_,(mtState::nMax_-filterState.fsm_.getValidCount())/(mtState::nCam_-camID),nDetectionBuckets_, scoreDetectionExponent_, + penaltyDistance_, zeroDistancePenalty_,false,minAbsoluteSTScore_); + const double t3 = (double) cv::getTickCount(); + if(verbose_) std::cout << "== Got " << filterState.fsm_.getValidCount() << " after adding " << newSet.size() << " features in camera " << camID << " (" << (t3-t2)/cv::getTickFrequency()*1000 << " ms)" << std::endl; + for(auto it = newSet.begin();it != newSet.end();++it){ + FeatureManager& f = filterState.fsm_.features_[*it]; + f.mpStatistics_->resetStatistics(filterState.t_); + f.mpStatistics_->status_[camID] = TRACKED; + f.mpStatistics_->lastPatchUpdate_ = filterState.t_; + f.mpDistance_->p_ = medianDepthParameters[camID]; + const float initRelDepthCovTemp_ = initCovFeature_(0,0); + initCovFeature_(0,0) = initRelDepthCovTemp_*pow(f.mpDistance_->getParameterDerivative()*f.mpDistance_->getDistance(),2); + filterState.resetFeatureCovariance(*it,initCovFeature_); + initCovFeature_(0,0) = initRelDepthCovTemp_; + if(doFrameVisualisation_){ + f.mpCoordinates_->drawPoint(filterState.img_[camID], cv::Scalar(255,255,0)); //changed color from blue to aquamarin + f.mpCoordinates_->drawText(filterState.img_[camID],std::to_string(f.idx_),cv::Scalar(255,255,0)); //changed color from blue to aquamarin + } - if(mtState::nCam_>1 && doStereoInitialization_){ - const int otherCam = (camID+1)%mtState::nCam_; - transformFeatureOutputCT_.setFeatureID(*it); - transformFeatureOutputCT_.setOutputCameraID(otherCam); - transformFeatureOutputCT_.transformState(filterState.state_,featureOutput_); - if(alignment_.align2DAdaptive(alignedCoordinates_,meas.aux().pyr_[otherCam],*f.mpMultilevelPatch_,featureOutput_.c(),startLevel_,endLevel_, - alignConvergencePixelRange_,alignCoverageRatio_,alignMaxUniSample_)){ - bool valid = mlpTemp1_.isMultilevelPatchInFrame(meas.aux().pyr_[otherCam],alignedCoordinates_,startLevel_,false); - if(valid && patchRejectionTh_ >= 0){ - mlpTemp1_.extractMultilevelPatchFromImage(meas.aux().pyr_[otherCam],alignedCoordinates_,startLevel_,false); - const float avgError = mlpTemp1_.computeAverageDifference(*f.mpMultilevelPatch_,endLevel_,startLevel_); - if(avgError > patchRejectionTh_){ - valid = false; - } - } - if(valid == true){ - if(doFrameVisualisation_){ - alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(150,0,0)); - alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(150,0,0)); + if(mtState::nCam_>1 && doStereoInitialization_){ + const int otherCam = (camID+1)%mtState::nCam_; + transformFeatureOutputCT_.setFeatureID(*it); + transformFeatureOutputCT_.setOutputCameraID(otherCam); + transformFeatureOutputCT_.transformState(filterState.state_,featureOutput_); + if(alignment_.align2DAdaptive(alignedCoordinates_,meas.aux().pyr_[otherCam],*f.mpMultilevelPatch_,featureOutput_.c(),startLevel_,endLevel_, + alignConvergencePixelRange_,alignCoverageRatio_,alignMaxUniSample_)){ + bool valid = mlpTemp1_.isMultilevelPatchInFrame(meas.aux().pyr_[otherCam],alignedCoordinates_,startLevel_,false); + if(valid && patchRejectionTh_ >= 0){ + mlpTemp1_.extractMultilevelPatchFromImage(meas.aux().pyr_[otherCam],alignedCoordinates_,startLevel_,false); + const float avgError = mlpTemp1_.computeAverageDifference(*f.mpMultilevelPatch_,endLevel_,startLevel_); + if(avgError > patchRejectionTh_){ + valid = false; + } } - if(f.mpCoordinates_->getDepthFromTriangulation(alignedCoordinates_,state.qCM(otherCam).rotate(V3D(state.MrMC(camID)-state.MrMC(otherCam))),state.qCM(otherCam)*state.qCM(camID).inverted(), *f.mpDistance_, 0.01)){ - filterState.resetFeatureCovariance(*it,initCovFeature_); // TODO: improve + if(valid == true){ + if(doFrameVisualisation_){ + alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(150,0,0)); + alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(150,0,0)); + } + if(f.mpCoordinates_->getDepthFromTriangulation(alignedCoordinates_,state.qCM(otherCam).rotate(V3D(state.MrMC(camID)-state.MrMC(otherCam))),state.qCM(otherCam)*state.qCM(camID).inverted(), *f.mpDistance_, 0.01)){ + filterState.resetFeatureCovariance(*it,initCovFeature_); // TODO: improve + } + } else { + if(doFrameVisualisation_){ + alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(0,0,150)); + alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(0,0,150)); + } } } else { if(doFrameVisualisation_){ - alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(0,0,150)); - alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(0,0,150)); + alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(0,150,0)); + alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(0,150,0)); } } - } else { - if(doFrameVisualisation_){ - alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(0,150,0)); - alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(0,150,0)); - } } } } From 6a6ad37fda37cf7b606557af813d90092ed3755b Mon Sep 17 00:00:00 2001 From: trleonie Date: Wed, 12 Dec 2018 14:30:39 +0100 Subject: [PATCH 19/33] addGlobalBest param added --- cfg/rovio_domi.info | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/cfg/rovio_domi.info b/cfg/rovio_domi.info index bd983f04..71cefd13 100644 --- a/cfg/rovio_domi.info +++ b/cfg/rovio_domi.info @@ -2,9 +2,9 @@ ; #include "/home/user/workspace/rovio/cfg/rovio_custom.info" Common { - doVECalibration true; Should the camera-IMU extrinsics be calibrated online + doVECalibration false; Should the camera-IMU extrinsics be calibrated online depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic) - verbose false; Is the verbose active + verbose true; Is the verbose active } Camera0 { @@ -153,6 +153,7 @@ ImgUpdate maxUncertaintyToDepthRatioForDepthInitialization 0.3; If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame useCrossCameraMeasurements false; Should cross measurements between frame be used. Might be turned of in calibration phase. doStereoInitialization false; Should a stereo match be used for feature initialization. + addGlobalBest true; Should the best features of all cameras be added (true) or the best of each camera (false) noiseGainForOffCamera 10.0; Factor added on update noise if not main camera discriminativeSamplingDistance 0.02; Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed). discriminativeSamplingGain 1.1; Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used). From 08bc0ac8359b6536ef6d584ec869fd3ed698028d Mon Sep 17 00:00:00 2001 From: trleonie Date: Wed, 12 Dec 2018 15:24:44 +0100 Subject: [PATCH 20/33] new param for info file: HE, HC, addGlobal, showCandidates --- cfg/rovio_domi.info | 6 +++++- include/rovio/FeatureManager.hpp | 6 +----- include/rovio/ImgUpdate.hpp | 10 +++++++++- include/rovio/RovioFilter.hpp | 1 + include/rovio/RovioNode.hpp | 7 ++----- 5 files changed, 18 insertions(+), 12 deletions(-) diff --git a/cfg/rovio_domi.info b/cfg/rovio_domi.info index 71cefd13..4813100c 100644 --- a/cfg/rovio_domi.info +++ b/cfg/rovio_domi.info @@ -5,6 +5,7 @@ Common doVECalibration false; Should the camera-IMU extrinsics be calibrated online depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic) verbose true; Is the verbose active + healthCheck true; } Camera0 { @@ -110,6 +111,7 @@ ImgUpdate maxNumIteration 20; doPatchWarping true; Should the patches be warped doFrameVisualisation true; Should the frames be visualized + showCandidates false; Should the possible feature candidates be displayed visualizePatches false; Should the patches be visualized useDirectMethod true; Should the EKF-innovation be based on direct intensity error (o.w. reprojection error) startLevel 2; Highest patch level which is being employed (must be smaller than the hardcoded template parameter) @@ -153,7 +155,6 @@ ImgUpdate maxUncertaintyToDepthRatioForDepthInitialization 0.3; If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame useCrossCameraMeasurements false; Should cross measurements between frame be used. Might be turned of in calibration phase. doStereoInitialization false; Should a stereo match be used for feature initialization. - addGlobalBest true; Should the best features of all cameras be added (true) or the best of each camera (false) noiseGainForOffCamera 10.0; Factor added on update noise if not main camera discriminativeSamplingDistance 0.02; Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed). discriminativeSamplingGain 1.1; Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used). @@ -176,6 +177,9 @@ ImgUpdate minNoMotionTime 1.0; Min duration of no-motion isEnabled 0; Should zero velocity update be applied, only works if MotionDetection.isEnabled is true } + + addGlobalBest true; Should the best features of all cameras be added (true) or the best of each camera (false) + histogramEqualize true; } Prediction { diff --git a/include/rovio/FeatureManager.hpp b/include/rovio/FeatureManager.hpp index dd425dd2..4c50b5fe 100644 --- a/include/rovio/FeatureManager.hpp +++ b/include/rovio/FeatureManager.hpp @@ -496,11 +496,7 @@ class FeatureSetManager{ } } - - - - // Check Buckets for overall best features - + //// Check Buckets for overall best features // Incrementally add features and update candidate buckets (Check distance of candidates with respect to the newly inserted feature). int addedCount = 0; std::vector< std::vector > nfCandidates(2, std::vector(nCam)); diff --git a/include/rovio/ImgUpdate.hpp b/include/rovio/ImgUpdate.hpp index 85e32936..76e2800b 100644 --- a/include/rovio/ImgUpdate.hpp +++ b/include/rovio/ImgUpdate.hpp @@ -209,8 +209,10 @@ ImgOutlierDetection,false>{ bool useDirectMethod_; /**,false>{ bool useCrossCameraMeasurements_; /**,false>{ zeroDistancePenalty_ = nDetectionBuckets_*1.0; useDirectMethod_ = true; doFrameVisualisation_ = true; + showCandidates_ = false; visualizePatches_ = false; verbose_ = false; + healthCheck_ = false; trackingUpperBound_ = 0.9; trackingLowerBound_ = 0.1; minTrackedAndFreeFeatures_ = 0.5; @@ -308,6 +313,7 @@ ImgOutlierDetection,false>{ useCrossCameraMeasurements_ = true; doStereoInitialization_ = true; addGlobalBest_ = false; + histogramEqualize_ = false; removalFactor_ = 1.1; alignmentGaussianWeightingSigma_ = 2.0; discriminativeSamplingDistance_ = 0.0; @@ -342,11 +348,13 @@ ImgOutlierDetection,false>{ boolRegister_.registerScalar("MotionDetection.isEnabled",doVisualMotionDetection_); boolRegister_.registerScalar("useDirectMethod",useDirectMethod_); boolRegister_.registerScalar("doFrameVisualisation",doFrameVisualisation_); + boolRegister_.registerScalar("showCandidates",showCandidates_); boolRegister_.registerScalar("visualizePatches",visualizePatches_); boolRegister_.registerScalar("removeNegativeFeatureAfterUpdate",removeNegativeFeatureAfterUpdate_); boolRegister_.registerScalar("useCrossCameraMeasurements",useCrossCameraMeasurements_); boolRegister_.registerScalar("doStereoInitialization",doStereoInitialization_); boolRegister_.registerScalar("addGlobalBest",addGlobalBest_); + boolRegister_.registerScalar("histogramEqualize",histogramEqualize_); boolRegister_.registerScalar("useIntensityOffsetForAlignment",alignment_.useIntensityOffset_); boolRegister_.registerScalar("useIntensitySqewForAlignment",alignment_.useIntensitySqew_); doubleRegister_.removeScalarByVar(updnoiP_(0,0)); @@ -999,7 +1007,7 @@ ImgOutlierDetection,false>{ if(verbose_) std::cout << "== Detected " << candidates_[camID].size() << " candidates in Camera" << camID << " on levels " << endLevel_ << "-" << startLevel_ << " (" << (t2-t1)/cv::getTickFrequency()*1000 << " ms)" << std::endl; // visualization of detected candidates - if(doFrameVisualisation_){ + if(doFrameVisualisation_ && showCandidates_){ for(int i=0;i, std::get<0>(mUpdates_).outlierDetection_.setEnabledAll(true); std::get<1>(mUpdates_).outlierDetection_.setEnabledAll(true); boolRegister_.registerScalar("Common.verbose",std::get<0>(mUpdates_).verbose_); + boolRegister_.registerScalar("Common.healthCheck",std::get<0>(mUpdates_).healthCheck_); mPrediction_.doubleRegister_.removeScalarByStr("alpha"); mPrediction_.doubleRegister_.removeScalarByStr("beta"); mPrediction_.doubleRegister_.removeScalarByStr("kappa"); diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index 76b70ead..66f2571c 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -128,9 +128,6 @@ class RovioNode{ bool forceMarkersPublishing_; bool forcePatchPublishing_; bool gotFirstMessages_; - - bool histogram_equalize_ = true; - bool healthCheck_ = false; std::mutex m_filter_; @@ -554,7 +551,7 @@ class RovioNode{ cv_ptr->image.copyTo(cv_img); // Histogram equalization - if(histogram_equalize_){ + if(mpImgUpdate_->histogramEqualize_){ constexpr size_t hist_bins = 256; cv::Mat hist; std::vector img_vec = {cv_img}; @@ -1082,7 +1079,7 @@ class RovioNode{ } gotFirstMessages_ = true; - if (healthCheck_) { + if(mpImgUpdate_->healthCheck_){ if(healthMonitor_.shouldResetEstimator(featureDistanceCov, imuOutput_)) { if(!init_state_.isInitialized()) { std::cout << "Reinitioalization already triggered. Ignoring request..."; From 3039c538bc08a9114042d25f4a1e2b1e19fe344d Mon Sep 17 00:00:00 2001 From: trleonie Date: Thu, 31 Jan 2019 15:51:41 +0100 Subject: [PATCH 21/33] updated camera order to match order in rovio_domi.launch --- cfg/rovio_domi.info | 70 ++++++++++++++++++++-------------------- launch/rovio_domi.launch | 8 ++--- 2 files changed, 39 insertions(+), 39 deletions(-) diff --git a/cfg/rovio_domi.info b/cfg/rovio_domi.info index 4813100c..c1911b41 100644 --- a/cfg/rovio_domi.info +++ b/cfg/rovio_domi.info @@ -4,43 +4,21 @@ Common { doVECalibration false; Should the camera-IMU extrinsics be calibrated online depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic) - verbose true; Is the verbose active - healthCheck true; + verbose false; Is the verbose active + healthCheck false; } Camera0 { - CalibrationFile ; Camera-Calibration file for intrinsics - qCM_x -0.0141053692505; X-entry of IMU to Camera quaterion (Hamilton) - qCM_y -0.00445598914788; Y-entry of IMU to Camera quaterion (Hamilton) - qCM_z 0.736220553648; Z-entry of IMU to Camera quaterion (Hamilton) - qCM_w 0.676579987219; W-entry of IMU to Camera quaterion (Hamilton) - MrMC_x -0.013725561541; X-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_y 0.0458722860308; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_z 0.00949435362177; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] -} -Camera1 -{ - CalibrationFile ; Camera-Calibration file for intrinsics - qCM_x -0.00953751922809; X-entry of IMU to Camera quaterion (Hamilton) - qCM_y -0.703628160853; Y-entry of IMU to Camera quaterion (Hamilton) - qCM_z -0.710497111443; Z-entry of IMU to Camera quaterion (Hamilton) - qCM_w 0.00320961269892; W-entry of IMU to Camera quaterion (Hamilton) - MrMC_x 0.0341624938784; X-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_y 0.0489333125579; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_z -0.0250547932613; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x -0.0141053692505; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.00445598914788; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z 0.736220553648; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.676579987219; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x -0.013725561541; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0458722860308; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z 0.00949435362177; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] } Camera2 -{ - CalibrationFile ; Camera-Calibration file for intrinsics - qCM_x 0.498284808832; X-entry of IMU to Camera quaterion (Hamilton) - qCM_y -0.500933590992; Y-entry of IMU to Camera quaterion (Hamilton) - qCM_z 0.506194778687; Z-entry of IMU to Camera quaterion (Hamilton) - qCM_w 0.494514542489; W-entry of IMU to Camera quaterion (Hamilton) - MrMC_x 0.0266940973582; X-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_y 0.0446798994558; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_z -0.00861710075576; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] -} -Camera3 { CalibrationFile ; Camera-Calibration file for intrinsics qCM_x -0.717540995787; X-entry of IMU to Camera quaterion (Hamilton) @@ -51,6 +29,17 @@ Camera3 MrMC_y -0.0480190261945; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] MrMC_z -0.0324906232515; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] } +Camera3 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x 0.498284808832; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.500933590992; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z 0.506194778687; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.494514542489; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x 0.0266940973582; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0446798994558; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.00861710075576; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} Camera4 { CalibrationFile ; Camera-Calibration file for intrinsics @@ -62,6 +51,17 @@ Camera4 MrMC_y 0.042329395001; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] MrMC_z -0.00623084134628; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] } +Camera1 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x -0.00953751922809; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.703628160853; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z -0.710497111443; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.00320961269892; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x 0.0341624938784; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0489333125579; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.0250547932613; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} Init { @@ -111,7 +111,7 @@ ImgUpdate maxNumIteration 20; doPatchWarping true; Should the patches be warped doFrameVisualisation true; Should the frames be visualized - showCandidates false; Should the possible feature candidates be displayed + showCandidates false; Should the possible feature candidates be displayed visualizePatches false; Should the patches be visualized useDirectMethod true; Should the EKF-innovation be based on direct intensity error (o.w. reprojection error) startLevel 2; Highest patch level which is being employed (must be smaller than the hardcoded template parameter) @@ -178,8 +178,8 @@ ImgUpdate isEnabled 0; Should zero velocity update be applied, only works if MotionDetection.isEnabled is true } - addGlobalBest true; Should the best features of all cameras be added (true) or the best of each camera (false) - histogramEqualize true; + addGlobalBest false; Should the best features of all cameras be added (true) or the best of each camera (false) + histogramEqualize false; } Prediction { diff --git a/launch/rovio_domi.launch b/launch/rovio_domi.launch index 7925e092..1f8636a6 100644 --- a/launch/rovio_domi.launch +++ b/launch/rovio_domi.launch @@ -4,14 +4,14 @@ - - + + - - + + From a0cfab901802b85a28cb5e7fcff0d128b588154d Mon Sep 17 00:00:00 2001 From: trleonie Date: Thu, 31 Jan 2019 15:54:41 +0100 Subject: [PATCH 22/33] clean up: removal of old files --- launch/rovio_M600.launch | 18 ------------------ launch/rovio_mavros_rs.launch | 21 --------------------- 2 files changed, 39 deletions(-) delete mode 100644 launch/rovio_M600.launch delete mode 100644 launch/rovio_mavros_rs.launch diff --git a/launch/rovio_M600.launch b/launch/rovio_M600.launch deleted file mode 100644 index 7cad7de1..00000000 --- a/launch/rovio_M600.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/rovio_mavros_rs.launch b/launch/rovio_mavros_rs.launch deleted file mode 100644 index bc8988e6..00000000 --- a/launch/rovio_mavros_rs.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - x - - - - - - - - - - \ No newline at end of file From ffd0564750d0dab52d95aaaa159998c2f6dcb9fc Mon Sep 17 00:00:00 2001 From: trleonie Date: Fri, 1 Feb 2019 14:36:24 +0100 Subject: [PATCH 23/33] clean up remove old files --- cfg/M600-rs_D435_down_intel.yaml | 13 -- cfg/calibration.yaml | 72 ---------- cfg/rovio_M600.info | 232 ------------------------------- cfg/rs_color_D415_intel.yaml | 13 -- cfg/rs_color_D415_kalibr.yaml | 14 -- cfg/rs_color_D435_intel.yaml | 13 -- launch/rovio_node_orig.launch | 8 -- launch/rovio_rosbag_node.launch | 12 -- 8 files changed, 377 deletions(-) delete mode 100644 cfg/M600-rs_D435_down_intel.yaml delete mode 100644 cfg/calibration.yaml delete mode 100644 cfg/rovio_M600.info delete mode 100644 cfg/rs_color_D415_intel.yaml delete mode 100644 cfg/rs_color_D415_kalibr.yaml delete mode 100644 cfg/rs_color_D435_intel.yaml delete mode 100644 launch/rovio_node_orig.launch delete mode 100644 launch/rovio_rosbag_node.launch diff --git a/cfg/M600-rs_D435_down_intel.yaml b/cfg/M600-rs_D435_down_intel.yaml deleted file mode 100644 index c0160be8..00000000 --- a/cfg/M600-rs_D435_down_intel.yaml +++ /dev/null @@ -1,13 +0,0 @@ -image_width: 640 -image_height: 480 -camera_name: camr -camera_matrix: - rows: 3 - cols: 3 - data: [618.293701172, 0.0, 323.747528076, 0.0, 618.570556641, 239.608337402, 0.0, 0.0, 1.0] -distortion_model: plumb_bob -distortion_coefficients: - rows: 1 - cols: 5 - data: [0.0, 0.0, 0.0, 0.0, 0.0] - diff --git a/cfg/calibration.yaml b/cfg/calibration.yaml deleted file mode 100644 index 074d2148..00000000 --- a/cfg/calibration.yaml +++ /dev/null @@ -1,72 +0,0 @@ -cam0: - T_cam_imu: - - [-0.08408111890742742, -0.9960984788103229, -0.02679899167946961, 0.04479369286657575] - - [0.9963498922995513, -0.0844393301122407, 0.012525639499406719, 0.017429964015559494] - - [-0.014739659356583449, -0.025648002689434925, 0.9995623654380427, -0.008515976150586862] - - [0.0, 0.0, 0.0, 1.0] - cam_overlaps: [] - camera_model: pinhole - distortion_coeffs: [-0.05242378016082595, -0.005452250979470613, -0.002750824731857844, - 0.001176879627814984] - distortion_model: equidistant - intrinsics: [393.01620151949186, 392.35754010306675, 360.133022189131, 239.82750356152488] - resolution: [752, 480] - rostopic: /uvc_camera/cam_0/image_raw -cam1: - T_cam_imu: - - [0.9996318198325032, -0.008930485458513018, -0.025621694085903285, -0.03360089532815607] - - [0.025343025375257167, -0.030032942880376667, 0.9992275783858109, 0.030203489020421376] - - [-0.009693082233500086, -0.9995090138520102, -0.029795559826848173, -0.04864993999452485] - - [0.0, 0.0, 0.0, 1.0] - cam_overlaps: [] - camera_model: pinhole - distortion_coeffs: [-0.05775660779258553, -0.016321098687079805, 0.010851033712369944, - -0.003727782103089848] - distortion_model: equidistant - intrinsics: [399.3480570111828, 398.3491316285275, 386.46344514949317, 249.82118871166142] - resolution: [752, 480] - rostopic: /uvc_camera/cam_1/image_raw -cam2: - T_cam_imu: - - [-0.014335233107286627, -0.9998565560354433, 0.009020445926071874, 0.045133886593555224] - - [0.0014261615362032298, -0.009041809365503639, -0.9999581049957411, -0.008250822703834645] - - [0.9998962281932238, -0.014321767919615314, 0.0015555734069045424, -0.026038027580066486] - - [0.0, 0.0, 0.0, 1.0] - cam_overlaps: [] - camera_model: pinhole - distortion_coeffs: [-0.06456227781767035, -0.013988324416174668, 0.016058120497332448, - -0.007586776661876683] - distortion_model: equidistant - intrinsics: [401.6472159834467, 400.20768312959996, 364.5763065095566, 237.33091443088992] - resolution: [752, 480] - rostopic: /uvc_camera/cam_2/image_raw -cam3: - T_cam_imu: - - [-0.005018093185921224, 0.9992645940012366, 0.03801433829911055, -0.04238657606191231] - - [0.004042825346933565, 0.03803477912356312, -0.9992682378321819, -0.0075742965840363006] - - [-0.9999792369364424, -0.00486073580475066, -0.0042307142948694865, -0.06461910888733008] - - [0.0, 0.0, 0.0, 1.0] - cam_overlaps: [] - camera_model: pinhole - distortion_coeffs: [-0.05606253878969147, -0.014037695708003596, 0.006384058502673369, - -0.001519392296560513] - distortion_model: equidistant - intrinsics: [398.9944782000244, 398.0805787425024, 364.4159902096, 275.47777743169365] - resolution: [752, 480] - rostopic: /uvc_camera/cam_3/image_raw -cam4: - T_cam_imu: - - [-0.9997974682265937, 0.017982575329980252, 0.00903601196300318, 0.03350202332038546] - - [0.008860893124254645, -0.009794219283195392, 0.9999127751167467, 0.02522916124620373] - - [0.01806950748455773, 0.9997903281454241, 0.009632893964497026, -0.04929900189157067] - - [0.0, 0.0, 0.0, 1.0] - cam_overlaps: [] - camera_model: pinhole - distortion_coeffs: [-0.058690582862360736, -0.004194002012911134, -0.0014205680884277923, - 0.00032045972552648583] - distortion_model: equidistant - intrinsics: [400.1030703837698, 399.06815731220064, 371.4006422531301, 248.04717800715062] - resolution: [752, 480] - rostopic: /uvc_camera/cam_4/image_raw - - diff --git a/cfg/rovio_M600.info b/cfg/rovio_M600.info deleted file mode 100644 index 4f7220f1..00000000 --- a/cfg/rovio_M600.info +++ /dev/null @@ -1,232 +0,0 @@ -; You can partially override parameter set in this file by creating your own subset of parameter in a separate info-file and include it with: -; #include "/home/user/workspace/rovio/cfg/rovio_custom.info" -Common -{ - doVECalibration true; Should the camera-IMU extrinsics be calibrated online - depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic) - verbose false; Is the verbose active -} -Camera0 -{ - CalibrationFile ; Camera-Calibration file for intrinsics - qCM_x -0.712595441024; X-entry of IMU to Camera quaterion (Hamilton) - qCM_y 0.701298215193; Y-entry of IMU to Camera quaterion (Hamilton) - qCM_z 0.0196987073794; Z-entry of IMU to Camera quaterion (Hamilton) - qCM_w 0.000715350070233; W-entry of IMU to Camera quaterion (Hamilton) - MrMC_x 0.135011088808; X-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_y 0.0301838079241; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_z -0.149016900074; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] -} -Camera1 -{ - CalibrationFile ; Camera-Calibration file for intrinsics - qCM_x 0.00151329637706; X-entry of IMU to Camera quaterion (Hamilton) - qCM_y -0.0123329249764; Y-entry of IMU to Camera quaterion (Hamilton) - qCM_z -0.702657352863; Z-entry of IMU to Camera quaterion (Hamilton) - qCM_w 0.711419885414; W-entry of IMU to Camera quaterion (Hamilton) - MrMC_x -0.0091929160801; X-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_y 0.0540646643676; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_z 0.0190387660614; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] -} -Init -{ - State - { - pos_0 0; X-entry of initial position (world to IMU in world) [m] - pos_1 0; Y-entry of initial position (world to IMU in world) [m] - pos_2 0; Z-entry of initial position (world to IMU in world) [m] - vel_0 0; X-entry of initial velocity (robocentric, IMU) [m/s] - vel_1 0; Y-entry of initial velocity (robocentric, IMU) [m/s] - vel_2 0; Z-entry of initial velocity (robocentric, IMU) [m/s] - acb_0 0; X-entry of accelerometer bias [m/s^2] - acb_1 0; Y-entry of accelerometer bias [m/s^2] - acb_2 0; Z-entry of accelerometer bias [m/s^2] - gyb_0 0; X-entry of gyroscope bias [rad/s] - gyb_1 0; Y-entry of gyroscope bias [rad/s] - gyb_2 0; Z-entry of gyroscope bias [rad/s] - att_x 0; X-entry of initial attitude (IMU to world, Hamilton) - att_y 0; Y-entry of initial attitude (IMU to world, Hamilton) - att_z 0; Z-entry of initial attitude (IMU to world, Hamilton) - att_w 1; W-entry of initial attitude (IMU to world, Hamilton) - } - Covariance - { - pos_0 0.0001; X-Covariance of initial position [m^2] - pos_1 0.0001; Y-Covariance of initial position [m^2] - pos_2 0.0001; Z-Covariance of initial position [m^2] - vel_0 1.0; X-Covariance of initial velocity [m^2/s^2] - vel_1 1.0; Y-Covariance of initial velocity [m^2/s^2] - vel_2 1.0; Z-Covariance of initial velocity [m^2/s^2] - acb_0 4e-4; X-Covariance of initial accelerometer bias [m^2/s^4] - acb_1 4e-4; Y-Covariance of initial accelerometer bias [m^2/s^4] - acb_2 4e-4; Z-Covariance of initial accelerometer bias [m^2/s^4] - gyb_0 3e-4; X-Covariance of initial gyroscope bias [rad^2/s^2] - gyb_1 3e-4; Y-Covariance of initial gyroscope bias [rad^2/s^2] - gyb_2 3e-4; Z-Covariance of initial gyroscope bias [rad^2/s^2] - vep 0.0001; Covariance of initial linear camera-IMU extrinsics, same for all entries [m^2] - att_0 0.1; X-Covariance of initial attitude [rad^2] - att_1 0.1; Y-Covariance of initial attitude [rad^2] - att_2 0.1; Z-Covariance of initial attitude [rad^2] - vea 0.01; Covariance of initial rotational camera-IMU extrinsics, same for all entries [rad^2] - } -} -ImgUpdate -{ - updateVecNormTermination 1e-4; - maxNumIteration 20; - doPatchWarping true; Should the patches be warped - doFrameVisualisation true; Should the frames be visualized - visualizePatches false; Should the patches be visualized - useDirectMethod true; Should the EKF-innovation be based on direct intensity error (o.w. reprojection error) - startLevel 2; Highest patch level which is being employed (must be smaller than the hardcoded template parameter) - endLevel 1; Lowest patch level which is being employed - nDetectionBuckets 100; Number of discretization buckets used during the candidates selection - MahalanobisTh 9.21; Mahalanobis treshold for the update, 5.8858356 - UpdateNoise - { - pix 2; Covariance used for the reprojection error, 1/focal_length is roughly 1 pixel std [rad] (~1/f ~ 1/400^2 = 1/160000) - int 400; Covariance used for the photometric error [intensity^2] - } - initCovFeature_0 0.5; Covariance for the initial distance (Relative to initialization depth [m^2/m^2]) - initCovFeature_1 1e-5; Covariance for the initial bearing vector, x-component [rad^2] - initCovFeature_2 1e-5; Covariance for the initial bearing vector, y-component [rad^2] - initDepth 0.5; Initial value for the initial distance parameter - startDetectionTh 0.8; Threshold for detecting new features (min: 0, max: 1) - scoreDetectionExponent 0.25; Exponent used for weighting the distance between candidates - penaltyDistance 100; Maximal distance which gets penalized during bucketing [pix] - zeroDistancePenalty 100; Penalty for zero distance (max: nDetectionBuckets) - statLocalQualityRange 10; Number of frames for local quality evaluation - statLocalVisibilityRange 100; Number of frames for local visibility evaluation - statMinGlobalQualityRange 100; Minimum number of frames for obtaining maximal global quality - trackingUpperBound 0.9; Threshold for local quality for min overall global quality - trackingLowerBound 0.8; Threshold for local quality for max overall global quality - minTrackedAndFreeFeatures 0.75; Minimum of amount of feature which are either tracked or free - removalFactor 1.1; Factor for enforcing feature removal if not enough free - minRelativeSTScore 0.75; Minimum relative ST-score for extracting new feature patch - minAbsoluteSTScore 5.0; Minimum absolute ST-score for extracting new feature patch - minTimeBetweenPatchUpdate 1.0; Minimum time between new multilevel patch extrection [s] - fastDetectionThreshold 5; Fast corner detector treshold while adding new feature - alignConvergencePixelRange 10; Assumed convergence range for image alignment (gets scaled with the level) [pixels] - alignCoverageRatio 2; How many sigma of the uncertainty should be covered in the adaptive alignement - alignMaxUniSample 1; Maximal number of alignment seeds on one side -> total number of sample = 2n+1. Carefull can get very costly if diverging! - patchRejectionTh 50.0; If the average itensity error is larger than this the feauture is rejected [intensity], if smaller 0 the no check is performed - alignmentHuberNormThreshold 10; Intensity error threshold for Huber norm (enabled if > 0) - alignmentGaussianWeightingSigma -1; Width of Gaussian which is used for pixel error weighting (enabled if > 0) - alignmentGradientExponent 0.0; Exponent used for gradient based weighting of residuals - useIntensityOffsetForAlignment true; Should an intensity offset between the patches be considered - useIntensitySqewForAlignment true; Should an intensity sqew between the patches be considered - removeNegativeFeatureAfterUpdate true; Should feature with negative distance get removed - maxUncertaintyToDepthRatioForDepthInitialization 0.3; If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame - useCrossCameraMeasurements true; Should cross measurements between frame be used. Might be turned of in calibration phase. - doStereoInitialization true; Should a stereo match be used for feature initialization. - noiseGainForOffCamera 10.0; Factor added on update noise if not main camera - discriminativeSamplingDistance 0.02; Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed). - discriminativeSamplingGain 1.1; Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used). - MotionDetection - { - isEnabled 0; Is the motion detection enabled - rateOfMovingFeaturesTh 0.5; Amount of feature with motion for overall motion detection - pixelCoordinateMotionTh 1.0; Threshold for motion detection for patched [pixels] - minFeatureCountForNoMotionDetection 5; Min feature count in frame for motion detection - } - ZeroVelocityUpdate - { - UpdateNoise - { - vel_0 0.01; X-Covariance of zero velocity update [m^2/s^2] - vel_1 0.01; Y-Covariance of zero velocity update [m^2/s^2] - vel_2 0.01; Z-Covariance of zero velocity update [m^2/s^2] - } - MahalanobisTh0 7.689997599999999; Mahalanobid distance for zero velocity updates - minNoMotionTime 1.0; Min duration of no-motion - isEnabled 0; Should zero velocity update be applied, only works if MotionDetection.isEnabled is true - } -} -Prediction -{ - PredictionNoise - { - pos_0 1e-4; X-covariance parameter of position prediction [m^2/s] - pos_1 1e-4; Y-covariance parameter of position prediction [m^2/s] - pos_2 1e-4; Z-covariance parameter of position prediction [m^2/s] - vel_0 4e-5; 4e-6 X-covariance parameter of velocity prediction [m^2/s^3] - vel_1 4e-5; Y-covariance parameter of velocity prediction [m^2/s^3] - vel_2 4e-5; Z-covariance parameter of velocity prediction [m^2/s^3] - acb_0 9e-6; X-covariance parameter of accelerometer bias prediction [m^2/s^5] - acb_1 9e-6; Y-covariance parameter of accelerometer bias prediction [m^2/s^5] - acb_2 9e-6; Z-covariance parameter of accelerometer bias prediction [m^2/s^5] - gyb_0 2e-8; X-covariance parameter of gyroscope bias prediction [rad^2/s^3] - gyb_1 2e-8; Y-covariance parameter of gyroscope bias prediction [rad^2/s^3] - gyb_2 2e-8; Z-covariance parameter of gyroscope bias prediction [rad^2/s^3] - vep 1e-6; Covariance parameter of linear extrinsics prediction [m^2/s] - att_0 7.6e-6; 7.6e-7 X-Covariance parameter of attitude prediction [rad^2/s] - att_1 7.6e-6; Y-Covariance parameter of attitude prediction [rad^2/s] - att_2 7.6e-6; Z-Covariance parameter of attitude prediction [rad^2/s] - vea 1e-6; Covariance parameter of rotational extrinsics prediction [rad^2/s] - dep 0.0001; Covariance parameter of distance prediction [m^2/s] - nor 0.00001; Covariance parameter of bearing vector prediction [rad^2/s] - - - } - MotionDetection - { - inertialMotionRorTh 0.1; Treshold on rotational rate for motion detection [rad/s] - inertialMotionAccTh 0.5; Treshold on acceleration for motion detection [m/s^2] - } -} -PoseUpdate -{ - UpdateNoise - { - pos_0 0.01; X-Covariance of linear pose measurement update [m^2] - pos_1 0.01; Y-Covariance of linear pose measurement update [m^2] - pos_2 0.01; Z-Covariance of linear pose measurement update [m^2] - att_0 0.01; X-Covariance of rotational pose measurement update [rad^2] - att_1 0.01; Y-Covariance of rotational pose measurement update [rad^2] - att_2 0.01; Z-Covariance of rotational pose measurement update [rad^2] - } - init_cov_IrIW 1; Covariance of initial pose between inertial frames, linear part [m^2] - init_cov_qWI 1; Covariance of initial pose between inertial frames, rotational part [rad^2] - init_cov_MrMV 1; Covariance of initial pose between inertial frames, linear part [m^2] - init_cov_qVM 1; Covariance of initial pose between inertial frames, rotational part [rad^2] - pre_cov_IrIW 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s] - pre_cov_qWI 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s] - pre_cov_MrMV 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s] - pre_cov_qVM 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s] - MahalanobisTh0 12.6511204; Mahalonobis distance treshold of pose measurement - doVisualization false; Should the measured poses be vizualized - enablePosition true; Should the linear part be used during the update - enableAttitude true; Should the rotation part be used during the update (e.g. set to fals eif feeding GPS-measurement) - noFeedbackToRovio true; By default the pose update is only used for aligning coordinate frame. Set to false if ROVIO should benefit frome the posed measurements. - doInertialAlignmentAtStart true; Should the transformation between I and W be explicitly computed and set with the first pose measurement. - timeOffset 0.0; Time offset added to the pose measurement timestamps - useOdometryCov false; Should the UpdateNoise position covariance be scaled using the covariance in the Odometry message - qVM_x 0; X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) - qVM_y 0; Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) - qVM_z 0; Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) - qVM_w -1; W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton) - MrMV_x 0; X-entry of vector representing IMU to reference body coordinate frame of pose measurement - MrMV_y 0; Y-entry of vector representing IMU to reference body coordinate frame of pose measurement - MrMV_z 0; Z-entry of vector representing IMU to reference body coordinate frame of pose measurement - qWI_x 0; X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) - qWI_y 0; Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) - qWI_z 0; Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) - qWI_w -1; W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton) - IrIW_x 0; X-entry of vector representing World to reference inertial coordinate frame of pose measurement - IrIW_y 0; Y-entry of vector representing World to reference inertial coordinate frame of pose measurement - IrIW_z 0; Z-entry of vector representing World to reference inertial coordinate frame of pose measurement -} -VelocityUpdate -{ - UpdateNoise - { - vel_0 0.0001 - vel_1 0.0001 - vel_2 0.0001 - } - MahalanobisTh0 7.689997599999999 - qAM_x 0 - qAM_y 0 - qAM_z 0 - qAM_w 1 -} \ No newline at end of file diff --git a/cfg/rs_color_D415_intel.yaml b/cfg/rs_color_D415_intel.yaml deleted file mode 100644 index ab05e856..00000000 --- a/cfg/rs_color_D415_intel.yaml +++ /dev/null @@ -1,13 +0,0 @@ -###### Camera Calibration File For Cam 0 of Euroc Datasets ###### -image_width: 640 -image_height: 480 -camera_name: camera ## RealSense D415 rgb module -camera_matrix: - rows: 3 - cols: 3 - data: [616.1016235351562, 0.0 , 319.61151123046875, 0.0, 615.8966064453125, 232.68994140625, 0.0, 0.0, 1.0] -distortion_model: equidistant -distortion_coefficients: - rows: 1 - cols: 4 - data: [0, 0, 0, 0] \ No newline at end of file diff --git a/cfg/rs_color_D415_kalibr.yaml b/cfg/rs_color_D415_kalibr.yaml deleted file mode 100644 index 7eaaa1f0..00000000 --- a/cfg/rs_color_D415_kalibr.yaml +++ /dev/null @@ -1,14 +0,0 @@ -###### Camera Calibration File For Cam 0 of Euroc Datasets ###### -image_width: 640 -image_height: 480 -camera_name: camera ## RealSense D415 rgb module -camera_matrix: - rows: 3 - cols: 3 - data: [602.6682976548108, 0.0 , 323.09018740329157, 0.0, 603.0133127786867, 228.72219305730522, 0.0, 0.0, 1.0] -distortion_model: equidistant -distortion_coefficients: - rows: 1 - cols: 4 - data: [0.1043930758106389, -0.2434771945266034, -0.0013695830857340307, - 0.0017089625472838964] \ No newline at end of file diff --git a/cfg/rs_color_D435_intel.yaml b/cfg/rs_color_D435_intel.yaml deleted file mode 100644 index 46a1769c..00000000 --- a/cfg/rs_color_D435_intel.yaml +++ /dev/null @@ -1,13 +0,0 @@ -###### Camera Calibration File For the RealSense D435 given by Intel ###### -image_width: 640 -image_height: 480 -camera_name: camera ## RealSense D435 rgb module -camera_matrix: - rows: 3 - cols: 3 - data: [618.293701171875, 0.0 , 323.7475280761719, 0.0, 618.570556640625, 239.60833740234375, 0.0, 0.0, 1.0] -distortion_model: equidistant -distortion_coefficients: - rows: 1 - cols: 4 - data: [0, 0, 0, 0] \ No newline at end of file diff --git a/launch/rovio_node_orig.launch b/launch/rovio_node_orig.launch deleted file mode 100644 index b2870740..00000000 --- a/launch/rovio_node_orig.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/launch/rovio_rosbag_node.launch b/launch/rovio_rosbag_node.launch deleted file mode 100644 index e9cfb34b..00000000 --- a/launch/rovio_rosbag_node.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - From f1de7bcea82c73cc0f4234ab218fae45ba4e1385 Mon Sep 17 00:00:00 2001 From: trleonie Date: Tue, 23 Oct 2018 09:59:59 +0200 Subject: [PATCH 24/33] clean up --- cfg/imgui.ini | 40 ---------------------------------------- 1 file changed, 40 deletions(-) delete mode 100644 cfg/imgui.ini diff --git a/cfg/imgui.ini b/cfg/imgui.ini deleted file mode 100644 index 4599d85a..00000000 --- a/cfg/imgui.ini +++ /dev/null @@ -1,40 +0,0 @@ -[Debug] -Pos=60,60 -Size=400,400 -Collapsed=0 - -[Splash Screen Banner] -Pos=746,574 -Size=1792,1008 -Collapsed=0 - -[nostreaming_popup] -Pos=916,454 -Size=1452,50 -Collapsed=0 - -[Notification parent window] -Pos=60,60 -Size=32,32 -Collapsed=0 - -[Stream of 0] -Pos=357,53 -Size=690,32 -Collapsed=0 - -[Stream of 1] -Pos=357,477 -Size=690,32 -Collapsed=0 - -[numbered_ruler] -Pos=1018,103 -Size=32,358 -Collapsed=0 - -[Stream of 3] -Pos=1083,53 -Size=690,32 -Collapsed=0 - From 0c91a535cf82b95a3a24bc3d3d4f6d2fa852697e Mon Sep 17 00:00:00 2001 From: trleonie Date: Tue, 20 Nov 2018 11:34:41 +0100 Subject: [PATCH 25/33] up to five cam setup --- cfg/rovio_domi.info | 23 +++++++++++++++++++ include/rovio/RovioNode.hpp | 29 +++++++++++++++++++++++- launch/rovio_domi.launch | 44 +++++++++++++++++++++++-------------- 3 files changed, 79 insertions(+), 17 deletions(-) diff --git a/cfg/rovio_domi.info b/cfg/rovio_domi.info index 88922a28..bd983f04 100644 --- a/cfg/rovio_domi.info +++ b/cfg/rovio_domi.info @@ -39,6 +39,29 @@ Camera2 MrMC_y 0.0446798994558; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] MrMC_z -0.00861710075576; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] } +Camera3 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x -0.717540995787; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.00571832829522; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z 0.0123040970923; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.696384110446; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x 0.0323515084872; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y -0.0480190261945; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.0324906232515; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} +Camera4 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x 0.490198388917; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y 0.5116843721; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z -0.490599785917; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.50714543566; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x -0.064799845427; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.042329395001; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.00623084134628; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} + Init { State diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index 28ae50ec..bfd16364 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -33,6 +33,8 @@ #include #include +#include + #include #include #include @@ -132,6 +134,8 @@ class RovioNode{ ros::Subscriber subImg0_; ros::Subscriber subImg1_; ros::Subscriber subImg2_; + ros::Subscriber subImg3_; + ros::Subscriber subImg4_; ros::Subscriber subGroundtruth_; ros::Subscriber subGroundtruthOdometry_; ros::Subscriber subVelocity_; @@ -207,9 +211,12 @@ class RovioNode{ // Subscribe topics subImu_ = nh_.subscribe("imu0", 1000, &RovioNode::imuCallback,this); + // subImg0_ = nh_.subscribe("cam0/image_raw", 1000, std::bind(&RovioNode::imgCallback, std::placeholders::_1, 0),this); subImg0_ = nh_.subscribe("cam0/image_raw", 1000, &RovioNode::imgCallback0,this); subImg1_ = nh_.subscribe("cam1/image_raw", 1000, &RovioNode::imgCallback1,this); subImg2_ = nh_.subscribe("cam2/image_raw", 1000, &RovioNode::imgCallback2,this); + subImg3_ = nh_.subscribe("cam3/image_raw", 1000, &RovioNode::imgCallback3,this); + subImg4_ = nh_.subscribe("cam4/image_raw", 1000, &RovioNode::imgCallback4,this); subGroundtruth_ = nh_.subscribe("pose", 1000, &RovioNode::groundtruthCallback,this); subGroundtruthOdometry_ = nh_.subscribe("odometry", 1000, &RovioNode::groundtruthOdometryCallback, this); subVelocity_ = nh_.subscribe("abss/twist", 1000, &RovioNode::velocityCallback,this); @@ -502,6 +509,26 @@ class RovioNode{ if(mtState::nCam_ > 2) imgCallback(img,2); } + /** \brief Image callback for the camera with ID 3 + * + * @param img - Image message. + * @todo generalize + */ + void imgCallback3(const sensor_msgs::ImageConstPtr & img) { + std::lock_guard lock(m_filter_); + if(mtState::nCam_ > 3) imgCallback(img,3); + } + + /** \brief Image callback for the camera with ID 4 + * + * @param img - Image message. + * @todo generalize + */ + void imgCallback4(const sensor_msgs::ImageConstPtr & img) { + std::lock_guard lock(m_filter_); + if(mtState::nCam_ > 4) imgCallback(img,4); + } + /** \brief Image callback. Adds images (as update measurements) to the filter. * * @param img - Image message. @@ -523,7 +550,7 @@ class RovioNode{ if(msgTime != imgUpdateMeas_.template get().imgTime_){ for(int i=0;i().isValidPyr_[i]){ - std::cout << " \033[31mFailed Synchronization of Camera Frames, t = " << msgTime << "\033[0m" << std::endl; + std::cout << " \033[31mFailed Synchronization of Camera Frames, t = " << msgTime << " " << camID << "\033[0m" << std::endl; } } imgUpdateMeas_.template get().reset(msgTime); diff --git a/launch/rovio_domi.launch b/launch/rovio_domi.launch index f14745ed..7925e092 100644 --- a/launch/rovio_domi.launch +++ b/launch/rovio_domi.launch @@ -1,22 +1,34 @@ - - - - - - - - + + + + + + + + + + + + + + + - - - - - - - - + + + + + + + + + + + + + From e62b66040225e880ddf70f4ba9954e75e51bc4f3 Mon Sep 17 00:00:00 2001 From: trleonie Date: Wed, 28 Nov 2018 11:24:41 +0100 Subject: [PATCH 26/33] added histogram equalization --- include/rovio/RovioNode.hpp | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index bfd16364..d9165686 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -545,6 +545,36 @@ class RovioNode{ } cv::Mat cv_img; cv_ptr->image.copyTo(cv_img); + + // Histogram equalization + //if(histogram_equalize_){ + constexpr size_t hist_bins = 256; + cv::Mat hist; + std::vector img_vec = {cv_img}; + cv::calcHist(img_vec, {0}, cv::Mat(), hist, {hist_bins}, + {0, hist_bins-1}, false); + cv::Mat lut(1, hist_bins, CV_8UC1); + double sum = 0.0; + //prevents an image full of noise if in total darkness + float max_per_bin = cv_img.cols * cv_img.rows * 0.02; + float min_per_bin = cv_img.cols * cv_img.rows * 0.002; + float total_pixels = 0; + for(size_t i = 0; i < hist_bins; ++i){ + float& bin = hist.at(i); + if(bin > max_per_bin){ + bin = max_per_bin; + } else if(bin < min_per_bin){ + bin = min_per_bin; + } + total_pixels += bin; + } + for(size_t i = 0; i < hist_bins; ++i){ + sum += hist.at(i) / total_pixels; + lut.at(i) = (hist_bins-1)*sum; + } + cv::LUT(cv_img, lut, cv_img); + //} + if(init_state_.isInitialized() && !cv_img.empty()){ double msgTime = img->header.stamp.toSec(); if(msgTime != imgUpdateMeas_.template get().imgTime_){ From 735a8bb43a18a2769e176c1d285e1b2225942574 Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 3 Dec 2018 09:14:41 +0100 Subject: [PATCH 27/33] added histogram equalization --- include/rovio/RovioNode.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index d9165686..11e2f2d6 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -125,6 +125,7 @@ class RovioNode{ bool forceMarkersPublishing_; bool forcePatchPublishing_; bool gotFirstMessages_; + bool histogram_equalize_ = true; std::mutex m_filter_; // Nodes, Subscriber, Publishers @@ -547,7 +548,7 @@ class RovioNode{ cv_ptr->image.copyTo(cv_img); // Histogram equalization - //if(histogram_equalize_){ + if(histogram_equalize_){ constexpr size_t hist_bins = 256; cv::Mat hist; std::vector img_vec = {cv_img}; @@ -573,7 +574,7 @@ class RovioNode{ lut.at(i) = (hist_bins-1)*sum; } cv::LUT(cv_img, lut, cv_img); - //} + } if(init_state_.isInitialized() && !cv_img.empty()){ double msgTime = img->header.stamp.toSec(); From 896873ce4a42f3c3565751be1544b709b0eaf16f Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 3 Dec 2018 09:16:13 +0100 Subject: [PATCH 28/33] visualization of candidates --- include/rovio/ImgUpdate.hpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/include/rovio/ImgUpdate.hpp b/include/rovio/ImgUpdate.hpp index 0f440925..ccca4c7c 100644 --- a/include/rovio/ImgUpdate.hpp +++ b/include/rovio/ImgUpdate.hpp @@ -987,6 +987,14 @@ ImgOutlierDetection,false>{ for(int l=endLevel_;l<=startLevel_;l++){ meas.aux().pyr_[camID].detectFastCorners(candidates_,l,fastDetectionThreshold_); } + + // visualization of detected candidates + if(doFrameVisualisation_){ + for(int i=0;i newSet = filterState.fsm_.addBestCandidates(candidates_,meas.aux().pyr_[camID],camID,filterState.t_, @@ -1005,8 +1013,8 @@ ImgOutlierDetection,false>{ filterState.resetFeatureCovariance(*it,initCovFeature_); initCovFeature_(0,0) = initRelDepthCovTemp_; if(doFrameVisualisation_){ - f.mpCoordinates_->drawPoint(filterState.img_[camID], cv::Scalar(255,0,0)); - f.mpCoordinates_->drawText(filterState.img_[camID],std::to_string(f.idx_),cv::Scalar(255,0,0)); + f.mpCoordinates_->drawPoint(filterState.img_[camID], cv::Scalar(255,255,0)); //changed color from blue to aquamarin + f.mpCoordinates_->drawText(filterState.img_[camID],std::to_string(f.idx_),cv::Scalar(255,255,0)); //changed color from blue to aquamarin } if(mtState::nCam_>1 && doStereoInitialization_){ From 13309d8f8ed773dbe55ecc9e482d682b5fdd48ab Mon Sep 17 00:00:00 2001 From: trleonie Date: Mon, 10 Dec 2018 09:21:25 +0100 Subject: [PATCH 29/33] health checker added --- include/rovio/HealthMonitor.hpp | 90 +++++++++++++++++++++++++++++++++ include/rovio/RovioNode.hpp | 23 +++++++++ 2 files changed, 113 insertions(+) create mode 100644 include/rovio/HealthMonitor.hpp diff --git a/include/rovio/HealthMonitor.hpp b/include/rovio/HealthMonitor.hpp new file mode 100644 index 00000000..8b6f245f --- /dev/null +++ b/include/rovio/HealthMonitor.hpp @@ -0,0 +1,90 @@ +// This file was stolen straight out of maplab + +#ifndef ROVIO_HEALTH_MONITOR_H_ +#define ROVIO_HEALTH_MONITOR_H_ + +#include +#include + +#include "rovio/CoordinateTransform/RovioOutput.hpp" +#include "rovio/RovioFilter.hpp" + +namespace rovio { +class RovioHealthMonitor { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + RovioHealthMonitor() : num_subsequent_unhealthy_updates_(0) {} + + // Returns true if healthy; false if unhealthy and reset was triggered. + bool shouldResetEstimator(const std::vector& distance_covs_in, const StandardOutput& imu_output) { + float feature_distance_covariance_median = 0; + std::vector distance_covs = distance_covs_in; + if (!distance_covs.empty()) { + const size_t middle_index = distance_covs.size() / 2; + std::nth_element(distance_covs.begin(), + distance_covs.begin() + middle_index, + distance_covs.end()); + feature_distance_covariance_median = distance_covs[middle_index]; + } + const float BvB_norm = imu_output.BvB().norm(); + if ((BvB_norm > kVelocityToConsiderStatic) && ((BvB_norm > kUnhealthyVelocity) || + (feature_distance_covariance_median > kUnhealthyFeatureDistanceCov))) { + ++num_subsequent_unhealthy_updates_; + std::cout << "Estimator fault counter: " + << num_subsequent_unhealthy_updates_ << "/" + << kMaxSubsequentUnhealthyUpdates << ". Might reset soon."; + if (num_subsequent_unhealthy_updates_ > kMaxSubsequentUnhealthyUpdates) { + std::cout << "Will reset ROVIOLI. Velocity norm: " << BvB_norm + << " (limit: " << kUnhealthyVelocity + << "), median of feature distance covariances: " + << feature_distance_covariance_median + << " (limit: " << kUnhealthyFeatureDistanceCov << ")."; + return true; + } + } else { + if (feature_distance_covariance_median < kHealthyFeatureDistanceCov) { + if (std::abs(feature_distance_covariance_median - + last_safe_pose_.feature_distance_covariance_median) < + kHealthyFeatureDistanceCovIncrement) { + last_safe_pose_.failsafe_WrWB = imu_output.WrWB(); + last_safe_pose_.failsafe_qBW = imu_output.qBW(); + last_safe_pose_.feature_distance_covariance_median = feature_distance_covariance_median; + } + } + num_subsequent_unhealthy_updates_ = 0; + } + return false; + } + + Eigen::Vector3d failsafe_WrWB() { return last_safe_pose_.failsafe_WrWB; } + + kindr::RotationQuaternionPD failsafe_qBW() { + return last_safe_pose_.failsafe_qBW; + } + + private: + struct RovioFailsafePose { + RovioFailsafePose() + : failsafe_WrWB(Eigen::Vector3d::Zero()), feature_distance_covariance_median(0.0) { + failsafe_qBW.setIdentity(); + } + Eigen::Vector3d failsafe_WrWB; + kindr::RotationQuaternionPD failsafe_qBW; + float feature_distance_covariance_median; + }; + + RovioFailsafePose last_safe_pose_; + int num_subsequent_unhealthy_updates_; + + // The landmark covariance is not a good measure for divergence if we are static. + static constexpr float kVelocityToConsiderStatic = 0.1f; + static constexpr int kMaxSubsequentUnhealthyUpdates = 1; + static constexpr float kHealthyFeatureDistanceCov = 0.5f; + static constexpr float kHealthyFeatureDistanceCovIncrement = 0.3f; + static constexpr float kUnhealthyFeatureDistanceCov = 1.0f; + static constexpr float kUnhealthyVelocity = 5.0f; +}; + +} // namespace rovio + #endif // ROVIO_HEALTH_MONITOR_H_ \ No newline at end of file diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index 11e2f2d6..76b70ead 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -52,6 +52,7 @@ #include #include "rovio/RovioFilter.hpp" +#include "rovio/HealthMonitor.hpp" #include "rovio/CoordinateTransform/RovioOutput.hpp" #include "rovio/CoordinateTransform/FeatureOutput.hpp" #include "rovio/CoordinateTransform/FeatureOutputReadable.hpp" @@ -86,6 +87,8 @@ class RovioNode{ typedef typename mtVelocityUpdate::mtMeas mtVelocityMeas; mtVelocityMeas velocityUpdateMeas_; + RovioHealthMonitor healthMonitor_; + struct FilterInitializationState { FilterInitializationState() : WrWM_(V3D::Zero()), @@ -125,7 +128,10 @@ class RovioNode{ bool forceMarkersPublishing_; bool forcePatchPublishing_; bool gotFirstMessages_; + bool histogram_equalize_ = true; + bool healthCheck_ = false; + std::mutex m_filter_; // Nodes, Subscriber, Publishers @@ -934,6 +940,8 @@ class RovioNode{ pubImuBias_.publish(imuBiasMsg_); } + std::vector featureDistanceCov; + // PointCloud message. if(pubPcl_.getNumSubscribers() > 0 || pubMarkers_.getNumSubscribers() > 0 || forcePclPublishing_ || forceMarkersPublishing_){ pclMsg_.header.seq = msgSeq_; @@ -976,6 +984,8 @@ class RovioNode{ featureOutputReadableCT_.transformState(featureOutput_,featureOutputReadable_); featureOutputReadableCT_.transformCovMat(featureOutput_,featureOutputCov_,featureOutputReadableCov_); + featureDistanceCov.push_back(static_cast(featureOutputReadableCov_(3, 3))); // for health checker + // Get landmark output landmarkOutputImuCT_.setFeatureID(i); landmarkOutputImuCT_.transformState(state,landmarkOutput_); @@ -1071,6 +1081,19 @@ class RovioNode{ pubPatch_.publish(patchMsg_); } gotFirstMessages_ = true; + + if (healthCheck_) { + if(healthMonitor_.shouldResetEstimator(featureDistanceCov, imuOutput_)) { + if(!init_state_.isInitialized()) { + std::cout << "Reinitioalization already triggered. Ignoring request..."; + return; + } + + init_state_.WrWM_ = healthMonitor_.failsafe_WrWB(); + init_state_.qMW_ = healthMonitor_.failsafe_qBW(); + init_state_.state_ = FilterInitializationState::State::WaitForInitExternalPose; + } + } } } } From 63d2fb2301b223ee8d2a3f2118b87bd0783ba437 Mon Sep 17 00:00:00 2001 From: trleonie Date: Wed, 12 Dec 2018 14:26:59 +0100 Subject: [PATCH 30/33] add best global features --- include/rovio/FeatureManager.hpp | 175 ++++++++++++++++++++++++++++++- include/rovio/ImgUpdate.hpp | 125 ++++++++++++++-------- 2 files changed, 254 insertions(+), 46 deletions(-) diff --git a/include/rovio/FeatureManager.hpp b/include/rovio/FeatureManager.hpp index 2fe0917b..dd425dd2 100644 --- a/include/rovio/FeatureManager.hpp +++ b/include/rovio/FeatureManager.hpp @@ -265,7 +265,7 @@ class FeatureSetManager{ * @param initTime - Current time (time at which the MultilevelPatchFeature%s are created from the candidates list). * @param l1 - Start pyramid level for the Shi-Tomasi Score computation of MultilevelPatchFeature%s extracted from the candidates list. * @param l2 - End pyramid level for the Shi-Tomasi Score computation of MultilevelPatchFeature%s extracted from the candidates list. - * @param maxAddedFeature - Maximal number of features which should be added to the mlpSet. + * @param maxAddedFeature - Maximal number of features which should be added to the mlpSet. * @param nDetectionBuckets - Number of buckets. * @param scoreDetectionExponent - Choose it between [0 1]. 1 : Candidate features are sorted linearly into the buckets, depending on their Shi-Tomasi score. * 0 : All candidate features are filled into the highest bucket. @@ -389,6 +389,179 @@ class FeatureSetManager{ return newFeatureIDs; } +//////////////////////////////////////////////////////////////////////////// FUNCTION FOR ADDING BEST GLOBAL FEATURES +//////////////////////////////////////////////////////////////////////////// added by Leonie Traffelet + + /** \brief Adds the best MultilevelPatchFeature%s from a candidates list to an existing MultilevelPatchSet of ALL CAMERAS. + * + * This function takes a given feature candidate list of all cameras and builds, in a first step, + * MultilevelPatchFeature%s out of it using a given image pyramid. For each MultilevelPatchFeature the + * corresponding Shi-Tomasi Score is computed. + * In a second step, the candidate MultilevelPatchFeature%s are sorted and + * placed into buckets of their detection camera, depending on their individual Shi-Tomasi Score. MultilevelPatchFeature%s in a high bucket + * (high bucket index) have higher Shi-Tomasi Scores than MultilevelPatchFeature%s which have been placed into a + * low bucket. + * In a third step, the MultilevelPatchFeature%s in the buckets are reordered, depending on their distance + * to already existing features in the given MultilevelPatchSet. A small distance to an existing feature is punished, + * by moving the concerned candidate MultilevelPatchFeature into a lower bucket. + * Finally the best MultilevelPatchFeatures of each camera bucket are compared to each other and the best ones are added to the existing MultilevelPatchSet. + * + * @param candidates[nCam] - Array containing the list of candidate feature coordinates per camera. + * @param pyr[nCam] - Image pyramids of all the cameras used to extract the MultilevelPatchFeature%s from the candidates list. + * @param initTime - Current time (time at which the MultilevelPatchFeature%s are created from the candidates list). + * @param l1 - Start pyramid level for the Shi-Tomasi Score computation of MultilevelPatchFeature%s extracted from the candidates list. + * @param l2 - End pyramid level for the Shi-Tomasi Score computation of MultilevelPatchFeature%s extracted from the candidates list. + * @param maxAddedFeature - Maximal number of features which should be added to the mlpSet. + * @param nDetectionBuckets - Number of buckets. + * @param scoreDetectionExponent - Choose it between [0 1]. 1 : Candidate features are sorted linearly into the buckets, depending on their Shi-Tomasi score. + * 0 : All candidate features are filled into the highest bucket. + * A small scoreDetectionExponent forces more candidate features into high buckets. + * @param penaltyDistance - If a candidate feature has a smaller distance to an existing feature in the mlpSet, it is punished (shifted in an lower bucket) dependent of its actual distance to the existing feature. + * @param zeroDistancePenalty - A candidate feature in a specific bucket is shifted zeroDistancePenalty-buckets back to a lower bucket if it has zero distance to an existing feature in the mlpSet. + * @param requireMax - Should the adding of maxAddedFeature be enforced? + * @param minScore - Shi-Tomasi Score threshold for the best (highest Shi-Tomasi Score) MultilevelPatchFeature extracted from the candidates list. + * If the best MultilevelPatchFeature has a Shi-Tomasi Score less than or equal this threshold, the function aborts and returns an empty map. + * + * @return an unordered_set, holding the indizes of the MultilevelPatchSet, at which the new MultilevelPatchFeature%s have been added (from the candidates list). + */ + + std::unordered_set addBestGlobalCandidates(const FeatureCoordinatesVec (&candidates)[nCam], const ImagePyramid (&pyr)[nCam], const double initTime, + const int l1, const int l2, const int maxAddedFeature, const int nDetectionBuckets, const double scoreDetectionExponent, + const double penaltyDistance, const double zeroDistancePenalty, const bool requireMax, const float minScore){ + std::unordered_set newFeatureIDs; + std::vector> multilevelPatches[nCam]; + std::vector>> buckets(nCam, std::vector>(nDetectionBuckets)); + + double d2; + double t2 = pow(penaltyDistance,2); + bool doDelete; + unsigned int newBucketID; + + for(int camID = 0;camID maxScore) maxScore = multilevelPatches[camID].back().s_; + } else { + multilevelPatches[camID].back().s_ = -1; + } + } + if(maxScore <= minScore){ + break; + } + + // Make buckets and fill based on score + float relScore; + for(int i=0;i 0.0){ + newBucketID = std::ceil((nDetectionBuckets-1)*(pow(relScore,static_cast(scoreDetectionExponent)))); + if(newBucketID>nDetectionBuckets-1) newBucketID = nDetectionBuckets-1; + buckets[camID][newBucketID].insert(i); + } + } + + // Move buckets based on current features + FeatureCoordinates featureCoordinates; + FeatureDistance featureDistance; + for(unsigned int i=0;itransformFeature(camID,*(features_[i].mpCoordinates_),*(features_[i].mpDistance_),featureCoordinates,featureDistance); + if(featureCoordinates.isInFront()){ + for (unsigned int bucketID = 1;bucketID < nDetectionBuckets;bucketID++) { + for (auto it_cand = buckets[camID][bucketID].begin();it_cand != buckets[camID][bucketID].end();) { + doDelete = false; + d2 = std::pow(featureCoordinates.get_c().x - candidates[camID][*it_cand].get_c().x,2) + std::pow(featureCoordinates.get_c().y - candidates[camID][*it_cand].get_c().y,2); // Squared distance between the existing feature and the candidate feature. + if(d2 > nfCandidates(2, std::vector(nCam)); + std::vector bestBucketID(nCam, nDetectionBuckets-1); + + + while(addedCount < maxAddedFeature && getValidCount() != nMax) { + // get best features in each camera + for (int camID = 0; camID < nCam; camID++){ + for (int bucketID = bestBucketID[camID];bucketID >= 0+static_cast(!requireMax);bucketID--) { + //std::cout << "in bucket for loop. camera: " << camID << " Bucket: " << bucketID << std::endl; + if(!buckets[camID][bucketID].empty()){ + nfCandidates[0][camID] = bucketID; + nfCandidates[1][camID] = *(buckets[camID][bucketID].begin()); + bestBucketID[camID] = bucketID; + break; + } + } + } + + // get best of these + const int nfCam = std::max_element(nfCandidates[1].begin(), nfCandidates[1].end()) - nfCandidates[1].begin(); + const int nfBucket = nfCandidates[0][nfCam]; + const int nf = nfCandidates[1][nfCam]; + + buckets[nfCam][nfBucket].erase(nf); + const int ind = makeNewFeature(nfCam); + features_[ind].mpCoordinates_->set_c(candidates[nfCam][nf].get_c()); + features_[ind].mpCoordinates_->camID_ = nfCam; + features_[ind].mpCoordinates_->set_warp_identity(); + features_[ind].mpCoordinates_->mpCamera_ = &mpMultiCamera_->cameras_[nfCam]; + *(features_[ind].mpMultilevelPatch_) = multilevelPatches[nfCam][nf]; + if(ind >= 0){ + newFeatureIDs.insert(ind); + } + addedCount++; + for (unsigned int bucketID2 = 1;bucketID2 <= nfBucket;bucketID2++) { + for (auto it_cand = buckets[nfCam][bucketID2].begin();it_cand != buckets[nfCam][bucketID2].end();) { + doDelete = false; + d2 = std::pow(candidates[nfCam][nf].get_c().x - candidates[nfCam][*it_cand].get_c().x,2) + std::pow(candidates[nfCam][nf].get_c().y - candidates[nfCam][*it_cand].get_c().y,2); + if(d2,int>& mp1, const std::tuple,int>&mp2){ return std::get<1>(mp1).s_ > std::get<1>(mp2).s_; } diff --git a/include/rovio/ImgUpdate.hpp b/include/rovio/ImgUpdate.hpp index ccca4c7c..85e32936 100644 --- a/include/rovio/ImgUpdate.hpp +++ b/include/rovio/ImgUpdate.hpp @@ -224,6 +224,7 @@ ImgOutlierDetection,false>{ int alignMaxUniSample_; bool useCrossCameraMeasurements_; /**,false>{ mutable MultilevelPatch mlpTemp2_; mutable FeatureCoordinates alignedCoordinates_; mutable FeatureCoordinates tempCoordinates_; - mutable FeatureCoordinatesVec candidates_; + mutable FeatureCoordinatesVec candidates_[mtState::nCam_]; + //mutable std::vector candidates_; mutable cv::Point2f c_temp_; mutable Eigen::Matrix2d c_J_; mutable Eigen::Matrix2d A_red_; @@ -305,6 +307,7 @@ ImgOutlierDetection,false>{ alignMaxUniSample_ = 5; useCrossCameraMeasurements_ = true; doStereoInitialization_ = true; + addGlobalBest_ = false; removalFactor_ = 1.1; alignmentGaussianWeightingSigma_ = 2.0; discriminativeSamplingDistance_ = 0.0; @@ -343,6 +346,7 @@ ImgOutlierDetection,false>{ boolRegister_.registerScalar("removeNegativeFeatureAfterUpdate",removeNegativeFeatureAfterUpdate_); boolRegister_.registerScalar("useCrossCameraMeasurements",useCrossCameraMeasurements_); boolRegister_.registerScalar("doStereoInitialization",doStereoInitialization_); + boolRegister_.registerScalar("addGlobalBest",addGlobalBest_); boolRegister_.registerScalar("useIntensityOffsetForAlignment",alignment_.useIntensityOffset_); boolRegister_.registerScalar("useIntensitySqewForAlignment",alignment_.useIntensitySqew_); doubleRegister_.removeScalarByVar(updnoiP_(0,0)); @@ -979,78 +983,109 @@ ImgOutlierDetection,false>{ } else { medianDepthParameters.fill(initDepth_); } + + if(verbose_) std::cout << "Adding keypoints" << std::endl; + + // Get Candidates for(int camID = 0;camID newSet = filterState.fsm_.addBestCandidates(candidates_,meas.aux().pyr_[camID],camID,filterState.t_, - endLevel_,startLevel_,(mtState::nMax_-filterState.fsm_.getValidCount())/(mtState::nCam_-camID),nDetectionBuckets_, scoreDetectionExponent_, - penaltyDistance_, zeroDistancePenalty_,false,minAbsoluteSTScore_); + std::unordered_set newSet = filterState.fsm_.addBestGlobalCandidates(candidates_,meas.aux().pyr_,filterState.t_, + endLevel_,startLevel_,(mtState::nMax_-filterState.fsm_.getValidCount()),nDetectionBuckets_, scoreDetectionExponent_, + penaltyDistance_, zeroDistancePenalty_,false,minAbsoluteSTScore_); const double t3 = (double) cv::getTickCount(); - if(verbose_) std::cout << "== Got " << filterState.fsm_.getValidCount() << " after adding " << newSet.size() << " features in camera " << camID << " (" << (t3-t2)/cv::getTickFrequency()*1000 << " ms)" << std::endl; + if(verbose_) std::cout << "== Got " << filterState.fsm_.getValidCount() << " after adding " << newSet.size() << " features in" << " " << (t3-t2)/cv::getTickFrequency()*1000 << " ms" << std::endl; for(auto it = newSet.begin();it != newSet.end();++it){ FeatureManager& f = filterState.fsm_.features_[*it]; f.mpStatistics_->resetStatistics(filterState.t_); - f.mpStatistics_->status_[camID] = TRACKED; + f.mpStatistics_->status_[f.mpCoordinates_->camID_] = TRACKED; f.mpStatistics_->lastPatchUpdate_ = filterState.t_; - f.mpDistance_->p_ = medianDepthParameters[camID]; + f.mpDistance_->p_ = medianDepthParameters[f.mpCoordinates_->camID_]; const float initRelDepthCovTemp_ = initCovFeature_(0,0); initCovFeature_(0,0) = initRelDepthCovTemp_*pow(f.mpDistance_->getParameterDerivative()*f.mpDistance_->getDistance(),2); filterState.resetFeatureCovariance(*it,initCovFeature_); initCovFeature_(0,0) = initRelDepthCovTemp_; if(doFrameVisualisation_){ - f.mpCoordinates_->drawPoint(filterState.img_[camID], cv::Scalar(255,255,0)); //changed color from blue to aquamarin - f.mpCoordinates_->drawText(filterState.img_[camID],std::to_string(f.idx_),cv::Scalar(255,255,0)); //changed color from blue to aquamarin + f.mpCoordinates_->drawPoint(filterState.img_[f.mpCoordinates_->camID_], cv::Scalar(255,255,0)); //changed color from blue to aquamarin + f.mpCoordinates_->drawText(filterState.img_[f.mpCoordinates_->camID_],std::to_string(f.idx_),cv::Scalar(255,255,0)); //changed color from blue to aquamarin } + } + } else { + for(int camID = 0;camID newSet = filterState.fsm_.addBestCandidates(candidates_[camID],meas.aux().pyr_[camID],camID,filterState.t_, + endLevel_,startLevel_,(mtState::nMax_-filterState.fsm_.getValidCount())/(mtState::nCam_-camID),nDetectionBuckets_, scoreDetectionExponent_, + penaltyDistance_, zeroDistancePenalty_,false,minAbsoluteSTScore_); + const double t3 = (double) cv::getTickCount(); + if(verbose_) std::cout << "== Got " << filterState.fsm_.getValidCount() << " after adding " << newSet.size() << " features in camera " << camID << " (" << (t3-t2)/cv::getTickFrequency()*1000 << " ms)" << std::endl; + for(auto it = newSet.begin();it != newSet.end();++it){ + FeatureManager& f = filterState.fsm_.features_[*it]; + f.mpStatistics_->resetStatistics(filterState.t_); + f.mpStatistics_->status_[camID] = TRACKED; + f.mpStatistics_->lastPatchUpdate_ = filterState.t_; + f.mpDistance_->p_ = medianDepthParameters[camID]; + const float initRelDepthCovTemp_ = initCovFeature_(0,0); + initCovFeature_(0,0) = initRelDepthCovTemp_*pow(f.mpDistance_->getParameterDerivative()*f.mpDistance_->getDistance(),2); + filterState.resetFeatureCovariance(*it,initCovFeature_); + initCovFeature_(0,0) = initRelDepthCovTemp_; + if(doFrameVisualisation_){ + f.mpCoordinates_->drawPoint(filterState.img_[camID], cv::Scalar(255,255,0)); //changed color from blue to aquamarin + f.mpCoordinates_->drawText(filterState.img_[camID],std::to_string(f.idx_),cv::Scalar(255,255,0)); //changed color from blue to aquamarin + } - if(mtState::nCam_>1 && doStereoInitialization_){ - const int otherCam = (camID+1)%mtState::nCam_; - transformFeatureOutputCT_.setFeatureID(*it); - transformFeatureOutputCT_.setOutputCameraID(otherCam); - transformFeatureOutputCT_.transformState(filterState.state_,featureOutput_); - if(alignment_.align2DAdaptive(alignedCoordinates_,meas.aux().pyr_[otherCam],*f.mpMultilevelPatch_,featureOutput_.c(),startLevel_,endLevel_, - alignConvergencePixelRange_,alignCoverageRatio_,alignMaxUniSample_)){ - bool valid = mlpTemp1_.isMultilevelPatchInFrame(meas.aux().pyr_[otherCam],alignedCoordinates_,startLevel_,false); - if(valid && patchRejectionTh_ >= 0){ - mlpTemp1_.extractMultilevelPatchFromImage(meas.aux().pyr_[otherCam],alignedCoordinates_,startLevel_,false); - const float avgError = mlpTemp1_.computeAverageDifference(*f.mpMultilevelPatch_,endLevel_,startLevel_); - if(avgError > patchRejectionTh_){ - valid = false; - } - } - if(valid == true){ - if(doFrameVisualisation_){ - alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(150,0,0)); - alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(150,0,0)); + if(mtState::nCam_>1 && doStereoInitialization_){ + const int otherCam = (camID+1)%mtState::nCam_; + transformFeatureOutputCT_.setFeatureID(*it); + transformFeatureOutputCT_.setOutputCameraID(otherCam); + transformFeatureOutputCT_.transformState(filterState.state_,featureOutput_); + if(alignment_.align2DAdaptive(alignedCoordinates_,meas.aux().pyr_[otherCam],*f.mpMultilevelPatch_,featureOutput_.c(),startLevel_,endLevel_, + alignConvergencePixelRange_,alignCoverageRatio_,alignMaxUniSample_)){ + bool valid = mlpTemp1_.isMultilevelPatchInFrame(meas.aux().pyr_[otherCam],alignedCoordinates_,startLevel_,false); + if(valid && patchRejectionTh_ >= 0){ + mlpTemp1_.extractMultilevelPatchFromImage(meas.aux().pyr_[otherCam],alignedCoordinates_,startLevel_,false); + const float avgError = mlpTemp1_.computeAverageDifference(*f.mpMultilevelPatch_,endLevel_,startLevel_); + if(avgError > patchRejectionTh_){ + valid = false; + } } - if(f.mpCoordinates_->getDepthFromTriangulation(alignedCoordinates_,state.qCM(otherCam).rotate(V3D(state.MrMC(camID)-state.MrMC(otherCam))),state.qCM(otherCam)*state.qCM(camID).inverted(), *f.mpDistance_, 0.01)){ - filterState.resetFeatureCovariance(*it,initCovFeature_); // TODO: improve + if(valid == true){ + if(doFrameVisualisation_){ + alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(150,0,0)); + alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(150,0,0)); + } + if(f.mpCoordinates_->getDepthFromTriangulation(alignedCoordinates_,state.qCM(otherCam).rotate(V3D(state.MrMC(camID)-state.MrMC(otherCam))),state.qCM(otherCam)*state.qCM(camID).inverted(), *f.mpDistance_, 0.01)){ + filterState.resetFeatureCovariance(*it,initCovFeature_); // TODO: improve + } + } else { + if(doFrameVisualisation_){ + alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(0,0,150)); + alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(0,0,150)); + } } } else { if(doFrameVisualisation_){ - alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(0,0,150)); - alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(0,0,150)); + alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(0,150,0)); + alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(0,150,0)); } } - } else { - if(doFrameVisualisation_){ - alignedCoordinates_.drawPoint(filterState.img_[otherCam], cv::Scalar(0,150,0)); - alignedCoordinates_.drawText(filterState.img_[otherCam],std::to_string(f.idx_),cv::Scalar(0,150,0)); - } } } } From c818080936bd075f36b86bc887943b120c00bd31 Mon Sep 17 00:00:00 2001 From: trleonie Date: Wed, 12 Dec 2018 14:30:39 +0100 Subject: [PATCH 31/33] addGlobalBest param added --- cfg/rovio_domi.info | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/cfg/rovio_domi.info b/cfg/rovio_domi.info index bd983f04..71cefd13 100644 --- a/cfg/rovio_domi.info +++ b/cfg/rovio_domi.info @@ -2,9 +2,9 @@ ; #include "/home/user/workspace/rovio/cfg/rovio_custom.info" Common { - doVECalibration true; Should the camera-IMU extrinsics be calibrated online + doVECalibration false; Should the camera-IMU extrinsics be calibrated online depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic) - verbose false; Is the verbose active + verbose true; Is the verbose active } Camera0 { @@ -153,6 +153,7 @@ ImgUpdate maxUncertaintyToDepthRatioForDepthInitialization 0.3; If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame useCrossCameraMeasurements false; Should cross measurements between frame be used. Might be turned of in calibration phase. doStereoInitialization false; Should a stereo match be used for feature initialization. + addGlobalBest true; Should the best features of all cameras be added (true) or the best of each camera (false) noiseGainForOffCamera 10.0; Factor added on update noise if not main camera discriminativeSamplingDistance 0.02; Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed). discriminativeSamplingGain 1.1; Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used). From 2ec4fce2025801d8359073c561d1851a6f0e3737 Mon Sep 17 00:00:00 2001 From: trleonie Date: Wed, 12 Dec 2018 15:24:44 +0100 Subject: [PATCH 32/33] new param for info file: HE, HC, addGlobal, showCandidates --- cfg/rovio_domi.info | 6 +++++- include/rovio/FeatureManager.hpp | 6 +----- include/rovio/ImgUpdate.hpp | 10 +++++++++- include/rovio/RovioFilter.hpp | 1 + include/rovio/RovioNode.hpp | 7 ++----- 5 files changed, 18 insertions(+), 12 deletions(-) diff --git a/cfg/rovio_domi.info b/cfg/rovio_domi.info index 71cefd13..4813100c 100644 --- a/cfg/rovio_domi.info +++ b/cfg/rovio_domi.info @@ -5,6 +5,7 @@ Common doVECalibration false; Should the camera-IMU extrinsics be calibrated online depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic) verbose true; Is the verbose active + healthCheck true; } Camera0 { @@ -110,6 +111,7 @@ ImgUpdate maxNumIteration 20; doPatchWarping true; Should the patches be warped doFrameVisualisation true; Should the frames be visualized + showCandidates false; Should the possible feature candidates be displayed visualizePatches false; Should the patches be visualized useDirectMethod true; Should the EKF-innovation be based on direct intensity error (o.w. reprojection error) startLevel 2; Highest patch level which is being employed (must be smaller than the hardcoded template parameter) @@ -153,7 +155,6 @@ ImgUpdate maxUncertaintyToDepthRatioForDepthInitialization 0.3; If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame useCrossCameraMeasurements false; Should cross measurements between frame be used. Might be turned of in calibration phase. doStereoInitialization false; Should a stereo match be used for feature initialization. - addGlobalBest true; Should the best features of all cameras be added (true) or the best of each camera (false) noiseGainForOffCamera 10.0; Factor added on update noise if not main camera discriminativeSamplingDistance 0.02; Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed). discriminativeSamplingGain 1.1; Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used). @@ -176,6 +177,9 @@ ImgUpdate minNoMotionTime 1.0; Min duration of no-motion isEnabled 0; Should zero velocity update be applied, only works if MotionDetection.isEnabled is true } + + addGlobalBest true; Should the best features of all cameras be added (true) or the best of each camera (false) + histogramEqualize true; } Prediction { diff --git a/include/rovio/FeatureManager.hpp b/include/rovio/FeatureManager.hpp index dd425dd2..4c50b5fe 100644 --- a/include/rovio/FeatureManager.hpp +++ b/include/rovio/FeatureManager.hpp @@ -496,11 +496,7 @@ class FeatureSetManager{ } } - - - - // Check Buckets for overall best features - + //// Check Buckets for overall best features // Incrementally add features and update candidate buckets (Check distance of candidates with respect to the newly inserted feature). int addedCount = 0; std::vector< std::vector > nfCandidates(2, std::vector(nCam)); diff --git a/include/rovio/ImgUpdate.hpp b/include/rovio/ImgUpdate.hpp index 85e32936..76e2800b 100644 --- a/include/rovio/ImgUpdate.hpp +++ b/include/rovio/ImgUpdate.hpp @@ -209,8 +209,10 @@ ImgOutlierDetection,false>{ bool useDirectMethod_; /**,false>{ bool useCrossCameraMeasurements_; /**,false>{ zeroDistancePenalty_ = nDetectionBuckets_*1.0; useDirectMethod_ = true; doFrameVisualisation_ = true; + showCandidates_ = false; visualizePatches_ = false; verbose_ = false; + healthCheck_ = false; trackingUpperBound_ = 0.9; trackingLowerBound_ = 0.1; minTrackedAndFreeFeatures_ = 0.5; @@ -308,6 +313,7 @@ ImgOutlierDetection,false>{ useCrossCameraMeasurements_ = true; doStereoInitialization_ = true; addGlobalBest_ = false; + histogramEqualize_ = false; removalFactor_ = 1.1; alignmentGaussianWeightingSigma_ = 2.0; discriminativeSamplingDistance_ = 0.0; @@ -342,11 +348,13 @@ ImgOutlierDetection,false>{ boolRegister_.registerScalar("MotionDetection.isEnabled",doVisualMotionDetection_); boolRegister_.registerScalar("useDirectMethod",useDirectMethod_); boolRegister_.registerScalar("doFrameVisualisation",doFrameVisualisation_); + boolRegister_.registerScalar("showCandidates",showCandidates_); boolRegister_.registerScalar("visualizePatches",visualizePatches_); boolRegister_.registerScalar("removeNegativeFeatureAfterUpdate",removeNegativeFeatureAfterUpdate_); boolRegister_.registerScalar("useCrossCameraMeasurements",useCrossCameraMeasurements_); boolRegister_.registerScalar("doStereoInitialization",doStereoInitialization_); boolRegister_.registerScalar("addGlobalBest",addGlobalBest_); + boolRegister_.registerScalar("histogramEqualize",histogramEqualize_); boolRegister_.registerScalar("useIntensityOffsetForAlignment",alignment_.useIntensityOffset_); boolRegister_.registerScalar("useIntensitySqewForAlignment",alignment_.useIntensitySqew_); doubleRegister_.removeScalarByVar(updnoiP_(0,0)); @@ -999,7 +1007,7 @@ ImgOutlierDetection,false>{ if(verbose_) std::cout << "== Detected " << candidates_[camID].size() << " candidates in Camera" << camID << " on levels " << endLevel_ << "-" << startLevel_ << " (" << (t2-t1)/cv::getTickFrequency()*1000 << " ms)" << std::endl; // visualization of detected candidates - if(doFrameVisualisation_){ + if(doFrameVisualisation_ && showCandidates_){ for(int i=0;i, std::get<0>(mUpdates_).outlierDetection_.setEnabledAll(true); std::get<1>(mUpdates_).outlierDetection_.setEnabledAll(true); boolRegister_.registerScalar("Common.verbose",std::get<0>(mUpdates_).verbose_); + boolRegister_.registerScalar("Common.healthCheck",std::get<0>(mUpdates_).healthCheck_); mPrediction_.doubleRegister_.removeScalarByStr("alpha"); mPrediction_.doubleRegister_.removeScalarByStr("beta"); mPrediction_.doubleRegister_.removeScalarByStr("kappa"); diff --git a/include/rovio/RovioNode.hpp b/include/rovio/RovioNode.hpp index 76b70ead..66f2571c 100644 --- a/include/rovio/RovioNode.hpp +++ b/include/rovio/RovioNode.hpp @@ -128,9 +128,6 @@ class RovioNode{ bool forceMarkersPublishing_; bool forcePatchPublishing_; bool gotFirstMessages_; - - bool histogram_equalize_ = true; - bool healthCheck_ = false; std::mutex m_filter_; @@ -554,7 +551,7 @@ class RovioNode{ cv_ptr->image.copyTo(cv_img); // Histogram equalization - if(histogram_equalize_){ + if(mpImgUpdate_->histogramEqualize_){ constexpr size_t hist_bins = 256; cv::Mat hist; std::vector img_vec = {cv_img}; @@ -1082,7 +1079,7 @@ class RovioNode{ } gotFirstMessages_ = true; - if (healthCheck_) { + if(mpImgUpdate_->healthCheck_){ if(healthMonitor_.shouldResetEstimator(featureDistanceCov, imuOutput_)) { if(!init_state_.isInitialized()) { std::cout << "Reinitioalization already triggered. Ignoring request..."; From a507af4d18f7914256096eb17e318e6ed90cdf20 Mon Sep 17 00:00:00 2001 From: trleonie Date: Thu, 31 Jan 2019 15:51:41 +0100 Subject: [PATCH 33/33] updated camera order to match order in rovio_domi.launch --- cfg/rovio_domi.info | 70 ++++++++++++++++++++-------------------- launch/rovio_domi.launch | 8 ++--- 2 files changed, 39 insertions(+), 39 deletions(-) diff --git a/cfg/rovio_domi.info b/cfg/rovio_domi.info index 4813100c..c1911b41 100644 --- a/cfg/rovio_domi.info +++ b/cfg/rovio_domi.info @@ -4,43 +4,21 @@ Common { doVECalibration false; Should the camera-IMU extrinsics be calibrated online depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic) - verbose true; Is the verbose active - healthCheck true; + verbose false; Is the verbose active + healthCheck false; } Camera0 { - CalibrationFile ; Camera-Calibration file for intrinsics - qCM_x -0.0141053692505; X-entry of IMU to Camera quaterion (Hamilton) - qCM_y -0.00445598914788; Y-entry of IMU to Camera quaterion (Hamilton) - qCM_z 0.736220553648; Z-entry of IMU to Camera quaterion (Hamilton) - qCM_w 0.676579987219; W-entry of IMU to Camera quaterion (Hamilton) - MrMC_x -0.013725561541; X-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_y 0.0458722860308; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_z 0.00949435362177; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] -} -Camera1 -{ - CalibrationFile ; Camera-Calibration file for intrinsics - qCM_x -0.00953751922809; X-entry of IMU to Camera quaterion (Hamilton) - qCM_y -0.703628160853; Y-entry of IMU to Camera quaterion (Hamilton) - qCM_z -0.710497111443; Z-entry of IMU to Camera quaterion (Hamilton) - qCM_w 0.00320961269892; W-entry of IMU to Camera quaterion (Hamilton) - MrMC_x 0.0341624938784; X-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_y 0.0489333125579; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_z -0.0250547932613; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x -0.0141053692505; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.00445598914788; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z 0.736220553648; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.676579987219; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x -0.013725561541; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0458722860308; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z 0.00949435362177; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] } Camera2 -{ - CalibrationFile ; Camera-Calibration file for intrinsics - qCM_x 0.498284808832; X-entry of IMU to Camera quaterion (Hamilton) - qCM_y -0.500933590992; Y-entry of IMU to Camera quaterion (Hamilton) - qCM_z 0.506194778687; Z-entry of IMU to Camera quaterion (Hamilton) - qCM_w 0.494514542489; W-entry of IMU to Camera quaterion (Hamilton) - MrMC_x 0.0266940973582; X-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_y 0.0446798994558; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] - MrMC_z -0.00861710075576; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] -} -Camera3 { CalibrationFile ; Camera-Calibration file for intrinsics qCM_x -0.717540995787; X-entry of IMU to Camera quaterion (Hamilton) @@ -51,6 +29,17 @@ Camera3 MrMC_y -0.0480190261945; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] MrMC_z -0.0324906232515; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] } +Camera3 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x 0.498284808832; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.500933590992; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z 0.506194778687; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.494514542489; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x 0.0266940973582; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0446798994558; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.00861710075576; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} Camera4 { CalibrationFile ; Camera-Calibration file for intrinsics @@ -62,6 +51,17 @@ Camera4 MrMC_y 0.042329395001; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] MrMC_z -0.00623084134628; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] } +Camera1 +{ + CalibrationFile ; Camera-Calibration file for intrinsics + qCM_x -0.00953751922809; X-entry of IMU to Camera quaterion (Hamilton) + qCM_y -0.703628160853; Y-entry of IMU to Camera quaterion (Hamilton) + qCM_z -0.710497111443; Z-entry of IMU to Camera quaterion (Hamilton) + qCM_w 0.00320961269892; W-entry of IMU to Camera quaterion (Hamilton) + MrMC_x 0.0341624938784; X-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_y 0.0489333125579; Y-entry of IMU to Camera vector (expressed in IMU CF) [m] + MrMC_z -0.0250547932613; Z-entry of IMU to Camera vector (expressed in IMU CF) [m] +} Init { @@ -111,7 +111,7 @@ ImgUpdate maxNumIteration 20; doPatchWarping true; Should the patches be warped doFrameVisualisation true; Should the frames be visualized - showCandidates false; Should the possible feature candidates be displayed + showCandidates false; Should the possible feature candidates be displayed visualizePatches false; Should the patches be visualized useDirectMethod true; Should the EKF-innovation be based on direct intensity error (o.w. reprojection error) startLevel 2; Highest patch level which is being employed (must be smaller than the hardcoded template parameter) @@ -178,8 +178,8 @@ ImgUpdate isEnabled 0; Should zero velocity update be applied, only works if MotionDetection.isEnabled is true } - addGlobalBest true; Should the best features of all cameras be added (true) or the best of each camera (false) - histogramEqualize true; + addGlobalBest false; Should the best features of all cameras be added (true) or the best of each camera (false) + histogramEqualize false; } Prediction { diff --git a/launch/rovio_domi.launch b/launch/rovio_domi.launch index 7925e092..1f8636a6 100644 --- a/launch/rovio_domi.launch +++ b/launch/rovio_domi.launch @@ -4,14 +4,14 @@ - - + + - - + +