I have been trying to setup rovio with the oCamS-1MGN-U. After some issues, like the camera being read as a color camera. I got it working with the default configs, although it was very slow. After reading some issues I calibrated the camera with kalibr. Resulting in the configs below.
Image and tracking is working but the position is trailing off-screen soon after launching rovio.
; You can partially override parameter set in this file by creating your own subset of parameter in a separate info-file and include it with:
; #include "/home/user/workspace/rovio/cfg/rovio_custom.info"
Common
{
doVECalibration true; Should the camera-IMU extrinsics be calibrated online
depthType 1; Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic)
verbose false; Is the verbose active
}
Camera0
{
CalibrationFile ; Camera-Calibration file for intrinsics
qCM_x 0.499417744996; X-entry of IMU to Camera quaterion (Hamilton)
qCM_y -0.498887155474; Y-entry of IMU to Camera quaterion (Hamilton)
qCM_z 0.504819895064; Z-entry of IMU to Camera quaterion (Hamilton)
qCM_w 0.496840412642; W-entry of IMU to Camera quaterion (Hamilton)
MrMC_x 0.00125665163835; X-entry of IMU to Camera vector (expressed in IMU CF) [m]
MrMC_y 0.0335818853603; Y-entry of IMU to Camera vector (expressed in IMU CF) [m]
MrMC_z 0.0224551369658; Z-entry of IMU to Camera vector (expressed in IMU CF) [m]
}
Camera1
{
CalibrationFile ; Camera-Calibration file for intrinsics
qCM_x 0.501268163392; X-entry of IMU to Camera quaterion (Hamilton)
qCM_y -0.498030306174; Y-entry of IMU to Camera quaterion (Hamilton)
qCM_z 0.501266422098; Z-entry of IMU to Camera quaterion (Hamilton)
qCM_w 0.499427689039; W-entry of IMU to Camera quaterion (Hamilton)
MrMC_x 0.000924287998468; X-entry of IMU to Camera vector (expressed in IMU CF) [m]
MrMC_y -0.0865171062762; Y-entry of IMU to Camera vector (expressed in IMU CF) [m]
MrMC_z 0.0232072458454; Z-entry of IMU to Camera vector (expressed in IMU CF) [m]
}
Init
{
State
{
pos_0 0; X-entry of initial position (world to IMU in world) [m]
pos_1 0; Y-entry of initial position (world to IMU in world) [m]
pos_2 0; Z-entry of initial position (world to IMU in world) [m]
vel_0 0; X-entry of initial velocity (robocentric, IMU) [m/s]
vel_1 0; Y-entry of initial velocity (robocentric, IMU) [m/s]
vel_2 0; Z-entry of initial velocity (robocentric, IMU) [m/s]
acb_0 0; X-entry of accelerometer bias [m/s^2]
acb_1 0; Y-entry of accelerometer bias [m/s^2]
acb_2 0; Z-entry of accelerometer bias [m/s^2]
gyb_0 0; X-entry of gyroscope bias [rad/s]
gyb_1 0; Y-entry of gyroscope bias [rad/s]
gyb_2 0; Z-entry of gyroscope bias [rad/s]
att_x 0; X-entry of initial attitude (IMU to world, Hamilton)
att_y 0; Y-entry of initial attitude (IMU to world, Hamilton)
att_z 0; Z-entry of initial attitude (IMU to world, Hamilton)
att_w 1; W-entry of initial attitude (IMU to world, Hamilton)
}
Covariance
{
pos_0 0.0001; X-Covariance of initial position [m^2]
pos_1 0.0001; Y-Covariance of initial position [m^2]
pos_2 0.0001; Z-Covariance of initial position [m^2]
vel_0 1.0; X-Covariance of initial velocity [m^2/s^2]
vel_1 1.0; Y-Covariance of initial velocity [m^2/s^2]
vel_2 1.0; Z-Covariance of initial velocity [m^2/s^2]
acb_0 4e-4; X-Covariance of initial accelerometer bias [m^2/s^4]
acb_1 4e-4; Y-Covariance of initial accelerometer bias [m^2/s^4]
acb_2 4e-4; Z-Covariance of initial accelerometer bias [m^2/s^4]
gyb_0 3e-4; X-Covariance of initial gyroscope bias [rad^2/s^2]
gyb_1 3e-4; Y-Covariance of initial gyroscope bias [rad^2/s^2]
gyb_2 3e-4; Z-Covariance of initial gyroscope bias [rad^2/s^2]
vep 0.0001; Covariance of initial linear camera-IMU extrinsics, same for all entries [m^2]
att_0 0.1; X-Covariance of initial attitude [rad^2]
att_1 0.1; Y-Covariance of initial attitude [rad^2]
att_2 0.1; Z-Covariance of initial attitude [rad^2]
vea 0.01; Covariance of initial rotational camera-IMU extrinsics, same for all entries [rad^2]
}
}
ImgUpdate
{
updateVecNormTermination 1e-4;
maxNumIteration 20;
doPatchWarping true; Should the patches be warped
doFrameVisualisation true; Should the frames be visualized
visualizePatches false; Should the patches be visualized
useDirectMethod true; Should the EKF-innovation be based on direct intensity error (o.w. reprojection error)
startLevel 2; Highest patch level which is being employed (must be smaller than the hardcoded template parameter)
endLevel 1; Lowest patch level which is being employed
nDetectionBuckets 100; Number of discretization buckets used during the candidates selection
MahalanobisTh 9.21; Mahalanobis treshold for the update, 5.8858356
UpdateNoise
{
pix 2; Covariance used for the reprojection error, 1/focal_length is roughly 1 pixel std [rad] (~1/f ~ 1/400^2 = 1/160000)
int 400; Covariance used for the photometric error [intensity^2]
}
initCovFeature_0 0.5; Covariance for the initial distance (Relative to initialization depth [m^2/m^2])
initCovFeature_1 1e-5; Covariance for the initial bearing vector, x-component [rad^2]
initCovFeature_2 1e-5; Covariance for the initial bearing vector, y-component [rad^2]
initDepth 0.5; Initial value for the initial distance parameter
startDetectionTh 0.8; Threshold for detecting new features (min: 0, max: 1)
scoreDetectionExponent 0.25; Exponent used for weighting the distance between candidates
penaltyDistance 100; Maximal distance which gets penalized during bucketing [pix]
zeroDistancePenalty 100; Penalty for zero distance (max: nDetectionBuckets)
statLocalQualityRange 10; Number of frames for local quality evaluation
statLocalVisibilityRange 100; Number of frames for local visibility evaluation
statMinGlobalQualityRange 100; Minimum number of frames for obtaining maximal global quality
trackingUpperBound 0.9; Threshold for local quality for min overall global quality
trackingLowerBound 0.8; Threshold for local quality for max overall global quality
minTrackedAndFreeFeatures 0.75; Minimum of amount of feature which are either tracked or free
removalFactor 1.1; Factor for enforcing feature removal if not enough free
minRelativeSTScore 0.75; Minimum relative ST-score for extracting new feature patch
minAbsoluteSTScore 5.0; Minimum absolute ST-score for extracting new feature patch
minTimeBetweenPatchUpdate 1.0; Minimum time between new multilevel patch extrection [s]
fastDetectionThreshold 5; Fast corner detector treshold while adding new feature
alignConvergencePixelRange 10; Assumed convergence range for image alignment (gets scaled with the level) [pixels]
alignCoverageRatio 2; How many sigma of the uncertainty should be covered in the adaptive alignement
alignMaxUniSample 1; Maximal number of alignment seeds on one side -> total number of sample = 2n+1. Carefull can get very costly if diverging!
patchRejectionTh 50.0; If the average itensity error is larger than this the feauture is rejected [intensity], if smaller 0 the no check is performed
alignmentHuberNormThreshold 10; Intensity error threshold for Huber norm (enabled if > 0)
alignmentGaussianWeightingSigma -1; Width of Gaussian which is used for pixel error weighting (enabled if > 0)
alignmentGradientExponent 0.0; Exponent used for gradient based weighting of residuals
useIntensityOffsetForAlignment true; Should an intensity offset between the patches be considered
useIntensitySqewForAlignment true; Should an intensity sqew between the patches be considered
removeNegativeFeatureAfterUpdate true; Should feature with negative distance get removed
maxUncertaintyToDepthRatioForDepthInitialization 0.3; If set to 0.0 the depth is initialized with the standard value provided above, otherwise ROVIO attempts to figure out a median depth in each frame
useCrossCameraMeasurements true; Should cross measurements between frame be used. Might be turned of in calibration phase.
doStereoInitialization true; Should a stereo match be used for feature initialization.
noiseGainForOffCamera 10.0; Factor added on update noise if not main camera
discriminativeSamplingDistance 0.02; Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed).
discriminativeSamplingGain 1.1; Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used).
MotionDetection
{
isEnabled 1; Is the motion detection enabled
rateOfMovingFeaturesTh 0.5; Amount of feature with motion for overall motion detection
pixelCoordinateMotionTh 1.0; Threshold for motion detection for patched [pixels]
minFeatureCountForNoMotionDetection 5; Min feature count in frame for motion detection
}
ZeroVelocityUpdate
{
UpdateNoise
{
vel_0 0.01; X-Covariance of zero velocity update [m^2/s^2]
vel_1 0.01; Y-Covariance of zero velocity update [m^2/s^2]
vel_2 0.01; Z-Covariance of zero velocity update [m^2/s^2]
}
MahalanobisTh0 7.689997599999999; Mahalanobid distance for zero velocity updates
minNoMotionTime 1.0; Min duration of no-motion
isEnabled 1; Should zero velocity update be applied, only works if MotionDetection.isEnabled is true
}
}
Prediction
{
PredictionNoise
{
pos_0 1e-4; X-covariance parameter of position prediction [m^2/s]
pos_1 1e-4; Y-covariance parameter of position prediction [m^2/s]
pos_2 1e-4; Z-covariance parameter of position prediction [m^2/s]
vel_0 4e-5; 4e-6 X-covariance parameter of velocity prediction [m^2/s^3]
vel_1 4e-5; Y-covariance parameter of velocity prediction [m^2/s^3]
vel_2 4e-5; Z-covariance parameter of velocity prediction [m^2/s^3]
acb_0 1e-8; X-covariance parameter of accelerometer bias prediction [m^2/s^5]
acb_1 1e-8; Y-covariance parameter of accelerometer bias prediction [m^2/s^5]
acb_2 1e-8; Z-covariance parameter of accelerometer bias prediction [m^2/s^5]
gyb_0 3.8e-7; X-covariance parameter of gyroscope bias prediction [rad^2/s^3]
gyb_1 3.8e-7; Y-covariance parameter of gyroscope bias prediction [rad^2/s^3]
gyb_2 3.8e-7; Z-covariance parameter of gyroscope bias prediction [rad^2/s^3]
vep 1e-8; Covariance parameter of linear extrinsics prediction [m^2/s]
att_0 7.6e-7; 7.6e-7 X-Covariance parameter of attitude prediction [rad^2/s]
att_1 7.6e-7; Y-Covariance parameter of attitude prediction [rad^2/s]
att_2 7.6e-7; Z-Covariance parameter of attitude prediction [rad^2/s]
vea 1e-8; Covariance parameter of rotational extrinsics prediction [rad^2/s]
dep 0.0001; Covariance parameter of distance prediction [m^2/s]
nor 0.00001; Covariance parameter of bearing vector prediction [rad^2/s]
}
MotionDetection
{
inertialMotionRorTh 0.1; Treshold on rotational rate for motion detection [rad/s]
inertialMotionAccTh 0.5; Treshold on acceleration for motion detection [m/s^2]
}
}
PoseUpdate
{
UpdateNoise
{
pos_0 0.01; X-Covariance of linear pose measurement update [m^2]
pos_1 0.01; Y-Covariance of linear pose measurement update [m^2]
pos_2 0.01; Z-Covariance of linear pose measurement update [m^2]
att_0 0.01; X-Covariance of rotational pose measurement update [rad^2]
att_1 0.01; Y-Covariance of rotational pose measurement update [rad^2]
att_2 0.01; Z-Covariance of rotational pose measurement update [rad^2]
}
init_cov_IrIW 1; Covariance of initial pose between inertial frames, linear part [m^2]
init_cov_qWI 1; Covariance of initial pose between inertial frames, rotational part [rad^2]
init_cov_MrMV 1; Covariance of initial pose between inertial frames, linear part [m^2]
init_cov_qVM 1; Covariance of initial pose between inertial frames, rotational part [rad^2]
pre_cov_IrIW 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s]
pre_cov_qWI 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s]
pre_cov_MrMV 1e-4; Covariance parameter of pose between inertial frames, linear part [m^2/s]
pre_cov_qVM 1e-4; Covariance parameter of pose between inertial frames, rotational part [rad^2/s]
MahalanobisTh0 12.6511204; Mahalonobis distance treshold of pose measurement
doVisualization false; Should the measured poses be vizualized
enablePosition true; Should the linear part be used during the update
enableAttitude true; Should the rotation part be used during the update (e.g. set to fals eif feeding GPS-measurement)
noFeedbackToRovio true; By default the pose update is only used for aligning coordinate frame. Set to false if ROVIO should benefit frome the posed measurements.
doInertialAlignmentAtStart true; Should the transformation between I and W be explicitly computed and set with the first pose measurement.
timeOffset 0.0; Time offset added to the pose measurement timestamps
useOdometryCov false; Should the UpdateNoise position covariance be scaled using the covariance in the Odometry message
qVM_x 0; X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
qVM_y 0; Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
qVM_z 0; Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
qVM_w -1; W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (Hamilton)
MrMV_x 0; X-entry of vector representing IMU to reference body coordinate frame of pose measurement
MrMV_y 0; Y-entry of vector representing IMU to reference body coordinate frame of pose measurement
MrMV_z 0; Z-entry of vector representing IMU to reference body coordinate frame of pose measurement
qWI_x 0; X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
qWI_y 0; Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
qWI_z 0; Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
qWI_w -1; W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (Hamilton)
IrIW_x 0; X-entry of vector representing World to reference inertial coordinate frame of pose measurement
IrIW_y 0; Y-entry of vector representing World to reference inertial coordinate frame of pose measurement
IrIW_z 0; Z-entry of vector representing World to reference inertial coordinate frame of pose measurement
}
As you can see there's quite a long shift between the IMU data. I've read in another issue that this could be the problem. You can find the ROS sourcecode here: https://github.com/withrobot/oCamS I needed to change the topic names and camera color to mono to get it to work.
From source it seems that both the image and sensor data use a timestamp from the device for the publish, so not on arrival.
I had some difficulty calibrating the IMU using kalibr_allan. The results from the accelerometer were as expected but the variance of the gyroscoop was showing divergent behavior on larger taus resulting in NaN for the gyro random walk.
I eventually used the z-axes to fit the line for random walk:
Resulting in these values for the IMU calibration:
So my thoughts are now that maybe this camera setup is not suited for rovio due to the timestamping issue or the IMU sensor quality is sub-par (BOSCH BNO055) or faulty.
I have been trying to setup rovio with the oCamS-1MGN-U. After some issues, like the camera being read as a color camera. I got it working with the default configs, although it was very slow. After reading some issues I calibrated the camera with kalibr. Resulting in the configs below.
Image and tracking is working but the position is trailing off-screen soon after launching rovio.
Kalibr output: camchain-imucam-kalibr_20hz_april_v2.yaml
Rovio configs: rovio_test.info
rovio_cam0.yaml
rovio_cam1.yaml
As you can see there's quite a long shift between the IMU data. I've read in another issue that this could be the problem. You can find the ROS sourcecode here: https://github.com/withrobot/oCamS I needed to change the topic names and camera color to mono to get it to work.
From source it seems that both the image and sensor data use a timestamp from the device for the publish, so not on arrival.
I had some difficulty calibrating the IMU using kalibr_allan. The results from the accelerometer were as expected but the variance of the gyroscoop was showing divergent behavior on larger taus resulting in NaN for the gyro random walk.
I eventually used the z-axes to fit the line for random walk:
Resulting in these values for the IMU calibration:
So my thoughts are now that maybe this camera setup is not suited for rovio due to the timestamping issue or the IMU sensor quality is sub-par (BOSCH BNO055) or faulty.