diff --git a/config/ikalibr-config.yaml b/config/ikalibr-config.yaml index 4c047ea..f786dcc 100644 --- a/config/ikalibr-config.yaml +++ b/config/ikalibr-config.yaml @@ -141,9 +141,17 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters # by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/config/spat-temp-priori.yaml b/config/spat-temp-priori.yaml index ce72dfe..653779c 100644 --- a/config/spat-temp-priori.yaml +++ b/config/spat-temp-priori.yaml @@ -47,4 +47,25 @@ SpatialTemporalPriori: # value: 0.03 # readout time of another rs camera # - key: /sensor2/topic - # value: 0.02 \ No newline at end of file + # value: 0.02 + MIN_VISUAL_SCALE: + # if set for a camera sensor, this is the lower bound for the estimated visual scale. + # if not set, the minimum visual scale is 0.001 + #- key: "/sensor1/topic" + # value: 0.9 + GRAVITY: + # the direction of the gravity vector in the reference IMU when it is in its default, + # gravity-aligned position. The magnitude of the vector should correspond to the magnitude + # set in ikalibr config. This is just an initialization value and it will be further optimized. + r0c0: 0.0 + r1c0: 0.0 + r2c0: -9.8 + INTRI_WEIGHTS: + # these weights specify how much should the optimized intrinsics stick to their priors given as + # camera or IMU configs in the ikalibr config file. + # - key: "/camera1" + # value: 1000 + # - key: "/camera2" + # value: 1 + # - key: "/imu1" + # value: 10000 \ No newline at end of file diff --git a/data/li-calib/Court-01/config-real.yaml b/data/li-calib/Court-01/config-real.yaml index bb4bb22..292cf59 100644 --- a/data/li-calib/Court-01/config-real.yaml +++ b/data/li-calib/Court-01/config-real.yaml @@ -65,8 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-02/config-real.yaml b/data/li-calib/Court-02/config-real.yaml index 413fbc2..c55589f 100644 --- a/data/li-calib/Court-02/config-real.yaml +++ b/data/li-calib/Court-02/config-real.yaml @@ -65,8 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-03/config-real.yaml b/data/li-calib/Court-03/config-real.yaml index cf74a3d..0b88607 100644 --- a/data/li-calib/Court-03/config-real.yaml +++ b/data/li-calib/Court-03/config-real.yaml @@ -65,8 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-04/config-real.yaml b/data/li-calib/Court-04/config-real.yaml index ca4fc8f..486bd3c 100644 --- a/data/li-calib/Court-04/config-real.yaml +++ b/data/li-calib/Court-04/config-real.yaml @@ -65,8 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Court-05/config-real.yaml b/data/li-calib/Court-05/config-real.yaml index 001ae9e..c7504e8 100644 --- a/data/li-calib/Court-05/config-real.yaml +++ b/data/li-calib/Court-05/config-real.yaml @@ -65,8 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-01/config-real.yaml b/data/li-calib/Garage-01/config-real.yaml index aa012b0..c65b4f4 100644 --- a/data/li-calib/Garage-01/config-real.yaml +++ b/data/li-calib/Garage-01/config-real.yaml @@ -65,8 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-02/config-real.yaml b/data/li-calib/Garage-02/config-real.yaml index 3ad38cd..2b44eb3 100644 --- a/data/li-calib/Garage-02/config-real.yaml +++ b/data/li-calib/Garage-02/config-real.yaml @@ -65,8 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-03/config-real.yaml b/data/li-calib/Garage-03/config-real.yaml index eac534b..58b1d66 100644 --- a/data/li-calib/Garage-03/config-real.yaml +++ b/data/li-calib/Garage-03/config-real.yaml @@ -65,8 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-04/config-real.yaml b/data/li-calib/Garage-04/config-real.yaml index 7b6e75d..d5c2247 100644 --- a/data/li-calib/Garage-04/config-real.yaml +++ b/data/li-calib/Garage-04/config-real.yaml @@ -65,8 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/li-calib/Garage-05/config-real.yaml b/data/li-calib/Garage-05/config-real.yaml index 6c575fd..38f16d5 100644 --- a/data/li-calib/Garage-05/config-real.yaml +++ b/data/li-calib/Garage-05/config-real.yaml @@ -65,8 +65,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml index 8945b39..75358c1 100644 --- a/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-14-52-11/config-real.yaml @@ -84,6 +84,10 @@ Configor: SpatTempPrioriPath: "/home/csl/ros_ws/iKalibr/src/ikalibr/config/spat-temp-priori.yaml" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml index 02cf043..cbc065e 100644 --- a/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-14-55-10/config-real.yaml @@ -84,6 +84,10 @@ Configor: SpatTempPrioriPath: "/home/csl/ros_ws/iKalibr/src/ikalibr/config/spat-temp-priori.yaml" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml index e8ae3d8..9d67871 100644 --- a/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-15-22-04/config-real.yaml @@ -66,8 +66,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml index 9ed7489..e3d4919 100644 --- a/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-15-30-05/config-real.yaml @@ -66,8 +66,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml index 3cc20fd..de2341d 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-34-01/config-real.yaml @@ -66,8 +66,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml index 78595d2..e3410fd 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-42-32/config-real.yaml @@ -84,8 +84,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml index 12c4219..9121eae 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-44-05/config-real.yaml @@ -84,8 +84,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml index d6652b3..08fe9c3 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-49-39/config-real.yaml @@ -61,8 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml index 2bda1d8..ea1b427 100644 --- a/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-16-55-29/config-real.yaml @@ -80,8 +80,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml index 1bf89da..83e0ce3 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-10-20/config-real.yaml @@ -82,8 +82,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml index 3e80952..4318c8e 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-12-05/config-real.yaml @@ -82,8 +82,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml index 9765ccd..5ccf7c0 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-17-17/config-real.yaml @@ -66,8 +66,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml index 84ecbfd..6109814 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-33-29/config-real.yaml @@ -66,8 +66,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml b/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml index da34b0f..6d5d64c 100644 --- a/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml +++ b/data/real-world/iKalibr-data-2024-06-25-17-37-20/config-real.yaml @@ -78,8 +78,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/river/data_202392614595/config-real.yaml b/data/river/data_202392614595/config-real.yaml index d5b1a3a..a76c252 100644 --- a/data/river/data_202392614595/config-real.yaml +++ b/data/river/data_202392614595/config-real.yaml @@ -57,8 +57,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/river/data_202392615561/config-real.yaml b/data/river/data_202392615561/config-real.yaml index ed2e101..d81f0fa 100644 --- a/data/river/data_202392615561/config-real.yaml +++ b/data/river/data_202392615561/config-real.yaml @@ -57,8 +57,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/river/data_202392616822/config-real.yaml b/data/river/data_202392616822/config-real.yaml index 8bf2dae..188814e 100644 --- a/data/river/data_202392616822/config-real.yaml +++ b/data/river/data_202392616822/config-real.yaml @@ -57,8 +57,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-1/config-real.yaml b/data/tum/rs-seq-1/config-real.yaml index f712c7d..23f3f64 100644 --- a/data/tum/rs-seq-1/config-real.yaml +++ b/data/tum/rs-seq-1/config-real.yaml @@ -61,8 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-10/config-real.yaml b/data/tum/rs-seq-10/config-real.yaml index 8068855..9c438dd 100644 --- a/data/tum/rs-seq-10/config-real.yaml +++ b/data/tum/rs-seq-10/config-real.yaml @@ -61,8 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-2/config-real.yaml b/data/tum/rs-seq-2/config-real.yaml index 85a6914..aa36902 100644 --- a/data/tum/rs-seq-2/config-real.yaml +++ b/data/tum/rs-seq-2/config-real.yaml @@ -61,8 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-3/config-real.yaml b/data/tum/rs-seq-3/config-real.yaml index 96d2f18..96451fd 100644 --- a/data/tum/rs-seq-3/config-real.yaml +++ b/data/tum/rs-seq-3/config-real.yaml @@ -61,8 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-4/config-real.yaml b/data/tum/rs-seq-4/config-real.yaml index 8214144..15ad729 100644 --- a/data/tum/rs-seq-4/config-real.yaml +++ b/data/tum/rs-seq-4/config-real.yaml @@ -61,8 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-5/config-real.yaml b/data/tum/rs-seq-5/config-real.yaml index 156cedc..06a5ecb 100644 --- a/data/tum/rs-seq-5/config-real.yaml +++ b/data/tum/rs-seq-5/config-real.yaml @@ -61,8 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-6/config-real.yaml b/data/tum/rs-seq-6/config-real.yaml index 15fd205..0fb645a 100644 --- a/data/tum/rs-seq-6/config-real.yaml +++ b/data/tum/rs-seq-6/config-real.yaml @@ -61,8 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-7/config-real.yaml b/data/tum/rs-seq-7/config-real.yaml index 82a06c7..3d3c5ba 100644 --- a/data/tum/rs-seq-7/config-real.yaml +++ b/data/tum/rs-seq-7/config-real.yaml @@ -61,8 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-8/config-real.yaml b/data/tum/rs-seq-8/config-real.yaml index 45f9c0f..1773b97 100644 --- a/data/tum/rs-seq-8/config-real.yaml +++ b/data/tum/rs-seq-8/config-real.yaml @@ -61,8 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/tum/rs-seq-9/config-real.yaml b/data/tum/rs-seq-9/config-real.yaml index 2744250..db4da6c 100644 --- a/data/tum/rs-seq-9/config-real.yaml +++ b/data/tum/rs-seq-9/config-real.yaml @@ -61,8 +61,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: false + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/desk-fast/config-real.yaml b/data/vector/desk-fast/config-real.yaml index 164e60d..14448b9 100644 --- a/data/vector/desk-fast/config-real.yaml +++ b/data/vector/desk-fast/config-real.yaml @@ -93,8 +93,16 @@ Configor: GravityNorm: 9.79 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/hdr-fast/config-real.yaml b/data/vector/hdr-fast/config-real.yaml index 152465a..875fd4f 100644 --- a/data/vector/hdr-fast/config-real.yaml +++ b/data/vector/hdr-fast/config-real.yaml @@ -88,8 +88,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/mountain-fast/config-real.yaml b/data/vector/mountain-fast/config-real.yaml index 4e68c88..9d89b4e 100644 --- a/data/vector/mountain-fast/config-real.yaml +++ b/data/vector/mountain-fast/config-real.yaml @@ -88,8 +88,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/robot-fast/config-real.yaml b/data/vector/robot-fast/config-real.yaml index d501fe6..ede9d77 100644 --- a/data/vector/robot-fast/config-real.yaml +++ b/data/vector/robot-fast/config-real.yaml @@ -88,8 +88,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/data/vector/sofa-fast/config-real.yaml b/data/vector/sofa-fast/config-real.yaml index 3a04376..804a267 100644 --- a/data/vector/sofa-fast/config-real.yaml +++ b/data/vector/sofa-fast/config-real.yaml @@ -88,8 +88,16 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/docs/details/config_template_note.md b/docs/details/config_template_note.md index 06c6252..c2764fd 100644 --- a/docs/details/config_template_note.md +++ b/docs/details/config_template_note.md @@ -68,10 +68,11 @@ Configor: Type: "AWR1843BOOST_CUSTOM" Weight: 10.0 # 1. Velodyne LiDARs: VLP_16_PACKET, VLP_POINTS - # 2. Ouster LiDARs: OUSTER_POINTS + # 2. Ouster LiDARs: OUSTER_POINTS (Ouster pointcloud with uint8_t ring field) # 3. Hesai Pandar XT LiDARs: PANDAR_XT_POINTS # 4. Livox LiDARs: LIVOX_CUSTOM (the official 'xfer_format'=1, mid-360 and avia is recommend) # 5. Robosense LiDARs: RSLIDAR_POINTS (tested on RS-Helios-16P) + # 6. Ouster LiDARs: OUSTER_POINTS_RING_16 (Ouster pointcloud with uint16_t ring field) LiDARTopics: # if no LiDAR is integrated in your sensor suite, just comment out the following key items - key: "/lidar0/scan" @@ -161,9 +162,17 @@ Configor: GravityNorm: 9.8 # priori about spatiotemporal parameters, given by corresponding config file path SpatTempPrioriPath: "" + # if nonempty, this should point to a knots.yaml file saved by a previous run of ikalibr + # splines will be loaded and initialized from these priors and their knots with priors + # will not be optimized (unless OptSplines is true) + SplinesPrioriPath: "" # if sensor are hardware-synchronized, you could choose to fix temporal parameters # by setting this field to 'false' OptTemporalParams: true + # if true, the last batch optimization will also optimize IMU nonlinearities + OptImuNonlinearity: false + # if true, even spline knots with priori information will be optimized + OptSplines: true # the range where the time offsets would be optimized. # make sure this range contains the ground truth of time offsets # If you're not sure, make this field large, but this could lead to longer optimization time diff --git a/docs/details/sensor_type.md b/docs/details/sensor_type.md index dd5af64..a01ba57 100644 --- a/docs/details/sensor_type.md +++ b/docs/details/sensor_type.md @@ -27,7 +27,7 @@ Currently, the following types of sensor are supported in `iKalibr`: + **LiDAR** type: corresponding type definition can be found [here](https://github.com/Unsigned-Long/iKalibr/blob/master/include/sensor/sensor_model.h#L109-L122). The type of LiDAR is more prone to error than others, most of the time error happens when decoding time stamps of points if the wrong LiDAR type is passed in `iKalibr`. + *Velodyne LiDARs*: `VLP_16_PACKET`, `VLP_POINTS`. - + *Ouster LiDARs*: `OUSTER_POINTS`. + + *Ouster LiDARs*: `OUSTER_POINTS`, `OUSTER_POINTS_RING_16`. + *Hesai Pandar XT LiDARs*: `PANDAR_XT_POINTS`. diff --git a/include/calib/calib_param_manager.h b/include/calib/calib_param_manager.h index 3249ee2..a3e3fa6 100644 --- a/include/calib/calib_param_manager.h +++ b/include/calib/calib_param_manager.h @@ -213,6 +213,10 @@ struct CalibParamManager { std::map Camera; std::map RGBD; + std::map PrioriIMU; + std::map PrioriCamera; + std::map PrioriRGBD; + static ns_veta::PinholeIntrinsic::Ptr LoadCameraIntri( const std::string &filename, CerealArchiveType::Enum archiveType = CerealArchiveType::Enum::YAML); @@ -245,6 +249,9 @@ struct CalibParamManager { // S2Manifold Eigen::Vector3d GRAVITY; + // true for every knot that has been loaded from priori information + std::map> splinesPrioriKnots; + public: // the constructor explicit CalibParamManager(const std::vector &imuTopics = {}, diff --git a/include/calib/estimator.h b/include/calib/estimator.h index b8db93b..ab7f21a 100644 --- a/include/calib/estimator.h +++ b/include/calib/estimator.h @@ -485,6 +485,17 @@ class Estimator : public ceres::Problem { double *TO_Sen2ToRef, double weight); + void AddPriorEqualityConstraint(const double *prior, double *address, double weight); + void AddPriorEqualityConstraint(const Eigen::Vector3d &prior, + Eigen::Vector3d &address, + double weight); + void AddPriorEqualityConstraint(const Eigen::Vector6d &prior, + Eigen::Vector6d &address, + double weight); + void AddPriorEqualityConstraint(const Sophus::SO3d &prior, + Sophus::SO3d &address, + double weight); + void PrintUninvolvedKnots() const; void AddVisualVelocityDepthFactor(Eigen::Vector3d *LIN_VEL_CmToWInCm, diff --git a/include/calib/spat_temp_priori.h b/include/calib/spat_temp_priori.h index 769fb85..bdfb74a 100644 --- a/include/calib/spat_temp_priori.h +++ b/include/calib/spat_temp_priori.h @@ -62,6 +62,9 @@ class SpatialTemporalPriori { std::map TO_Sen1ToSen2; // readout time of rs cameras std::map RS_READOUT; + Eigen::Vector3d GRAVITY; + std::map MIN_VISUAL_SCALE; + std::map INTRI_WEIGHTS; public: SpatialTemporalPriori() = default; @@ -76,6 +79,22 @@ class SpatialTemporalPriori { [[nodiscard]] const std::map &GetReadout() const; + [[nodiscard]] std::optional GetGravity() const; + + [[nodiscard]] const std::map &GetMinVisualScale() const; + + [[nodiscard]] bool HasSO3ToBr(const std::string& topic) const; + + [[nodiscard]] std::optional GetSO3ToBr(const std::string& topic) const; + + [[nodiscard]] bool HasPosInBr(const std::string& topic) const; + + [[nodiscard]] std::optional GetPosInBr(const std::string& topic) const; + + [[nodiscard]] bool HasTOToBr(const std::string& topic) const; + + [[nodiscard]] std::optional GetTOToBr(const std::string& topic) const; + void CheckValidityWithConfigor() const; void AddSpatTempPrioriConstraint(Estimator &estimator, CalibParamManager &parMagr) const; @@ -94,12 +113,17 @@ class SpatialTemporalPriori { return {false, {}}; } +private: + mutable std::set hasSO3ToBr; + mutable std::set hasPosToBr; + mutable std::set hasTOToBr; + public: // Serialization template void serialize(Archive &ar) { ar(CEREAL_NVP(SO3_Sen1ToSen2), CEREAL_NVP(POS_Sen1InSen2), CEREAL_NVP(TO_Sen1ToSen2), - CEREAL_NVP(RS_READOUT)); + CEREAL_NVP(RS_READOUT), CEREAL_NVP(GRAVITY), CEREAL_NVP(MIN_VISUAL_SCALE)); } // save the parameters to file using cereal library diff --git a/include/config/configor.h b/include/config/configor.h index 70d5ae4..c343a53 100644 --- a/include/config/configor.h +++ b/include/config/configor.h @@ -254,9 +254,12 @@ struct Configor { static struct Prior { static std::string SpatTempPrioriPath; + static std::string SplinesPrioriPath; static double GravityNorm; static constexpr int SplineOrder = 4; static bool OptTemporalParams; + static bool OptSplines; + static bool OptImuNonlinearity; static double TimeOffsetPadding; static double ReadoutTimePadding; static double MapDownSample; @@ -319,6 +322,8 @@ struct Configor { ar(CEREAL_NVP(SpatTempPrioriPath), CEREAL_NVP(GravityNorm), CEREAL_NVP(OptTemporalParams), CEREAL_NVP(TimeOffsetPadding), CEREAL_NVP(ReadoutTimePadding), CEREAL_NVP(MapDownSample), + CEREAL_NVP(OptImuNonlinearity), CEREAL_NVP(SplinesPrioriPath), + CEREAL_NVP(OptSplines), cereal::make_nvp("KnotTimeDist", knotTimeDist), cereal::make_nvp("NDTLiDAROdometer", ndtLiDAROdometer), cereal::make_nvp("LiDARDataAssociate", lidarDataAssociate)); diff --git a/include/factor/prior_equality_factor.hpp b/include/factor/prior_equality_factor.hpp new file mode 100644 index 0000000..8b41c8e --- /dev/null +++ b/include/factor/prior_equality_factor.hpp @@ -0,0 +1,103 @@ +// iKalibr: Unified Targetless Spatiotemporal Calibration Framework +// Copyright 2024, the School of Geodesy and Geomatics (SGG), Wuhan University, China +// https://github.com/Unsigned-Long/iKalibr.git +// +// Author: Shuolong Chen (shlchen@whu.edu.cn) +// GitHub: https://github.com/Unsigned-Long +// ORCID: 0000-0002-5283-9057 +// +// Purpose: See .h/.hpp file. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * The names of its contributors can not be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef IKALIBR_PRIOR_EQUALITY_FACTOR_HPP +#define IKALIBR_PRIOR_EQUALITY_FACTOR_HPP + +#include "ctraj/utils/eigen_utils.hpp" +#include "ctraj/utils/sophus_utils.hpp" +#include "ceres/dynamic_autodiff_cost_function.h" +#include "util/utils.h" + +namespace { +bool IKALIBR_UNIQUE_NAME(_2_) = ns_ikalibr::_1_(__FILE__); +} + +namespace ns_ikalibr { +template +struct PriorEqualityFactor { +private: + const double* _prior; + double _weight; + +public: + explicit PriorEqualityFactor(const double *prior, double weight) + : _prior(prior), + _weight(weight) {} + + static auto Create(const double *prior, double weight) { + return new ceres::DynamicAutoDiffCostFunction( + new PriorEqualityFactor<1>(prior, weight)); + } + + static auto Create(const Eigen::Vector3d &prior, double weight) { + return new ceres::DynamicAutoDiffCostFunction( + new PriorEqualityFactor<3>(prior.data(), weight)); + } + + static auto Create(const Eigen::Vector6d &prior, double weight) { + return new ceres::DynamicAutoDiffCostFunction( + new PriorEqualityFactor<6>(prior.data(), weight)); + } + + static auto Create(const Sophus::SO3d &prior, double weight) { + return new ceres::DynamicAutoDiffCostFunction( + new PriorEqualityFactor<4>(prior.data(), weight)); + } + + static std::size_t TypeHashCode() { return typeid(PriorEqualityFactor).hash_code(); } + +public: + /** + * param blocks: + * [ Prior | ... ] + */ + template + bool operator()(T const *const *sKnots, T *sResiduals) const { + const T* const actual = sKnots[0]; + + Eigen::Map> residuals(sResiduals); + for (size_t i = 0; i < PriorSize; ++i) { + residuals(i) = T(_weight) * (_prior[i] - actual[i]); + } + + return true; + } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace ns_ikalibr + +#endif // IKALIBR_PRIOR_EQUALITY_FACTOR_HPP diff --git a/include/sensor/lidar_data_loader.h b/include/sensor/lidar_data_loader.h index c6a8aff..2d54abb 100644 --- a/include/sensor/lidar_data_loader.h +++ b/include/sensor/lidar_data_loader.h @@ -188,6 +188,18 @@ class OusterLiDAR : public LiDARDataLoader { LiDARFrame::Ptr UnpackScan(const rosbag::MessageInstance &msgInstance) override; }; +class OusterRing16LiDAR : public LiDARDataLoader { +public: + using Ptr = std::shared_ptr; + +public: + explicit OusterRing16LiDAR(LidarModelType lidarModel); + + static OusterRing16LiDAR::Ptr Create(LidarModelType lidarModel); + + LiDARFrame::Ptr UnpackScan(const rosbag::MessageInstance &msgInstance) override; +}; + class PandarXTLiDAR : public LiDARDataLoader { public: using Ptr = std::shared_ptr; diff --git a/include/sensor/sensor_model.h b/include/sensor/sensor_model.h index d383157..1cff23b 100644 --- a/include/sensor/sensor_model.h +++ b/include/sensor/sensor_model.h @@ -118,6 +118,8 @@ struct LidarModel { LIVOX_CUSTOM, RSLIDAR_POINTS, + + OUSTER_POINTS_RING_16, }; static std::string UnsupportedLiDARModelMsg(const std::string &modelStr); diff --git a/include/solver/batch_opt_option.hpp b/include/solver/batch_opt_option.hpp index 6240cf3..3590b4e 100644 --- a/include/solver/batch_opt_option.hpp +++ b/include/solver/batch_opt_option.hpp @@ -137,6 +137,13 @@ struct BatchOptOption { if (Configor::IsEventIntegrated()) { options = MergeOptions(options, AryToVecWithAppend(MultiEventIMU)); } + // Optimize IMU nonlinearity in the last batch + if (!options.empty() && Configor::Prior::OptImuNonlinearity) { + auto& opt = options.back(); + opt |= Opt::OPT_GYRO_MAP_COEFF; + opt |= Opt::OPT_ACCE_MAP_COEFF; + opt |= Opt::OPT_SO3_AtoG; + } } if (options.empty()) { throw Status(Status::CRITICAL, "unknown error happened! (unknown sensor suite)"); diff --git a/include/solver/calib_solver.h b/include/solver/calib_solver.h index 7077a81..b1f2562 100644 --- a/include/solver/calib_solver.h +++ b/include/solver/calib_solver.h @@ -232,6 +232,12 @@ class CalibSolver { */ virtual ~CalibSolver(); + /** + * get the raw timestamp of the beginning of the used data + * @return the timestamp + */ + double GetRawStartTimestamp() const; + protected: /** * transform an input veta using given transformation information, if scale is provide, @@ -244,6 +250,12 @@ class CalibSolver { const ns_veta::Posed &curToNew, double scale); + /** + * Load spline bundle priori data. + * @param splinesPrioriPath path to the file saved by CalibSolverIO::SaveBSplines() + */ + void LoadSplineBundlePriori(const std::string& splinesPrioriPath); + /** * align vectors to a new coordinate frame where gravity pointing to negative z-axis. * the splines (both rotation and translation splines), as well as the gravity vector would be diff --git a/include/util/cloud_define.hpp b/include/util/cloud_define.hpp index 9b40931..28fb4bb 100644 --- a/include/util/cloud_define.hpp +++ b/include/util/cloud_define.hpp @@ -112,6 +112,25 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR8Y, (std::uint8_t, ring, ring) (std::uint32_t, t, t)) +// OUSTER lidar with uint16_t ring +struct EIGEN_ALIGN16 PointXYZIR16Y { + PCL_ADD_POINT4D; // quad-word XYZ + float intensity; // laser intensity reading + std::uint16_t ring; // laser ring number + std::uint32_t t; + float range; + + PCL_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment +}; + +POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR16Y, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (std::uint16_t, ring, ring) + (std::uint32_t, t, t)) + struct EIGEN_ALIGN16 PointXYZITR { PCL_ADD_POINT4D; float intensity; @@ -147,6 +166,9 @@ using PosITPointCloud = pcl::PointCloud; using OusterPoint = PointXYZIR8Y; using OusterPointCloud = pcl::PointCloud; +using OusterRing16Point = PointXYZIR16Y; +using OusterRing16PointCloud = pcl::PointCloud; + using ColorPoint = pcl::PointXYZRGBA; using ColorPointCloud = pcl::PointCloud; diff --git a/src/calib/calib_param_manager.cpp b/src/calib/calib_param_manager.cpp index 20cf4d8..3f43c6a 100644 --- a/src/calib/calib_param_manager.cpp +++ b/src/calib/calib_param_manager.cpp @@ -158,6 +158,22 @@ CalibParamManager::Ptr CalibParamManager::InitParamsFromConfigor() { } } + for (const auto& [topic, intri] : parMarg->INTRI.IMU) { + parMarg->INTRI.PrioriIMU[topic] = IMUIntrinsics::Create(); + *parMarg->INTRI.PrioriIMU[topic] = *parMarg->INTRI.IMU[topic]; + } + for (const auto& [topic, intri] : parMarg->INTRI.Camera) { + parMarg->INTRI.PrioriCamera[topic] = ns_veta::PinholeIntrinsic::Create(0, 0, 0, 0, 0, 0); + *parMarg->INTRI.PrioriCamera[topic] = *parMarg->INTRI.Camera[topic]; + } + for (const auto& [topic, intri] : parMarg->INTRI.RGBD) { + parMarg->INTRI.PrioriRGBD[topic] = RGBDIntrinsics::Create( + ns_veta::PinholeIntrinsic::Create(0, 0, 0, 0, 0, 0), 0, 0); + *parMarg->INTRI.PrioriRGBD[topic]->intri = *parMarg->INTRI.RGBD[topic]->intri; + parMarg->INTRI.PrioriRGBD[topic]->alpha = parMarg->INTRI.RGBD[topic]->alpha; + parMarg->INTRI.PrioriRGBD[topic]->beta = parMarg->INTRI.RGBD[topic]->beta; + } + // align to the negative 'z' axis parMarg->GRAVITY = Eigen::Vector3d(0.0, 0.0, -Configor::Prior::GravityNorm); diff --git a/src/calib/estimator.cpp b/src/calib/estimator.cpp index 8a5b85f..43be6d5 100644 --- a/src/calib/estimator.cpp +++ b/src/calib/estimator.cpp @@ -42,6 +42,7 @@ #include "factor/linear_knots_factor.hpp" #include "factor/prior_extri_pos_factor.hpp" #include "factor/prior_extri_so3_factor.hpp" +#include "factor/prior_equality_factor.hpp" #include "factor/prior_time_offset_factor.hpp" #include "factor/radar_inertial_align_factor.hpp" #include "factor/radar_inertial_rot_align_factor.hpp" @@ -105,6 +106,7 @@ void Estimator::AddRdKnotsData(std::vector ¶mBlockVec, const Estimator::SplineBundleType::RdSplineType &spline, const Estimator::SplineMetaType &splineMeta, bool setToConst) { + const auto& prioriKnots = parMagr->splinesPrioriKnots.at(Configor::Preference::SCALE_SPLINE); // for each segment for (const auto &seg : splineMeta.segments) { // the factor 'seg.dt * 0.5' is the treatment for numerical accuracy @@ -118,7 +120,12 @@ void Estimator::AddRdKnotsData(std::vector ¶mBlockVec, paramBlockVec.push_back(data); // set this param block to be constant - if (setToConst) { + bool setKnotToConst = setToConst; + if (!setToConst && !Configor::Prior::OptSplines) { + if (i < prioriKnots.size()) + setKnotToConst = prioriKnots.at(i); + } + if (setKnotToConst) { this->SetParameterBlockConstant(data); } } @@ -129,6 +136,7 @@ void Estimator::AddSo3KnotsData(std::vector ¶mBlockVec, const Estimator::SplineBundleType::So3SplineType &spline, const Estimator::SplineMetaType &splineMeta, bool setToConst) { + const auto& prioriKnots = parMagr->splinesPrioriKnots.at(Configor::Preference::SO3_SPLINE); // for each segment for (const auto &seg : splineMeta.segments) { // the factor 'seg.dt * 0.5' is the treatment for numerical accuracy @@ -142,7 +150,12 @@ void Estimator::AddSo3KnotsData(std::vector ¶mBlockVec, paramBlockVec.push_back(data); // set this param block to be constant - if (setToConst) { + bool setKnotToConst = setToConst; + if (!setToConst && !Configor::Prior::OptSplines) { + if (i < prioriKnots.size()) + setKnotToConst = prioriKnots.at(i); + } + if (setKnotToConst) { this->SetParameterBlockConstant(data); } } @@ -1822,6 +1835,88 @@ void Estimator::AddPriorTimeOffsetConstraint(const double &TO_Sen1ToSen2, this->AddResidualBlock(costFunc, nullptr, paramBlockVec); } +void Estimator::AddPriorEqualityConstraint(const double *prior, double *address, double weight) { + // create a cost function + auto costFunc = PriorEqualityFactor<1>::Create(prior, weight); + + // prior + costFunc->AddParameterBlock(1); + + // set Residuals + costFunc->SetNumResiduals(1); + + // organize the param block vector + std::vector paramBlockVec; + + paramBlockVec.push_back(address); + + // pass to problem + this->AddResidualBlock(costFunc, nullptr, paramBlockVec); +} + +void Estimator::AddPriorEqualityConstraint(const Eigen::Vector3d &prior, + Eigen::Vector3d &address, + double weight) { + // create a cost function + auto costFunc = PriorEqualityFactor<3>::Create(prior, weight); + + // prior + costFunc->AddParameterBlock(3); + + // set Residuals + costFunc->SetNumResiduals(3); + + // organize the param block vector + std::vector paramBlockVec; + + paramBlockVec.push_back(address.data()); + + // pass to problem + this->AddResidualBlock(costFunc, nullptr, paramBlockVec); +} + +void Estimator::AddPriorEqualityConstraint(const Eigen::Vector6d &prior, + Eigen::Vector6d &address, + double weight) { + // create a cost function + auto costFunc = PriorEqualityFactor<6>::Create(prior, weight); + + // prior + costFunc->AddParameterBlock(6); + + // set Residuals + costFunc->SetNumResiduals(6); + + // organize the param block vector + std::vector paramBlockVec; + + paramBlockVec.push_back(address.data()); + + // pass to problem + this->AddResidualBlock(costFunc, nullptr, paramBlockVec); +} + +void Estimator::AddPriorEqualityConstraint(const Sophus::SO3d &prior, + Sophus::SO3d &address, + double weight) { + // create a cost function + auto costFunc = PriorEqualityFactor<4>::Create(prior, weight); + + // prior + costFunc->AddParameterBlock(4); + + // set Residuals + costFunc->SetNumResiduals(4); + + // organize the param block vector + std::vector paramBlockVec; + + paramBlockVec.push_back(address.data()); + + // pass to problem + this->AddResidualBlock(costFunc, nullptr, paramBlockVec); +} + void Estimator::PrintUninvolvedKnots() const { { const auto &so3Knots = splines->GetSo3Spline(Configor::Preference::SO3_SPLINE).GetKnots(); diff --git a/src/calib/spat_temp_priori.cpp b/src/calib/spat_temp_priori.cpp index 11853f2..7e79855 100644 --- a/src/calib/spat_temp_priori.cpp +++ b/src/calib/spat_temp_priori.cpp @@ -68,6 +68,81 @@ const std::map& SpatialTemporalPriori::GetReadout() const { return RS_READOUT; } +std::optional SpatialTemporalPriori::GetGravity() const { + if (GRAVITY == Eigen::Vector3d::Zero()) + return {}; + return GRAVITY; +} + +const std::map& SpatialTemporalPriori::GetMinVisualScale() const { + return MIN_VISUAL_SCALE; +} + +bool SpatialTemporalPriori::HasSO3ToBr(const std::string& topic) const { + return hasSO3ToBr.find(topic) != hasSO3ToBr.end(); +} + +std::optional SpatialTemporalPriori::GetSO3ToBr(const std::string& topic) const { + if (!HasSO3ToBr(topic)) + return {}; + + const auto& refImu = Configor::DataStream::ReferIMU; + + for (const auto& [fromTo, so3] : this->SO3_Sen1ToSen2) { + const auto& [from, to] = fromTo; + if (to == refImu && from == topic) { + return so3; + } else if (from == refImu && to == topic) { + return so3.inverse(); + } + } + return {}; +} + +bool SpatialTemporalPriori::HasPosInBr(const std::string& topic) const { + return hasPosToBr.find(topic) != hasPosToBr.end(); +} + +std::optional SpatialTemporalPriori::GetPosInBr(const std::string& topic) const { + if (!HasPosInBr(topic)) + return {}; + + const auto& refImu = Configor::DataStream::ReferIMU; + + for (const auto& [fromTo, pos] : this->POS_Sen1InSen2) { + const auto& [from, to] = fromTo; + if (to == refImu && from == topic) { + return pos; + } else if (from == refImu && to == topic) { + // we know the so3 exists because of the HasPosToBr() check above + const auto so3 = GetSO3ToBr(to); + return (*so3) * (-pos); + } + } + return {}; +} + +bool SpatialTemporalPriori::HasTOToBr(const std::string& topic) const { + return hasTOToBr.find(topic) != hasTOToBr.end(); +} + +std::optional SpatialTemporalPriori::GetTOToBr(const std::string& topic) const { + if (!HasTOToBr(topic)) + return {}; + + const auto& refImu = Configor::DataStream::ReferIMU; + + for (const auto& [fromTo, offset] : this->TO_Sen1ToSen2) { + const auto& [from, to] = fromTo; + if (to == refImu && from == topic) { + return offset; + } else if (from == refImu && to == topic) { + return -offset; + } + } + return {}; +} + void SpatialTemporalPriori::CheckValidityWithConfigor() const { // check map if its ambiguous if (auto [res, p] = IsMapAmbiguous(this->SO3_Sen1ToSen2); res) { @@ -173,6 +248,60 @@ void SpatialTemporalPriori::CheckValidityWithConfigor() const { readout, sensor, RT_PADDING); } } + + if (GRAVITY != Eigen::Vector3d::Zero()) { + if (std::abs(GRAVITY.norm() - Configor::Prior::GravityNorm) > 1e-3) { + throw Status(Status::ERROR, "the given prior gravity vector [{}, {}, {}] does not have " + "norm equal to Prior::GravityNorm ({})! The vector's norm is: {}.", + GRAVITY.x(), GRAVITY.y(), GRAVITY.z(), Configor::Prior::GravityNorm, + GRAVITY.norm()); + } + } + + for (const auto& [cam, _] : MIN_VISUAL_SCALE) { + if (optCamModelType.count(cam) == 0) { + throw Status(Status::ERROR, "MIN_VISUAL_SCALE defined for topic '{}' which is not an " + "optical camera topic!", cam); + } + } + + for (const auto& [topic, weight] : INTRI_WEIGHTS) { + if (optCamModelType.count(topic) == 0 && + Configor::DataStream::EventTopics.count(topic) == 0 && + Configor::DataStream::IMUTopics.count(topic) == 0) { + throw Status(Status::ERROR, "INTRI_WEIGHTS defined for topic '{}' which is neither IMU " + "nor camera topic!", topic); + } + if (weight < 0) { + throw Status(Status::ERROR, "INTRI_WEIGHTS defined for topic '{}' is negative!"); + } + } + + const auto& refImu = Configor::DataStream::ReferIMU; + + for (const auto& [fromTo, _] : SO3_Sen1ToSen2) { + const auto& [from, to] = fromTo; + if (from == refImu) + hasSO3ToBr.insert(to); + else if (to == refImu) + hasSO3ToBr.insert(from); + } + + for (const auto& [fromTo, _] : POS_Sen1InSen2) { + const auto& [from, to] = fromTo; + if (from == refImu) + hasPosToBr.insert(to); + else if (to == refImu && hasSO3ToBr.find(from) != hasSO3ToBr.end()) + hasPosToBr.insert(from); + } + + for (const auto& [fromTo, _] : TO_Sen1ToSen2) { + const auto& [from, to] = fromTo; + if (from == refImu) + hasTOToBr.insert(to); + else if (to == refImu) + hasTOToBr.insert(from); + } } void SpatialTemporalPriori::AddSpatTempPrioriConstraint(Estimator& estimator, @@ -255,6 +384,10 @@ void SpatialTemporalPriori::AddSpatTempPrioriConstraint(Estimator& estimator, // only one of the param block has been added to problem, we then add the constraint, // to make sure a unique least-squares solution estimator.AddPriorExtriSO3Constraint(Sen1ToSen2, rot1, rot2, PrioriWeight); + if (sen1 == RefIMU && estimator.HasParameterBlock(rot1->data())) { + *rot1 = Sophus::SO3d(); + estimator.SetParameterBlockConstant(rot1->data()); + } } } for (const auto& [sensorPair, Sen1InSen2] : this->POS_Sen1InSen2) { @@ -270,6 +403,10 @@ void SpatialTemporalPriori::AddSpatTempPrioriConstraint(Estimator& estimator, } else if (estimator.HasParameterBlock(pos1->data()) || estimator.HasParameterBlock(pos2->data())) { estimator.AddPriorExtriPOSConstraint(Sen1InSen2, pos1, rot2, pos2, PrioriWeight); + if (sen1 == RefIMU && estimator.HasParameterBlock(pos1->data())) { + *pos1 = Eigen::Vector3d::Zero(); + estimator.SetParameterBlockConstant(pos1->data()); + } } } for (const auto& [sensorPair, Sen1ToSen2] : this->TO_Sen1ToSen2) { @@ -283,6 +420,10 @@ void SpatialTemporalPriori::AddSpatTempPrioriConstraint(Estimator& estimator, } } else if (estimator.HasParameterBlock(to1) || estimator.HasParameterBlock(to2)) { estimator.AddPriorTimeOffsetConstraint(Sen1ToSen2, to1, to2, PrioriWeight); + if (sen1 == RefIMU && estimator.HasParameterBlock(to1)) { + *to1 = 0.0; + estimator.SetParameterBlockConstant(to1); + } } } // readout times (we set them as constraints in optimization) @@ -293,6 +434,80 @@ void SpatialTemporalPriori::AddSpatTempPrioriConstraint(Estimator& estimator, estimator.SetParameterBlockConstant(data); } } + const auto gravityPrior = this->GetGravity(); + if (gravityPrior) { + auto gravity = &parMagr.GRAVITY; + *gravity = *gravityPrior; + if (estimator.HasParameterBlock(gravity->data())) { + estimator.SetParameterBlockConstant(gravity->data()); + } + } + for (const auto& [topic, weight] : this->INTRI_WEIGHTS) { + if (parMagr.INTRI.IMU.count(topic) > 0) { + const auto& intri = parMagr.INTRI.IMU.at(topic); + const auto& prioriIntri = parMagr.INTRI.PrioriIMU.at(topic); + if (std::isinf(weight)) { + // set params constant + if (estimator.HasParameterBlock(intri->ACCE.BIAS.data())) { + estimator.SetParameterBlockConstant(intri->ACCE.BIAS.data()); + } + if (estimator.HasParameterBlock(intri->ACCE.MAP_COEFF.data())) { + estimator.SetParameterBlockConstant(intri->ACCE.MAP_COEFF.data()); + } + if (estimator.HasParameterBlock(intri->GYRO.BIAS.data())) { + estimator.SetParameterBlockConstant(intri->GYRO.BIAS.data()); + } + if (estimator.HasParameterBlock(intri->GYRO.MAP_COEFF.data())) { + estimator.SetParameterBlockConstant(intri->GYRO.MAP_COEFF.data()); + } + if (estimator.HasParameterBlock(intri->SO3_AtoG.data())) { + estimator.SetParameterBlockConstant(intri->SO3_AtoG.data()); + } + } else if (std::isfinite(weight)) { + estimator.AddPriorEqualityConstraint( + prioriIntri->ACCE.BIAS, intri->ACCE.BIAS, weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->ACCE.MAP_COEFF, intri->ACCE.MAP_COEFF, weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->GYRO.BIAS, intri->GYRO.BIAS, weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->GYRO.MAP_COEFF, intri->GYRO.MAP_COEFF, weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->SO3_AtoG, intri->SO3_AtoG, weight); + } + } + if (parMagr.INTRI.Camera.count(topic) > 0 || parMagr.INTRI.RGBD.count(topic) > 0) { + const auto isRGBD = parMagr.INTRI.RGBD.count(topic) > 0; + const auto& intri = isRGBD ? + parMagr.INTRI.RGBD.at(topic)->intri : parMagr.INTRI.Camera.at(topic); + const auto& prioriIntri = isRGBD ? + parMagr.INTRI.PrioriRGBD.at(topic)->intri : parMagr.INTRI.PrioriCamera.at(topic); + if (std::isinf(weight)) { + // set params constant + if (estimator.HasParameterBlock(intri->FXAddress())) { + estimator.SetParameterBlockConstant(intri->FXAddress()); + } + if (estimator.HasParameterBlock(intri->FYAddress())) { + estimator.SetParameterBlockConstant(intri->FYAddress()); + } + if (estimator.HasParameterBlock(intri->CXAddress())) { + estimator.SetParameterBlockConstant(intri->CXAddress()); + } + if (estimator.HasParameterBlock(intri->CYAddress())) { + estimator.SetParameterBlockConstant(intri->CYAddress()); + } + } else if (std::isfinite(weight)) { + estimator.AddPriorEqualityConstraint( + prioriIntri->FXAddress(), intri->FXAddress(), weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->FYAddress(), intri->FYAddress(), weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->CXAddress(), intri->CXAddress(), weight); + estimator.AddPriorEqualityConstraint( + prioriIntri->CYAddress(), intri->CYAddress(), weight); + } + } + } spdlog::info("add spatial and temp priori constraint finished"); } diff --git a/src/config/configor.cpp b/src/config/configor.cpp index f213b7c..be30070 100644 --- a/src/config/configor.cpp +++ b/src/config/configor.cpp @@ -89,6 +89,7 @@ const std::string Configor::DataStream::PkgPath = ros::package::getPath("ikalibr const std::string Configor::DataStream::DebugPath = PkgPath + "/debug/"; std::string Configor::Prior::SpatTempPrioriPath = {}; +std::string Configor::Prior::SplinesPrioriPath = {}; double Configor::Prior::GravityNorm = {}; double Configor::Prior::TimeOffsetPadding = {}; double Configor::Prior::ReadoutTimePadding = {}; @@ -121,6 +122,8 @@ const double Configor::Prior::LossForReprojFactor = 1.0; const double Configor::Prior::LossForOpticalFlowFactor = 30.0; bool Configor::Prior::OptTemporalParams = {}; +bool Configor::Prior::OptSplines = true; +bool Configor::Prior::OptImuNonlinearity = false; bool Configor::Preference::UseCudaInSolving = {}; OutputOption Configor::Preference::Outputs = OutputOption::NONE; diff --git a/src/sensor/lidar_data_loader.cpp b/src/sensor/lidar_data_loader.cpp index d38bae1..625b9af 100644 --- a/src/sensor/lidar_data_loader.cpp +++ b/src/sensor/lidar_data_loader.cpp @@ -79,6 +79,9 @@ LiDARDataLoader::Ptr LiDARDataLoader::GetLoader(const std::string &lidarModelStr case LidarModelType::RSLIDAR_POINTS: dataLoader = RSLIDAR_POINTS::Create(lidarModel); break; + case LidarModelType::OUSTER_POINTS_RING_16: + dataLoader = OusterRing16LiDAR::Create(lidarModel); + break; default: throw Status(Status::WARNING, LidarModel::UnsupportedLiDARModelMsg(lidarModelStr)); } @@ -394,6 +397,60 @@ LiDARFrame::Ptr OusterLiDAR::UnpackScan(const rosbag::MessageInstance &msgInstan return LiDARFrame::Create(timebase, cloud); } +// ------------------------------ +// OusterLiDAR with uint16_t ring +// ------------------------------ +OusterRing16LiDAR::OusterRing16LiDAR(LidarModelType lidarModel) + : LiDARDataLoader(lidarModel) {} + +OusterRing16LiDAR::Ptr OusterRing16LiDAR::Create(LidarModelType lidarModel) { + return std::make_shared(lidarModel); +} + +LiDARFrame::Ptr OusterRing16LiDAR::UnpackScan(const rosbag::MessageInstance &msgInstance) { + sensor_msgs::PointCloud2::ConstPtr lidarMsg = + msgInstance.instantiate(); + + CheckMessage(lidarMsg); + + OusterRing16PointCloud pcIn; + pcl::fromROSMsg(*lidarMsg, pcIn); + + if (lidarMsg->header.stamp.isZero()) { + Status(Status::WARNING, "lidar scan with zero timestamp exists!!!"); + } + double timebase = lidarMsg->header.stamp.toSec(); + + IKalibrPointCloud::Ptr cloud(new IKalibrPointCloud()); + cloud->is_dense = false; + cloud->resize(pcIn.size()); + + std::size_t j = 0; + for (const auto &src : pcIn) { + if (IS_POS_NAN(src)) { + continue; + } + + double depth = sqrt(src.x * src.x + src.y * src.y + src.z * src.z); + + if (depth > 60 || depth < 1) { + continue; + } else { + IKalibrPoint dstPoint; + dstPoint.x = src.x; + dstPoint.y = src.y; + dstPoint.z = src.z; + dstPoint.timestamp = timebase + static_cast(src.t) * 1E-9; + // attention: use 'PointXYZT' as 'IKalibrPoint' rather than 'PointXYZIT' here + // dstPoint.intensity = src.intensity; + cloud->at(j++) = dstPoint; + } + } + cloud->resize(j); + + return LiDARFrame::Create(timebase, cloud); +} + // ------------- // PandarXTLiDAR // ------------- diff --git a/src/sensor/sensor_model.cpp b/src/sensor/sensor_model.cpp index 807d871..69a071f 100644 --- a/src/sensor/sensor_model.cpp +++ b/src/sensor/sensor_model.cpp @@ -113,8 +113,11 @@ std::string LidarModel::UnsupportedLiDARModelMsg(const std::string &modelStr) { "3. Hesai Pandar XT LiDARs: PANDAR_XT_POINTS\n" "4. Livox LiDARs: LIVOX_CUSTOM (the official 'xfer_format'=1, " "mid-360 and avia is recommend)\n" + "5. Robosense LiDARs: RSLIDAR_POINTS (tested on RS-Helios-16P)\n" + "6. Ouster LiDARs: OUSTER_POINTS_RING_16 (Ouster pointcloud with " + "uint16_t ring field)" "...\n" - "If you need to use other IMU types, " + "If you need to use other LiDAR types, " "please 'Issues' us on the profile of the github repository.", modelStr); } diff --git a/src/solver/calib_solver_common_impl.cpp b/src/solver/calib_solver_common_impl.cpp index a1b31dd..416b1b4 100644 --- a/src/solver/calib_solver_common_impl.cpp +++ b/src/solver/calib_solver_common_impl.cpp @@ -110,6 +110,10 @@ CalibSolver::CalibSolver(CalibDataManager::Ptr calibDataManager, _splines = CreateSplineBundle( _dataMagr->GetCalibStartTimestamp(), _dataMagr->GetCalibEndTimestamp(), Configor::Prior::KnotTimeDist::SO3Spline, Configor::Prior::KnotTimeDist::ScaleSpline); + _parMagr->splinesPrioriKnots[Configor::Preference::SO3_SPLINE].resize( + _splines->GetSo3Spline(Configor::Preference::SO3_SPLINE).GetKnots().size(), false); + _parMagr->splinesPrioriKnots[Configor::Preference::SCALE_SPLINE].resize( + _splines->GetRdSpline(Configor::Preference::SCALE_SPLINE).GetKnots().size(), false); // create viewer _viewer = Viewer::Create(_parMagr, _splines); @@ -133,6 +137,12 @@ CalibSolver::CalibSolver(CalibDataManager::Ptr calibDataManager, spdlog::info("priori about spatial and temporal parameters are given: '{}'", Configor::Prior::SpatTempPrioriPath); } + + // spline bundle priori + if (std::filesystem::exists(Configor::Prior::SplinesPrioriPath)) { + LoadSplineBundlePriori(Configor::Prior::SplinesPrioriPath); + spdlog::info("priori about splines are given: '{}'", Configor::Prior::SplinesPrioriPath); + } } CalibSolver::Ptr CalibSolver::Create(const CalibDataManager::Ptr &calibDataManager, @@ -151,6 +161,10 @@ CalibSolver::~CalibSolver() { } } +double CalibSolver::GetRawStartTimestamp() const { + return _dataMagr->GetRawStartTimestamp(); +} + std::optional CalibSolver::CurBrToW(double timeByBr) const { if (GetScaleType() != TimeDeriv::LIN_POS_SPLINE) { throw Status(Status::CRITICAL, @@ -277,6 +291,57 @@ CalibSolver::SplineBundleType::Ptr CalibSolver::CreateSplineBundle(double st, return SplineBundleType::Create({so3SplineInfo, scaleSplineInfo}); } +void CalibSolver::LoadSplineBundlePriori(const std::string &splinesPrioriPath) { + // Load the format saved by CalibSolverIO::SaveBSplines() + auto prioriBundle = SplineBundleType::Create({}); + std::ifstream file(splinesPrioriPath); + auto ar = GetInputArchiveVariant(file, Configor::Preference::OutputDataFormat); + double rawStartTime; + SerializeByInputArchiveVariant(ar, Configor::Preference::OutputDataFormat, + cereal::make_nvp("splines", *prioriBundle), + cereal::make_nvp("start_time", rawStartTime)); + + try { + auto& spline = _splines->GetSo3Spline(Configor::Preference::SO3_SPLINE); + const auto& prioriSpline = prioriBundle->GetSo3Spline(Configor::Preference::SO3_SPLINE); + auto& prioriKnots = _parMagr->splinesPrioriKnots.at(Configor::Preference::SO3_SPLINE); + size_t numPrioriKnots {0u}; + for (size_t i = 0; i < spline.GetKnots().size(); ++i) { + const double t = spline.MinTime() + static_cast(i) * spline.GetTimeInterval(); + const double prioriT = t + _dataMagr->GetRawStartTimestamp() - rawStartTime; + if (prioriSpline.TimeStampInRange(prioriT)) { + const auto [tmp, prioriI] = prioriSpline.ComputeTIndex(prioriT); + spline.GetKnot(i) = prioriSpline.GetKnot(prioriI); + prioriKnots[i] = true; + numPrioriKnots++; + } + } + spdlog::info("priori about SO3 spline are given: '{}' ({} knots)", + splinesPrioriPath, numPrioriKnots); + } catch (const std::out_of_range&) { + } + + try { + auto& spline = _splines->GetRdSpline(Configor::Preference::SCALE_SPLINE); + const auto& prioriSpline = prioriBundle->GetRdSpline(Configor::Preference::SCALE_SPLINE); + auto& prioriKnots = _parMagr->splinesPrioriKnots.at(Configor::Preference::SCALE_SPLINE); + size_t numPrioriKnots {0u}; + for (size_t i = 0; i < spline.GetKnots().size(); ++i) { + const double t = spline.MinTime() + static_cast(i) * spline.GetTimeInterval(); + const double prioriT = t + _dataMagr->GetRawStartTimestamp() - rawStartTime; + if (prioriSpline.TimeStampInRange(prioriT)) { + const auto [tmp, prioriI] = prioriSpline.ComputeTIndex(prioriT); + spline.GetKnot(i) = prioriSpline.GetKnot(prioriI); + prioriKnots[i] = true; + numPrioriKnots++; + } + } + spdlog::info("priori about scale spline are given: '{}' ({} knots)", + splinesPrioriPath, numPrioriKnots); + } catch (const std::out_of_range&) { + } +} + void CalibSolver::AlignStatesToGravity() const { auto &so3Spline = _splines->GetSo3Spline(Configor::Preference::SO3_SPLINE); auto &scaleSpline = _splines->GetRdSpline(Configor::Preference::SCALE_SPLINE); @@ -503,6 +568,10 @@ ns_veta::Veta::Ptr CalibSolver::TryLoadSfMData(const std::string &topic, const auto &nameToOurIdx = info.GetImagesNameToIdx(); for (const auto &[IdFromColmap, image] : images) { + if (nameToOurIdx.find(image.name_) == nameToOurIdx.end()) { + spdlog::warn("Image {} from images.txt not found in info.yaml.", image.name_); + continue; + } const auto &viewId = nameToOurIdx.at(image.name_); const auto &poseId = viewId; diff --git a/src/solver/calib_solver_init_prep_ii_align_impl.cpp b/src/solver/calib_solver_init_prep_ii_align_impl.cpp index 4ee2b54..4e7ee5b 100644 --- a/src/solver/calib_solver_init_prep_ii_align_impl.cpp +++ b/src/solver/calib_solver_init_prep_ii_align_impl.cpp @@ -32,6 +32,8 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include "calib/calib_param_manager.h" +#include "calib/spat_temp_priori.h" #include "solver/calib_solver.h" namespace { @@ -41,7 +43,19 @@ bool IKALIBR_UNIQUE_NAME(_2_) = ns_ikalibr::_1_(__FILE__); namespace ns_ikalibr { void CalibSolver::InitPrepInertialInertialAlign() { - // There is no need to prepare for inertial-inertial alignment + for (const auto& [topic, _] : Configor::DataStream::IMUTopics) { + if (const auto so3 = _priori->GetSO3ToBr(topic)) { + _parMagr->EXTRI.SO3_BiToBr.at(topic) = *so3; + spdlog::info("extrinsic rotation read from priori information for IMU '{}'", topic); + } + } + + for (const auto& [topic, _] : Configor::DataStream::IMUTopics) { + if (const auto offset = _priori->GetTOToBr(topic)) { + _parMagr->TEMPORAL.TO_BiToBr.at(topic) = *offset; + spdlog::info("time offset read from priori information for IMU '{}'", topic); + } + } } } // namespace ns_ikalibr \ No newline at end of file diff --git a/src/solver/calib_solver_init_prep_li_align_impl.cpp b/src/solver/calib_solver_init_prep_li_align_impl.cpp index f06b32b..baba6d1 100644 --- a/src/solver/calib_solver_init_prep_li_align_impl.cpp +++ b/src/solver/calib_solver_init_prep_li_align_impl.cpp @@ -35,6 +35,7 @@ #include "calib/calib_data_manager.h" #include "calib/calib_param_manager.h" #include "calib/estimator.h" +#include "calib/spat_temp_priori.h" #include "core/lidar_odometer.h" #include "core/rotation_estimator.h" #include "core/scan_undistortion.h" @@ -55,6 +56,21 @@ void CalibSolver::InitPrepLiDARInertialAlign() const { } const auto &so3Spline = _splines->GetSo3Spline(Configor::Preference::SO3_SPLINE); const auto &scaleSpline = _splines->GetRdSpline(Configor::Preference::SCALE_SPLINE); + + for (const auto& [topic, _] : Configor::DataStream::LiDARTopics) { + if (const auto so3 = _priori->GetSO3ToBr(topic)) { + _parMagr->EXTRI.SO3_LkToBr.at(topic) = *so3; + spdlog::info("extrinsic rotation read from priori information for lidar '{}'", topic); + } + } + + for (const auto& [topic, _] : Configor::DataStream::LiDARTopics) { + if (const auto offset = _priori->GetTOToBr(topic)) { + _parMagr->TEMPORAL.TO_LkToBr.at(topic) = *offset; + spdlog::info("time offset read from priori information for lidar '{}'", topic); + } + } + /** * we throw the head and tail data as the rotations from the fitted SO3 Spline in that range are * poor @@ -70,6 +86,11 @@ void CalibSolver::InitPrepLiDARInertialAlign() const { */ spdlog::info("LiDARs are integrated, initializing extrinsic rotations of LiDARs..."); for (const auto &[topic, data] : _dataMagr->GetLiDARMeasurements()) { + const auto hasSO3 = _priori->HasSO3ToBr(topic); + const auto hasTO = _priori->HasTOToBr(topic); + if (hasSO3 && (!Configor::Prior::OptTemporalParams || hasTO)) + continue; + spdlog::info("performing ndt odometer for '{}' for extrinsic rotation initialization...", topic); diff --git a/src/solver/calib_solver_init_prep_pci_align_impl.cpp b/src/solver/calib_solver_init_prep_pci_align_impl.cpp index 2cb18ea..432c17b 100644 --- a/src/solver/calib_solver_init_prep_pci_align_impl.cpp +++ b/src/solver/calib_solver_init_prep_pci_align_impl.cpp @@ -35,6 +35,7 @@ #include "calib/calib_data_manager.h" #include "calib/calib_param_manager.h" #include "calib/estimator.h" +#include "calib/spat_temp_priori.h" #include "core/rotation_estimator.h" #include "core/vision_only_sfm.h" #include "opencv2/highgui.hpp" @@ -53,6 +54,21 @@ void CalibSolver::InitPrepPosCameraInertialAlign() const { } const auto& so3Spline = _splines->GetSo3Spline(Configor::Preference::SO3_SPLINE); const auto& scaleSpline = _splines->GetRdSpline(Configor::Preference::SCALE_SPLINE); + + for (const auto& [topic, _] : Configor::DataStream::PosCameraTopics()) { + if (const auto so3 = _priori->GetSO3ToBr(topic)) { + _parMagr->EXTRI.SO3_CmToBr.at(topic) = *so3; + spdlog::info("extrinsic rotation read from priori information for camera '{}'", topic); + } + } + + for (const auto& [topic, _] : Configor::DataStream::PosCameraTopics()) { + if (const auto offset = _priori->GetTOToBr(topic)) { + _parMagr->TEMPORAL.TO_CmToBr.at(topic) = *offset; + spdlog::info("time offset read from priori information for camera '{}'", topic); + } + } + /** * we throw the head and tail data as the rotations from the fitted SO3 Spline in that range are * poor @@ -74,6 +90,11 @@ void CalibSolver::InitPrepPosCameraInertialAlign() const { // the min distance between two features (to ensure features are distributed uniformly) constexpr int minDist = 25; for (const auto& [topic, _] : Configor::DataStream::PosCameraTopics()) { + const auto hasSO3 = _priori->HasSO3ToBr(topic); + const auto hasTO = _priori->HasTOToBr(topic); + if (hasSO3 && (!Configor::Prior::OptTemporalParams || hasTO)) + continue; + const auto& frameVec = _dataMagr->GetCameraMeasurements(topic); spdlog::info( "perform rotation-only visual odometer to recover extrinsic rotations for '{}'...", @@ -96,6 +117,8 @@ void CalibSolver::InitPrepPosCameraInertialAlign() const { spdlog::warn( "tracking failed when grab the '{}' image frame!!! try to reinitialize", i); } + if (hasSO3) + continue; // we do not want to try to recover the extrinsic rotation too frequent if ((odometer->GetRotations().size() < 50) || @@ -115,13 +138,15 @@ void CalibSolver::InitPrepPosCameraInertialAlign() const { break; } } - if (!rotEstimator->SolveStatus()) { - throw Status(Status::ERROR, - "initialize rotation 'SO3_CmToBr' failed, this may be related to " - "insufficiently excited motion or bad images."); - } else { - spdlog::info("extrinsic rotation of '{}' is recovered using '{:06}' frames", topic, - odometer->GetRotations().size()); + if (!hasSO3) { + if (!rotEstimator->SolveStatus()) { + throw Status(Status::ERROR, + "initialize rotation 'SO3_CmToBr' failed, this may be related to " + "insufficiently excited motion or bad images."); + } else { + spdlog::info("extrinsic rotation of '{}' is recovered using '{:06}' frames", topic, + odometer->GetRotations().size()); + } } _viewer->UpdateSensorViewer(); @@ -141,6 +166,9 @@ void CalibSolver::InitPrepPosCameraInertialAlign() const { // perform time offset estimation and extrinsic rotation refinement for (const auto& [topic, _] : Configor::DataStream::PosCameraTopics()) { + if (_priori->HasTOToBr(topic)) + continue; + const auto& rotations = rotOnlyOdom.at(topic)->GetRotations(); // this field should be zero here double TO_CmToBr = _parMagr->TEMPORAL.TO_CmToBr.at(topic); diff --git a/src/solver/calib_solver_init_sen_inertial_align_impl.cpp b/src/solver/calib_solver_init_sen_inertial_align_impl.cpp index 6b37aa2..b47f6cf 100644 --- a/src/solver/calib_solver_init_sen_inertial_align_impl.cpp +++ b/src/solver/calib_solver_init_sen_inertial_align_impl.cpp @@ -35,6 +35,7 @@ #include "calib/calib_data_manager.h" #include "calib/calib_param_manager.h" #include "calib/estimator.h" +#include "calib/spat_temp_priori.h" #include "core/lidar_odometer.h" #include "solver/calib_solver.h" #include "util/utils_tpl.hpp" @@ -102,6 +103,14 @@ void CalibSolver::InitSensorInertialAlign() const { const auto &poseSeq = odometer->GetOdomPoseVec(); double TO_LkToBr = _parMagr->TEMPORAL.TO_LkToBr.at(lidarTopic); + auto lidarOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(lidarTopic)) { + _parMagr->EXTRI.POS_LkInBr.at(lidarTopic) = *pos; + spdlog::info("extrinsic translation read from priori information for lidar '{}'", + lidarTopic); + lidarOptOption &= ~OptOption::OPT_POS_LkInBr; + } + // create linear velocity sequence linVelSeqLk[lidarTopic] = std::vector(poseSeq.size(), Eigen::Vector3d::Zero()); @@ -114,7 +123,7 @@ void CalibSolver::InitSensorInertialAlign() const { std::max(1, int(DESIRED_TIME_INTERVAL * _dataMagr->GetLiDARAvgFrequency(lidarTopic))); spdlog::info("add lidar-inertial alignment factors for '{}' and '{}', align step: {}", - lidarTopic, Configor::DataStream::ReferIMU); + lidarTopic, Configor::DataStream::ReferIMU, ALIGN_STEP); for (int i = 0; i < static_cast(poseSeq.size()) - ALIGN_STEP; ++i) { const auto &sPose = poseSeq.at(i), ePose = poseSeq.at(i + ALIGN_STEP); @@ -136,7 +145,7 @@ void CalibSolver::InitSensorInertialAlign() const { odometer->GetMapTime(), // the map time &curLidarLinVelSeq.at(i), // the start velocity (to be estimated) &curLidarLinVelSeq.at(i + ALIGN_STEP), // the end velocity (to be estimated) - optOption, // the optimize option + lidarOptOption, // the optimize option weight); // the weigh } } @@ -185,6 +194,14 @@ void CalibSolver::InitSensorInertialAlign() const { frame->GetTimestamp()); } + auto camOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(camTopic)) { + _parMagr->EXTRI.POS_CmInBr.at(camTopic) = *pos; + spdlog::info("extrinsic translation read from priori information for camera '{}'", + camTopic); + camOptOption &= ~OptOption::OPT_POS_CmInBr; + } + // create linear velocity sequence linVelSeqCm[camTopic] = std::vector(constructedFrames.size(), Eigen::Vector3d::Zero()); @@ -202,6 +219,11 @@ void CalibSolver::InitSensorInertialAlign() const { spdlog::info("add visual-inertial alignment factors for '{}' and '{}', align step: {}", camTopic, Configor::DataStream::ReferIMU, ALIGN_STEP); + std::optional minScale; + if (_priori->GetMinVisualScale().count(camTopic) > 0) { + minScale = _priori->GetMinVisualScale().at(camTopic); + } + for (int i = 0; i < static_cast(constructedFrames.size()) - ALIGN_STEP; ++i) { const auto &sPose = constructedFrames.at(i); const auto &ePose = constructedFrames.at(i + ALIGN_STEP); @@ -226,8 +248,12 @@ void CalibSolver::InitSensorInertialAlign() const { &curCamLinVelSeq.at(i), // the start velocity (to be estimated) &curCamLinVelSeq.at(i + ALIGN_STEP), // the end velocity (to be estimated) &scale, // the visual scale (to be estimated) - optOption, // the optimize option + camOptOption, // the optimize option weight); // the weigh + + if (minScale) { + estimator->SetParameterLowerBound(&scale, 0, *minScale); + } } } @@ -236,6 +262,15 @@ void CalibSolver::InitSensorInertialAlign() const { std::vector linVelSeqBr(std::floor((et - st) / dt), Eigen::Vector3d::Zero()); if (Configor::DataStream::IMUTopics.size() >= 2) { for (const auto &[topic, frames] : _dataMagr->GetIMUMeasurements()) { + auto imuOptOption = optOption; + const auto pos = _priori->GetPosInBr(topic); + if (topic != Configor::DataStream::ReferIMU && pos) { + _parMagr->EXTRI.POS_BiInBr.at(topic) = *pos; + spdlog::info("extrinsic translation read from priori information for IMU '{}'", + topic); + imuOptOption &= ~OptOption::OPT_POS_BiInBr; + } + spdlog::info("add inertial alignment factors for '{}'...", topic); int count = 0; for (int i = 0; i < static_cast(linVelSeqBr.size()) - 1; ++i) { @@ -244,17 +279,17 @@ void CalibSolver::InitSensorInertialAlign() const { Eigen::Vector3d *sVel = &linVelSeqBr.at(sIdx), *eVel = &linVelSeqBr.at(eIdx); estimator->AddInertialAlignment( - frames, // imu frames - topic, // the ros topic of this imu - sTimeByBr, // the start time stamped by the reference imu - eTimeByBr, // the end time stamped by the reference imu - sVel, // the start velocity (to be estimated) - eVel, // the end velocity (to be estimated) - optOption, // the optimize option + frames, // imu frames + topic, // the ros topic of this imu + sTimeByBr, // the start time stamped by the reference imu + eTimeByBr, // the end time stamped by the reference imu + sVel, // the start velocity (to be estimated) + eVel, // the end velocity (to be estimated) + imuOptOption, // the optimize option Configor::DataStream::IMUTopics.at(topic).AcceWeight); ++count; } - spdlog::info("constraint count of inertial alignment for '{}': {}", topic, + spdlog::info("constraint count of inertial alignment for '{}'-'{}': {}", topic, Configor::DataStream::ReferIMU, count); } } @@ -266,6 +301,14 @@ void CalibSolver::InitSensorInertialAlign() const { const auto &refIMUFrames = _dataMagr->GetIMUMeasurements(Configor::DataStream::ReferIMU); + auto radarOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(radarTopic)) { + _parMagr->EXTRI.POS_RjInBr.at(radarTopic) = *pos; + spdlog::info("extrinsic translation read from priori information for radar '{}'", + radarTopic); + radarOptOption &= ~OptOption::OPT_POS_RjInBr; + } + const int ALIGN_STEP = std::max(1, int(DESIRED_TIME_INTERVAL * _dataMagr->GetRadarAvgFrequency(radarTopic))); @@ -308,7 +351,7 @@ void CalibSolver::InitSensorInertialAlign() const { radarTopic, // the ros topic of this radar sArray, // the start target array eArray, // the end target array - optOption, // the optimization option + radarOptOption, // the optimization option weight); // the weight ++count; } @@ -323,6 +366,14 @@ void CalibSolver::InitSensorInertialAlign() const { const auto &frames = _dataMagr->GetIMUMeasurements(Configor::DataStream::ReferIMU); + auto rgbdOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(rgbdTopic)) { + _parMagr->EXTRI.POS_DnInBr.at(rgbdTopic) = *pos; + spdlog::info("extrinsic translation read from priori information for rgbd camera '{}'", + rgbdTopic); + rgbdOptOption &= ~OptOption::OPT_POS_DnInBr; + } + const int ALIGN_STEP = std::max(1, int(DESIRED_TIME_INTERVAL * _dataMagr->GetRGBDAvgFrequency(rgbdTopic))); @@ -351,7 +402,7 @@ void CalibSolver::InitSensorInertialAlign() const { rgbdTopic, // the ros topic of this rgbd camera bodyFrameVels.at(i), // the start velocity bodyFrameVels.at(i + ALIGN_STEP), // the end velocity - optOption, // the optimization option + rgbdOptOption, // the optimization option weight); // the weight ++count; } @@ -366,6 +417,14 @@ void CalibSolver::InitSensorInertialAlign() const { double TO_CmToBr = _parMagr->TEMPORAL.TO_CmToBr.at(topic); const auto &frames = _dataMagr->GetIMUMeasurements(Configor::DataStream::ReferIMU); + auto camOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(topic)) { + _parMagr->EXTRI.POS_CmInBr.at(topic) = *pos; + spdlog::info("extrinsic translation read from priori information for camera '{}'", + topic); + camOptOption &= ~OptOption::OPT_POS_CmInBr; + } + const int ALIGN_STEP = std::max(1, int(DESIRED_TIME_INTERVAL * _dataMagr->GetCameraAvgFrequency(topic))); @@ -401,7 +460,7 @@ void CalibSolver::InitSensorInertialAlign() const { &curVelScales.at(i), // the scale of start velocity to be estimated velDirs.at(i + ALIGN_STEP), // the direction of the end velocity &curVelScales.at(i + ALIGN_STEP), // the scale of end velocity to be estimated - optOption, // the optimization option + camOptOption, // the optimization option weight); // the weight ++count; } @@ -416,6 +475,14 @@ void CalibSolver::InitSensorInertialAlign() const { double TO_EsToBr = _parMagr->TEMPORAL.TO_EsToBr.at(topic); const auto &frames = _dataMagr->GetIMUMeasurements(Configor::DataStream::ReferIMU); + auto eventOptOption = optOption; + if (const auto pos = _priori->GetPosInBr(topic)) { + _parMagr->EXTRI.POS_EsInBr.at(topic) = *pos; + spdlog::info("extrinsic translation read from priori information for event camera '{}'", + topic); + eventOptOption &= ~OptOption::OPT_POS_EsInBr; + } + double freq = static_cast(velDirs.size()) / (velDirs.back().first - velDirs.front().first); const int ALIGN_STEP = std::max(1, int(DESIRED_TIME_INTERVAL * freq)); @@ -450,7 +517,7 @@ void CalibSolver::InitSensorInertialAlign() const { &curVelScales.at(i), // the scale of start velocity to be estimated velDirs.at(i + ALIGN_STEP), // the direction of the end velocity &curVelScales.at(i + ALIGN_STEP), // the scale of end velocity to be estimated - optOption, // the optimization option + eventOptOption, // the optimization option weight); // the weight ++count; } diff --git a/src/solver/calib_solver_io.cpp b/src/solver/calib_solver_io.cpp index bb54b9a..b59ccf3 100644 --- a/src/solver/calib_solver_io.cpp +++ b/src/solver/calib_solver_io.cpp @@ -158,8 +158,10 @@ void CalibSolverIO::SaveBSplines(int hz) const { auto filename = saveDir + "/knots" + Configor::GetFormatExtension(); std::ofstream file(filename); auto ar = GetOutputArchiveVariant(file, Configor::Preference::OutputDataFormat); + const double st = _solver->GetRawStartTimestamp(); SerializeByOutputArchiveVariant(ar, Configor::Preference::OutputDataFormat, - cereal::make_nvp("splines", *_solver->_splines)); + cereal::make_nvp("splines", *_solver->_splines), + cereal::make_nvp("start_time", st)); } spdlog::info("saving splines finished!"); }