{"action":"created","issue":{"url":"https://api.github.com/repos/ethz-asl/rovio/issues/40","repository_url":"https://api.github.com/repos/ethz-asl/rovio","labels_url":"https://api.github.com/repos/ethz-asl/rovio/issues/40/labels{/name}","comments_url":"https://api.github.com/repos/ethz-asl/rovio/issues/40/comments","events_url":"https://api.github.com/repos/ethz-asl/rovio/issues/40/events","html_url":"https://github.com/ethz-asl/rovio/issues/40","id":136805123,"number":40,"title":"medianDepth for new features initialization","user":{"login":"devbharat","id":3105122,"avatar_url":"https://avatars.githubusercontent.com/u/3105122?v=3","gravatar_id":"","url":"https://api.github.com/users/devbharat","html_url":"https://github.com/devbharat","followers_url":"https://api.github.com/users/devbharat/followers","following_url":"https://api.github.com/users/devbharat/following{/other_user}","gists_url":"https://api.github.com/users/devbharat/gists{/gist_id}","starred_url":"https://api.github.com/users/devbharat/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/devbharat/subscriptions","organizations_url":"https://api.github.com/users/devbharat/orgs","repos_url":"https://api.github.com/users/devbharat/repos","events_url":"https://api.github.com/users/devbharat/events{/privacy}","received_events_url":"https://api.github.com/users/devbharat/received_events","type":"User","site_admin":false},"labels":[],"state":"open","locked":false,"assignee":null,"milestone":null,"comments":14,"created_at":"2016-02-26T20:23:51Z","updated_at":"2016-03-08T15:42:45Z","closed_at":null,"body":"In image update\r\n\r\n    // Get new features\r\n    if(filterState.fsm_.getValidCount() < startDetectionTh_*mtState::nMax_){\r\n      // Compute the median depth parameters for each camera, using the state features.\r\n      std::array<double, mtState::nCam_> medianDepthParameters;\r\n      if(maxUncertaintyToDepthRatioForDepthInitialization_>0){\r\n        filterState.getMedianDepthParameters(initDepth_, &medianDepthParameters,maxUncertaintyToDepthRatioForDepthInitialization_);\r\n      } else {\r\n        medianDepthParameters.fill(initDepth_);\r\n      }\r\n\r\nTwo things to get better initialization estimate when a new frame loses too many features:\r\n  - **Computing medianDepthParameters for every frame** rather just the frames where number of tracked features is low, since frames with many tracked features gives better initialization estimate\r\n  - Assume some smoothness in motion and **use previous frame's median for current frame depth initialization**. \r\n\r\nThis handles the vicious cycle where I suddenly lose most features in the current frame so its mediandepth for initialization is not good enough, which leads to bad depth initialization of all new features which leads to tracking failure for most of them in the subsequent imgUpdate and so on till it diverges. This for example happened when I am about to land close to the ground. I tested this strategy and it works better than the one we currently have. "},"comment":{"url":"https://api.github.com/repos/ethz-asl/rovio/issues/comments/193828749","html_url":"https://github.com/ethz-asl/rovio/issues/40#issuecomment-193828749","issue_url":"https://api.github.com/repos/ethz-asl/rovio/issues/40","id":193828749,"user":{"login":"devbharat","id":3105122,"avatar_url":"https://avatars.githubusercontent.com/u/3105122?v=3","gravatar_id":"","url":"https://api.github.com/users/devbharat","html_url":"https://github.com/devbharat","followers_url":"https://api.github.com/users/devbharat/followers","following_url":"https://api.github.com/users/devbharat/following{/other_user}","gists_url":"https://api.github.com/users/devbharat/gists{/gist_id}","starred_url":"https://api.github.com/users/devbharat/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/devbharat/subscriptions","organizations_url":"https://api.github.com/users/devbharat/orgs","repos_url":"https://api.github.com/users/devbharat/repos","events_url":"https://api.github.com/users/devbharat/events{/privacy}","received_events_url":"https://api.github.com/users/devbharat/received_events","type":"User","site_admin":false},"created_at":"2016-03-08T15:42:45Z","updated_at":"2016-03-08T15:42:45Z","body":"This is the one I am using. Calibration params might be different for you. Most critical values are in the Image update section.\r\n\r\n    ; 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:\r\n    ; #include \"/home/user/workspace/rovio/cfg/rovio_custom.info\"\r\n    Common\r\n    {\r\n        doVECalibration true;       Should the camera-IMU extrinsics be calibrated online\r\n        depthType 1;                Type of depth parametrization (0: normal, 1: inverse depth, 2: log, 3: hyperbolic)\r\n        verbose false;              Is the verbose active\r\n    }\r\n    Camera0\r\n    {\r\n        CalibrationFile /home/bharat/Documents/Master_Thesis/softwares/catkin_workspace/src/rovio/cfg/phycamsv10_newcalib_radtan.yaml\r\n        qCM_x  0.7060\r\n        qCM_y  0.7082\r\n        qCM_z  0.0005\r\n        qCM_w  -0.0009\r\n        MrMC_x -0.00159984\r\n        MrMC_y -0.00058784\r\n        MrMC_z -0.00099081\r\n    }\r\n    Camera1\r\n    {\r\n        CalibrationFile /home/bharat/Documents/Master_Thesis/softwares/catkin_workspace/src/rovio/cfg/phycamsv10.yaml\r\n        qCM_x  0.0\r\n        qCM_y  0.0\r\n        qCM_z  0.7071\r\n        qCM_w  0.7071\r\n        MrMC_x 0\r\n        MrMC_y 0\r\n        MrMC_z 0\r\n    }\r\n    Init\r\n    {\r\n        State\r\n        {\r\n            pos_0 0;            X-entry of initial position (world to IMU in world) [m]\r\n            pos_1 0;            Y-entry of initial position (world to IMU in world) [m]\r\n            pos_2 0;            Z-entry of initial position (world to IMU in world) [m]\r\n            vel_0 0;            X-entry of initial velocity (robocentric, IMU) [m/s]\r\n            vel_1 0;            Y-entry of initial velocity (robocentric, IMU) [m/s]\r\n            vel_2 0;            Z-entry of initial velocity (robocentric, IMU) [m/s]\r\n            acb_0 0; 0.22;          X-entry of accelerometer bias [m/s^2]\r\n            acb_1 0; 0.15;          Y-entry of accelerometer bias [m/s^2]\r\n            acb_2 0; 0.13;          Z-entry of accelerometer bias [m/s^2]\r\n            gyb_0 0; -0.011;            X-entry of gyroscope bias [rad/s]\r\n            gyb_1 0; -0.016;            Y-entry of gyroscope bias [rad/s]\r\n            gyb_2 0;            Z-entry of gyroscope bias [rad/s]\r\n            att_x 0;            X-entry of initial attitude (IMU to world, JPL)\r\n            att_y 0;            Y-entry of initial attitude (IMU to world, JPL)\r\n            att_z 0;            Z-entry of initial attitude (IMU to world, JPL)\r\n            att_w 1;            W-entry of initial attitude (IMU to world, JPL)\r\n        }\r\n        Covariance\r\n        {\r\n            pos_0 0.0001;       X-Covariance of initial position [m^2]\r\n            pos_1 0.0001;       Y-Covariance of initial position [m^2]\r\n            pos_2 0.0001;       Z-Covariance of initial position [m^2]\r\n            vel_0 1.0;          X-Covariance of initial velocity [m^2/s^2]\r\n            vel_1 1.0;          Y-Covariance of initial velocity [m^2/s^2]\r\n            vel_2 1.0;          Z-Covariance of initial velocity [m^2/s^2]\r\n            acb_0 4e-4;         X-Covariance of initial accelerometer bias [m^2/s^4]\r\n            acb_1 4e-4;         Y-Covariance of initial accelerometer bias [m^2/s^4]\r\n            acb_2 4e-4;         Z-Covariance of initial accelerometer bias [m^2/s^4]\r\n            gyb_0 3e-4;         X-Covariance of initial gyroscope bias [rad^2/s^2]\r\n            gyb_1 3e-4;         Y-Covariance of initial gyroscope bias [rad^2/s^2]\r\n            gyb_2 3e-4;         Z-Covariance of initial gyroscope bias [rad^2/s^2]\r\n            vep 0.01;           Covariance of initial linear camera-IMU extrinsics, same for all entries [m^2]\r\n            att_0 0.1;          X-Covariance of initial attitude [rad^2]\r\n            att_1 0.1;          Y-Covariance of initial attitude [rad^2]\r\n            att_2 0.1;          Z-Covariance of initial attitude [rad^2]\r\n            vea 0.01;           Covariance of initial rotational camera-IMU extrinsics, same for all entries [rad^2]\r\n        }\r\n    }\r\n    ImgUpdate\r\n    {\r\n        updateVecNormTermination 1e-4;\r\n        maxNumIteration 20;\r\n        doPatchWarping true;                                        Should the patches be warped\r\n        doFrameVisualisation true;                                  Should the frames be visualized\r\n        visualizePatches true;                                      Should the patches be visualized\r\n        useDirectMethod true;                                       Should the EKF-innovation be based on direct intensity error (o.w. reprojection error)\r\n        ;innovationInterpolationFactor 1.0;                         How much should be used from the direct or indirect error terms. Value must be between 0 and 1.\r\n        startLevel 2;                                               Highest patch level which is being employed (must be smaller than the hardcoded template parameter)\r\n        endLevel 1;                                                 Lowest patch level which is being employed\r\n        nDetectionBuckets 100;                                      Number of discretization buckets used during the candidates selection\r\n        MahalanobisTh 10.88; orig2.21;                                          Mahalanobis treshold for the update, 5.8858356\r\n        UpdateNoise\r\n        {\r\n            nor 1e-5;\r\n            pix 1;2 orig                                                Covariance used for the reprojection error, 1/focal_length is roughly 1 pixel std [rad] (~1/f ~ 1/400^2 = 1/160000)\r\n            int 10;10000 originally                                 Covariance used for the photometric error [intensity^2]\r\n            dist_0 0.05;                                                \r\n        }\r\n        initCovFeature_0 0.2;                                       Covariance for the initial distance (in the selected parametrization)\r\n        initCovFeature_1 1e-5;                                      Covariance for the initial bearing vector, x-component [rad]\r\n        initCovFeature_2 1e-5;                                      Covariance for the initial bearing vector, y-component [rad]\r\n        initCovTagFeature_0 0.1;                                    Covariance for the tag corners initial distance (in the selected parametrization)\r\n        initCovTagFeature_1 1e-5;                                   Covariance for the tag corners initial bearing vector, x-component [rad]\r\n        initCovTagFeature_2 1e-5;                                   Covariance for the tag corners initial bearing vector, y-component [rad]\r\n        initDepth 2.85;0.5 orig                                             Initial value for the initial distance parameter\r\n        startDetectionTh 0.9;                                       Threshold for detecting new features (min: 0, max: 1)\r\n        scoreDetectionExponent 0.25;                                Exponent used for weighting the distance between candidates\r\n        penaltyDistance 50;                                     Maximal distance which gets penalized during bucketing [pix]\r\n        zeroDistancePenalty 100;                                    Penalty for zero distance (max: nDetectionBuckets)\r\n        statLocalQualityRange 10;                                   Number of frames for local quality evaluation\r\n        statLocalVisibilityRange 100;                               Number of frames for local visibility evaluation\r\n        statMinGlobalQualityRange 100;                              Minimum number of frames for obtaining maximal global quality\r\n        trackingUpperBound 0.9;                                     Threshold for local quality for min overall global quality\r\n        trackingLowerBound 0.8;                                     Threshold for local quality for max overall global quality\r\n        minTrackedAndFreeFeatures 0.75;                             Minimum of amount of feature which are either tracked or free\r\n        removalFactor 1.1;                                          Factor for enforcing feature removal if not enough free\r\n        minRelativeSTScore 0.75;                                    Minimum relative ST-score for extracting new feature patch\r\n        minAbsoluteSTScore 5.0;                                     Minimum absolute ST-score for extracting new feature patch\r\n        minTimeBetweenPatchUpdate 1.0;                              Minimum time between new multilevel patch extrection [s]\r\n        fastDetectionThreshold 20;                                  Fast corner detector treshold while adding new feature\r\n        alignConvergencePixelRange 10;                              Assumed convergence range for image alignment (gets scaled with the level) [pixels]\r\n        alignCoverageRatio 2;2 ORIG                                     How many sigma of the uncertainty should be covered in the adaptive alignement\r\n        alignMaxUniSample 1;                                        Maximal number of alignment seeds on one side -> total number of sample = 2n+1. Carefull can get very costly if diverging!\r\n        ;matchingPixelThreshold 0.0;                                    Below this treshold no pre-alignment is carried out [pixels]\r\n        ;minNoAlignment 5;                                          Minimal number of alignment every feature must make through.\r\n        ;specialLinearizationThreshold 0.0;                         Below this treshold no special linearization point gets computed [pixel]\r\n        patchRejectionTh 60.0; 50 orig                                      If the average itensity error is larger than this the feauture is rejected [intensity], if smaller 0 the no check is performed\r\n        alignmentHuberNormThreshold -1;                             Intensity error threshold for Huber norm (enabled if > 0)\r\n        alignmentGaussianWeightingSigma -1;                         Width of Gaussian which is used for pixel error weighting (enabled if > 0)\r\n        alignmentGradientExponent 0.0;                              Exponent used for gradient based weighting of residuals\r\n        useIntensityOffsetForAlignment true;                        Should an intensity offset between the patches be considered\r\n        useIntensitySqewForAlignment true;                          Should an intensity sqew between the patches be considered\r\n        ;bearingVectorMahalTh 30.21;                                    Threshold for the aligned patch to be accepted as inlier\r\n        removeNegativeFeatureAfterUpdate true;                      Should feature with negative distance get removed\r\n        maxUncertaintyToDepthRatioForDepthInitialization 0.3;0.3 orig       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\r\n        useCrossCameraMeasurements true;                            Should cross measurements between frame be used. Might be turned of in calibration phase.\r\n        doStereoInitialization true;                                Should a stereo match be used for feature initialization.\r\n        noiseGainForOffCamera 10.0;                                 Factor added on update noise if not main camera\r\n        discriminativeSamplingDistance 0.0;0.02 orig                        Sampling distance for checking discriminativity of patch (if <= 0.0 no check is performed).\r\n        discriminativeSamplingGain 2.0; 2.0 orig                            Gain for threshold above which the samples must lie (if <= 1.0 the patchRejectionTh is used).\r\n        MotionDetection\r\n        {\r\n            isEnabled 0;                                            Is the motion detection enabled\r\n            rateOfMovingFeaturesTh 0.5;                             Amount of feature with motion for overall motion detection\r\n            pixelCoordinateMotionTh 1.0;                            Threshold for motion detection for patched [pixels]\r\n            minFeatureCountForNoMotionDetection 5;                  Min feature count in frame for motion detection\r\n        }\r\n        ZeroVelocityUpdate\r\n        {\r\n            UpdateNoise\r\n            {\r\n                vel_0 0.01;                                         X-Covariance of zero velocity update [m^2/s^2]\r\n                vel_1 0.01;                                         Y-Covariance of zero velocity update [m^2/s^2]\r\n                vel_2 0.01;                                         Z-Covariance of zero velocity update [m^2/s^2]\r\n            }\r\n            MahalanobisTh0 7.689997599999999;                       Mahalanobid distance for zero velocity updates\r\n            minNoMotionTime 1.0;                                    Min duration of no-motion\r\n            isEnabled 0;                                            Should zero velocity update be applied, only works if MotionDetection.isEnabled is true\r\n        }\r\n    }\r\n    Prediction\r\n    {\r\n        PredictionNoise\r\n        {\r\n            pos_0 1e-4;                             X-covariance parameter of position prediction [m^2/s]\r\n            pos_1 1e-4;                             Y-covariance parameter of position prediction [m^2/s]\r\n            pos_2 1e-4;                             Z-covariance parameter of position prediction [m^2/s]\r\n            vel_0 4e-4; 4e-6                        X-covariance parameter of velocity prediction [m^2/s^3]\r\n            vel_1 4e-4;                             Y-covariance parameter of velocity prediction [m^2/s^3]\r\n            vel_2 4e-4;                             Z-covariance parameter of velocity prediction [m^2/s^3]\r\n            acb_0 1e-8;                             X-covariance parameter of accelerometer bias prediction [m^2/s^5]\r\n            acb_1 1e-8;                             Y-covariance parameter of accelerometer bias prediction [m^2/s^5]\r\n            acb_2 1e-8;                             Z-covariance parameter of accelerometer bias prediction [m^2/s^5]\r\n            gyb_0 3.8e-7;                           X-covariance parameter of gyroscope bias prediction [rad^2/s^3]\r\n            gyb_1 3.8e-7;                           Y-covariance parameter of gyroscope bias prediction [rad^2/s^3]\r\n            gyb_2 3.8e-7;                           Z-covariance parameter of gyroscope bias prediction [rad^2/s^3]\r\n            vep 1e-8;                               Covariance parameter of linear extrinsics prediction [m^2/s]\r\n            att_0 7.6e-6; 7.6e-7                    X-Covariance parameter of attitude prediction [rad^2/s]\r\n            att_1 7.6e-6;                           Y-Covariance parameter of attitude prediction [rad^2/s]\r\n            att_2 7.6e-6;                           Z-Covariance parameter of attitude prediction [rad^2/s]\r\n            vea 1e-8;                               Covariance parameter of rotational extrinsics prediction [rad^2/s]\r\n            dep 0.0001;                         Covariance parameter of distance prediction [m^2/s]\r\n            nor 0.00001;                                Covariance parameter of bearing vector prediction [rad^2/s]\r\n        }\r\n        MotionDetection\r\n        {\r\n            inertialMotionRorTh 0.1;                Treshold on rotational rate for motion detection [rad/s]\r\n            inertialMotionAccTh 0.5;                Treshold on acceleration for motion detection [m/s^2]\r\n        }\r\n    }\r\n    PoseUpdate\r\n    {\r\n        UpdateNoise\r\n        {\r\n            pos_0 0.5;                          X-Covariance of linear pose measurement update [m^2]\r\n            pos_1 0.5;                          Y-Covariance of linear pose measurement update [m^2]\r\n            pos_2 0.01;                         Z-Covariance of linear pose measurement update [m^2]\r\n            att_0 10.1;                         X-Covariance of rotational pose measurement update [rad^2]\r\n            att_1 10.1;                         Y-Covariance of rotational pose measurement update [rad^2]\r\n            att_2 10.1;                         Z-Covariance of rotational pose measurement update [rad^2]\r\n        }\r\n        init_cov_IrIW 1;                            Covariance of initial pose between inertial frames, linear part [m^2]\r\n        init_cov_qWI 1;                             Covariance of initial pose between inertial frames, rotational part [rad^2]\r\n        init_cov_MrMV 1;                            Covariance of initial pose between inertial frames, linear part [m^2]\r\n        init_cov_qVM 1;                             Covariance of initial pose between inertial frames, rotational part [rad^2]\r\n        pre_cov_IrIW 1e-4;                          Covariance parameter of pose between inertial frames, linear part [m^2/s]\r\n        pre_cov_qWI 1e-4;                           Covariance parameter of pose between inertial frames, rotational part [rad^2/s]\r\n        pre_cov_MrMV 1e-4;                          Covariance parameter of pose between inertial frames, linear part [m^2/s]\r\n        pre_cov_qVM 1e-4;                           Covariance parameter of pose between inertial frames, rotational part [rad^2/s]\r\n        MahalanobisTh0 12.6511204;                  Mahalonobis distance treshold of pose measurement\r\n        doVisualization true;                       Should the measured poses be vizualized\r\n        enablePosition true;                        Should the linear part be used during the update\r\n        enableAttitude false;                       Should the rotation part be used during the update (e.g. set to fals eif feeding GPS-measurement)\r\n        noFeedbackToRovio false;                        By default the pose update is only used for aligning coordinate frame. Set to false if ROVIO should benefit frome the posed measurements.\r\n        doInertialAlignmentAtStart true;            Should the transformation between I and W be explicitly computed and set with the first pose measurement.\r\n        timeOffset 9.9;                             Time offset added to the pose measurement timestamps\r\n        imu_timedelay 0.0;                      Artificially added IMU Timedelay [s] 0.0628505901891\r\n        qVM_x 0.0;                                  X-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)\r\n        qVM_y 1.0;                                  Y-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)\r\n        qVM_z 0.0;                                  Z-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)\r\n        qVM_w 0.0;                                  W-entry of quaterion representing IMU to reference body coordinate frame of pose measurement (JPL)\r\n        MrMV_x 0;                                   X-entry of vector representing IMU to reference body coordinate frame of pose measurement\r\n        MrMV_y 0;                                   Y-entry of vector representing IMU to reference body coordinate frame of pose measurement\r\n        MrMV_z 0;                                   Z-entry of vector representing IMU to reference body coordinate frame of pose measurement\r\n        qWI_x 0.0;                                  X-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)\r\n        qWI_y 0.0;                                  Y-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)\r\n        qWI_z 0.0;                                  Z-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)\r\n        qWI_w 1.0;                                  W-entry of quaterion representing World to reference inertial coordinate frame of pose measurement (JPL)\r\n        IrIW_x 0.5;                                 X-entry of vector representing World to reference inertial coordinate frame of pose measurement\r\n        IrIW_y -0.5;                                    Y-entry of vector representing World to reference inertial coordinate frame of pose measurement\r\n        IrIW_z 0;                                   Z-entry of vector representing World to reference inertial coordinate frame of pose measurement\r\n    }\r\n"}}