Exemple #1
0
        public override void func_hi_and_dhi_by_dxp_and_dhi_by_dyi(Vector yi, Vector xp)
        {
            // This function gives relative position of feature: also call this hR
            // (vector from camera to feature in robot frame)
            func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, xp);

            // Project this from 3D into the 2D image using our camera
            Vector     feature_3D_position = get_zeroedyiRES();
            WideCamera cam = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera;

            hiRES = cam.Project(feature_3D_position);

            // And ask the camera what the Jacobian of this projection was
            MatrixFixed dhid_by_dzeroedyi = cam.ProjectionJacobian();

            // Form the required Jacobians
            dhi_by_dxpRES = dhid_by_dzeroedyi * dzeroedyi_by_dxpRES;
            dhi_by_dyiRES = dhid_by_dzeroedyi * dzeroedyi_by_dyiRES;
        }