Esempio n. 1
0
        /// <summary>
        /// Constructor for known features. The different number of
        /// arguments differentiates it from the constructor for partially-initialised
        /// features
        /// </summary>
        /// <param name="id">reference to the feature identifier</param>
        /// <param name="?"></param>
        public Feature(byte[] id, uint lab, uint list_pos,
                       Scene_Single scene, Vector y_known,
                       Vector xp_o,
                       Feature_Measurement_Model f_m_m, uint k_f_l)
        {
            feature_measurement_model = f_m_m;
            feature_constructor_bookeeping();

            identifier       = id;
            label            = lab;
            position_in_list = list_pos;   // Position of new feature in list

            // Save the vehicle position where this feature was acquired
            xp_orig = new Vector(xp_o);

            // Straighforward initialisation of state and covariances
            y   = y_known;
            Pxy = new MatrixFixed(scene.get_motion_model().STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
            Pxy.Fill(0.0f);
            Pyy = new MatrixFixed(feature_measurement_model.FEATURE_STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
            Pyy.Fill(0.0f);

            int         i = 0;
            MatrixFixed newPyjyi_to_store;

            foreach (Feature it in scene.get_feature_list_noconst())
            {
                if (i < position_in_list)
                {
                    newPyjyi_to_store = new MatrixFixed(
                        it.get_feature_measurement_model().FEATURE_STATE_SIZE,
                        feature_measurement_model.FEATURE_STATE_SIZE);

                    //add to the list
                    matrix_block_list.Add(newPyjyi_to_store);
                }

                i++;
            }

            known_feature_label = (int)k_f_l;

            if (feature_measurement_model.fully_initialised_flag)
            {
                partially_initialised_feature_measurement_model = null;
                fully_initialised_feature_measurement_model     =
                    (Fully_Initialised_Feature_Measurement_Model)feature_measurement_model;
            }
            else
            {
                fully_initialised_feature_measurement_model     = null;
                partially_initialised_feature_measurement_model =
                    (Partially_Initialised_Feature_Measurement_Model)feature_measurement_model;
            }
        }
Esempio n. 2
0
        /// <summary>
        /// project a feature from 3D into 2D image coordinates
        /// </summary>
        /// <param name="feat"></param>
        public void projectFeature(Feature feat, ref int screen_x, ref int screen_y)
        {
            Vector yi = feat.get_y();
            Vector xp = scene.get_motion_model().get_xpRES();

            Fully_Initialised_Feature_Measurement_Model fully_init_fmm =
                feat.get_fully_initialised_feature_measurement_model();

            fully_init_fmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yi, xp);

            screen_x = (int)fully_init_fmm.get_hiRES()[0];
            screen_y = (int)fully_init_fmm.get_hiRES()[1];
        }
Esempio n. 3
0
        /// <summary>
        /// Convert a partially-initialised feature to a fully-initialised feature,
        /// given information about the free parameters \vct{\lambda}.
        /// The new state \vct{y}_{fi} is given by calling
        /// Partially_Initialised_Feature_Measurement_Model::func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda().
        /// where the various Jacobians are returned by calls to
        /// Partially_Initialised_Feature_Measurement_Model, and the covariance matrices
        /// \mat{P}_{kl} are already known and stored in the class, except for
        /// \mat{P}_{\vct{\lambda}}, which is passed to the function.
        /// </summary>
        /// <param name="lambda">The mean value for \vct{\lambda}</param>
        /// <param name="Plambda">The covariance for \vct{\lambda}</param>
        /// <param name="scene">The SLAM map</param>
        public void convert_from_partially_to_fully_initialised(
            Vector lambda, MatrixFixed Plambda, Scene_Single scene)
        {
            // We'll do all the work here in feature.cc though probably this only
            // works with scene_single...

            // We calculate new state yfi(ypi, lambda)
            // New feature covariance
            // Pyfiyfi = dyfi_by_dypi Pypiypi dyfi_by_dypiT +
            //           dyfi_by_dlambda Plambda dyfi_by_dlambdaT
            // And we change cross covariances as follows:
            // Pxyfi = Pxypi dyfi_by_dypiT
            // Pyjyfi = Pyjypi dyfi_by_dypiT   for j < i (since we only store top-right
            // Pyfiyj = dyfi_by_dypi Pypiyj    for j > i  part of covariance matrix)

            partially_initialised_feature_measurement_model.func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda(y, lambda);

            MatrixFixed dyfi_by_dypiT    = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES().Transpose();
            MatrixFixed dyfi_by_dlambdaT = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES().Transpose();

            // Replace y
            y = new Vector(partially_initialised_feature_measurement_model.get_yfiRES());

            // Replace Pxy
            Pxy = Pxy * dyfi_by_dypiT;

            // Replace Pyy
            MatrixFixed Pypiypi_1 = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() *
                                    Pyy * dyfi_by_dypiT;
            MatrixFixed Pypiypi_2 = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES() *
                                    Plambda * dyfi_by_dlambdaT;

            Pyy = Pypiypi_1 + Pypiypi_2;

            // Pyjyi elements for j < i (covariance with features before i in list)
            uint i = position_in_list;

            MatrixFixed m_it;
            int         j;

            for (j = 0; j < position_in_list; j++)
            {
                m_it = (MatrixFixed)matrix_block_list[j];
                matrix_block_list[j] = m_it * dyfi_by_dypiT;
            }


            Feature it;
            int     idx = scene.feature_list.IndexOf(this);

            for (j = idx + 1; j < scene.feature_list.Count; j++)
            {
                it = (Feature)(scene.feature_list[j]);
                it.matrix_block_list[(int)i] = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() * (MatrixFixed)it.matrix_block_list[(int)i];
                it.increment_position_in_total_state_vector(-(int)feature_measurement_model.FEATURE_STATE_SIZE);
            }


            // Change the total state size in scene, here with a negative increment
            uint size1 = partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model.FEATURE_STATE_SIZE;
            uint size2 = partially_initialised_feature_measurement_model.FEATURE_STATE_SIZE;

            scene.increment_total_state_size((int)size1 - (int)size2);

            // Change fmm for this model to fully-initialised one
            feature_measurement_model =
                partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model;

            partially_initialised_feature_measurement_model = null;
            fully_initialised_feature_measurement_model     =
                (Fully_Initialised_Feature_Measurement_Model)feature_measurement_model;


            //assert(fully_initialised_feature_measurement_model != NULL);

            // Need to reallocate any other matrices
            // Assume that measurement size doesn't change
            dh_by_dy.Resize(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
        }
Esempio n. 4
0
        /// <summary>
        /// Constructor for partially-initialised features.
        /// </summary>
        /// <param name="id">reference to the feature</param>
        /// <param name="lab"></param>
        /// <param name="list_pos"></param>
        /// <param name="scene"></param>
        /// <param name="h"></param>
        /// <param name="p_i_f_m_m"></param>
        public Feature(byte[] id, uint lab, uint list_pos,
                       Scene_Single scene, Vector h,
                       Partially_Initialised_Feature_Measurement_Model p_i_f_m_m,
                       Vector feature_colour)
        {
            feature_measurement_model = p_i_f_m_m;
            partially_initialised_feature_measurement_model = p_i_f_m_m;
            fully_initialised_feature_measurement_model     = null;

            // Stuff below substituted from Feature_common
            //   Feature_common(id, lab, list_pos, scene, h);

            feature_constructor_bookeeping();

            identifier       = id;
            label            = lab;
            position_in_list = list_pos;        // Position of new feature in list
            position_in_total_state_vector = 0; // This should be set properly
            colour = feature_colour;
            //when feature is added

            // Save the vehicle position where this feature was acquired
            scene.get_motion_model().func_xp(scene.get_xv());
            //xp_orig = scene.get_motion_model().get_xpRES();
            xp_orig = new Vector(scene.get_motion_model().get_xpRES());

            // Call model functions to calculate feature state, measurement noise
            // and associated Jacobians. Results are stored in RES matrices

            // First calculate "position state" and Jacobian
            scene.get_motion_model().func_xp(scene.get_xv());
            scene.get_motion_model().func_dxp_by_dxv(scene.get_xv());

            // Now ask the model to initialise the state vector and calculate Jacobians
            // so that I can go and calculate the covariance matrices
            partially_initialised_feature_measurement_model.func_ypi_and_dypi_by_dxp_and_dypi_by_dhi_and_Ri(h, scene.get_motion_model().get_xpRES());

            // State y
            //y = partially_initialised_feature_measurement_model.get_ypiRES();
            y = new Vector(partially_initialised_feature_measurement_model.get_ypiRES());

            // Temp_FS1 will store dypi_by_dxv
            MatrixFixed Temp_FS1 =
                partially_initialised_feature_measurement_model.get_dypi_by_dxpRES() *
                scene.get_motion_model().get_dxp_by_dxvRES();

            // Pxy
            Pxy = scene.get_Pxx() * Temp_FS1.Transpose();

            // Pyy
            Pyy = Temp_FS1 * scene.get_Pxx() * Temp_FS1.Transpose()
                  + partially_initialised_feature_measurement_model.get_dypi_by_dhiRES()
                  * partially_initialised_feature_measurement_model.get_RiRES()
                  * partially_initialised_feature_measurement_model.get_dypi_by_dhiRES().Transpose();

            // Covariances of this feature with others
            int j = 0;

            foreach (Feature it in scene.get_feature_list_noconst())
            {
                if (j < position_in_list)
                {
                    // new Pypiyj = dypi_by_dxv . Pxyj
                    // Size of this is FEATURE_STATE_SIZE(new) by FEATURE_STATE_SIZE(old)
                    MatrixFixed m = it.get_Pxy();
                    MatrixFixed newPyjypi_to_store = (Temp_FS1 * m).Transpose();

                    //add to the list
                    matrix_block_list.Add(newPyjypi_to_store);
                }
                j++;
            }

            known_feature_label = -1;
        }
        // Function to find a region in an image guided by current motion prediction
        // which doesn't overlap existing features
        public static bool FindNonOverlappingRegion(Scene_Single scene,
                                                    Vector local_u,
                                                    float delta_t,
                                                    Partially_Initialised_Feature_Measurement_Model default_feature_type_for_initialisation,
                                                    uint camera_width,
                                                    uint camera_height,
                                                    uint BOXSIZE,
                                                    ref int init_feature_search_ustart,
                                                    ref int init_feature_search_vstart,
                                                    ref int init_feature_search_ufinish,
                                                    ref int init_feature_search_vfinish,
                                                    uint FEATURE_INIT_STEPS_TO_PREDICT,
                                                    float FEATURE_INIT_DEPTH_HYPOTHESIS,
                                                    Random rnd)
        {
            ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();

            Vector local_xv = new Vector(scene.get_xv());

            for (uint i = 0; i < FEATURE_INIT_STEPS_TO_PREDICT; i++)
            {
                scene.get_motion_model().func_fv_and_dfv_by_dxv(local_xv, local_u, delta_t);
                local_xv.Update(scene.get_motion_model().get_fvRES());
            }

            threed_motion_model.func_xp(local_xv);
            Vector local_xp = new Vector(threed_motion_model.get_xpRES());

            threed_motion_model.func_r(local_xp);
            Vector3D rW = threed_motion_model.get_rRES();

            threed_motion_model.func_q(local_xp);
            Quaternion qWR = threed_motion_model.get_qRES();

            // yW =  rW + RWR hR
            Vector3D hR = new Vector3D(0.0f, 0.0f, FEATURE_INIT_DEPTH_HYPOTHESIS);

            // Used Inverse + transpose because this was't compiling the normal way
            Vector3D yW = new Vector3D(rW + qWR.RotationMatrix() * hR);

            // Then project that point into the current camera position
            scene.get_motion_model().func_xp(scene.get_xv());

            Fully_Initialised_Feature_Measurement_Model fully_init_fmm =
                (Fully_Initialised_Feature_Measurement_Model)(default_feature_type_for_initialisation.more_initialised_feature_measurement_model);


            Vector yWVNL = yW.GetVNL3();

            fully_init_fmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yWVNL, scene.get_motion_model().get_xpRES());

            // Now, this defines roughly how much we expect a feature initialised to move
            float suggested_u        = fully_init_fmm.get_hiRES()[0];
            float suggested_v        = fully_init_fmm.get_hiRES()[1];
            float predicted_motion_u = camera_width / 2.0f - suggested_u;
            float predicted_motion_v = camera_height / 2.0f - suggested_v;

            // So, the limits of a "safe" region within which we can initialise
            // features so that they end up staying within the screen
            // (Making the approximation that the whole screen moves like the
            // centre point)
            int safe_feature_search_ustart  = (int)(-predicted_motion_u);
            int safe_feature_search_vstart  = (int)(-predicted_motion_v);
            int safe_feature_search_ufinish = (int)(camera_width - predicted_motion_u);
            int safe_feature_search_vfinish = (int)(camera_height - predicted_motion_v);

            if (safe_feature_search_ustart < ((int)((BOXSIZE - 1) / 2) + 1))
            {
                safe_feature_search_ustart = (int)((BOXSIZE - 1) / 2 + 1);
            }
            if (safe_feature_search_ufinish > (int)camera_width - ((int)((BOXSIZE - 1) / 2) + 1))
            {
                safe_feature_search_ufinish = (int)(camera_width - (BOXSIZE - 1) / 2 - 1);
            }
            if (safe_feature_search_vstart < ((int)((BOXSIZE - 1) / 2) + 1))
            {
                safe_feature_search_vstart = (int)((BOXSIZE - 1) / 2 + 1);
            }
            if (safe_feature_search_vfinish > (int)camera_height - ((int)((BOXSIZE - 1) / 2) + 1))
            {
                safe_feature_search_vfinish = (int)(camera_height - (BOXSIZE - 1) / 2 - 1);
            }

            return(FindNonOverlappingRegionNoPredict(safe_feature_search_ustart,
                                                     safe_feature_search_vstart,
                                                     safe_feature_search_ufinish,
                                                     safe_feature_search_vfinish,
                                                     scene,
                                                     ref init_feature_search_ustart,
                                                     ref init_feature_search_vstart,
                                                     ref init_feature_search_ufinish,
                                                     ref init_feature_search_vfinish, rnd));
        }
        /// <summary>
        /// Function to find non-overlapping region over without prediction
        /// this is really the service function called by both the above
        /// </summary>
        /// <param name="safe_feature_search_ustart"></param>
        /// <param name="safe_feature_search_vstart"></param>
        /// <param name="safe_feature_search_ufinish"></param>
        /// <param name="safe_feature_search_vfinish"></param>
        /// <param name="scene"></param>
        /// <param name="init_feature_search_ustart"></param>
        /// <param name="init_feature_search_vstart"></param>
        /// <param name="init_feature_search_ufinish"></param>
        /// <param name="init_feature_search_vfinish"></param>
        /// <param name="rnd"></param>
        /// <returns></returns>
        public static bool FindNonOverlappingRegionNoPredict(
            int safe_feature_search_ustart,
            int safe_feature_search_vstart,
            int safe_feature_search_ufinish,
            int safe_feature_search_vfinish,
            Scene_Single scene,
            ref int init_feature_search_ustart,
            ref int init_feature_search_vstart,
            ref int init_feature_search_ufinish,
            ref int init_feature_search_vfinish,
            Random rnd)
        {
            int i, j;
            //if (Camera_Constants.DEBUGDUMP) cout << "FNOLRNP timer start: " << timerlocal << endl;

            int INIT_FEATURE_SEARCH_WIDTH  = 80;
            int INIT_FEATURE_SEARCH_HEIGHT = 60;

            // Within this, choose a random region
            // Check that we've got some room for manouevre
            if ((safe_feature_search_ufinish - safe_feature_search_ustart > INIT_FEATURE_SEARCH_WIDTH) &&
                (safe_feature_search_vfinish - safe_feature_search_vstart > INIT_FEATURE_SEARCH_HEIGHT))
            {
                // Try a few times to get one that's not overlapping with any features
                // we know about
                int NUMBER_OF_RANDOM_INIT_FEATURE_SEARCH_REGION_TRIES = 5;
                int FEATURE_SEPARATION_MINIMUM = (int)Camera_Constants.BOXSIZE * 3;

                // Build vectors of feature positions so we only have to work them out once
                ArrayList u_array = new ArrayList();
                ArrayList v_array = new ArrayList();

                Feature it;
                for (j = 0; j < scene.get_feature_list().Count; j++)
                {
                    it = (Feature)(scene.get_feature_list())[j];

                    //Vector z = it.get_z();
                    //u_array.Add(z[0]);
                    //v_array.Add(z[1]);


                    if (it.get_feature_measurement_model().fully_initialised_flag)
                    {
                        //Vector z = it.get_z();
                        //u_array.Add(z[0]);
                        //v_array.Add(z[1]);


                        Fully_Initialised_Feature_Measurement_Model fifmm =
                            (Fully_Initialised_Feature_Measurement_Model)(it.get_feature_measurement_model());
                        fifmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(it.get_y(), scene.get_motion_model().get_xpRES());

                        // Check that this is not a feature behind the camera
                        if (it.get_feature_measurement_model().feature_graphics_type == "THREED_POINT")
                        {
                            fifmm.func_zeroedyigraphics_and_Pzeroedyigraphics(it.get_y(),
                                                                              scene.get_xv(), scene.get_Pxx(), it.get_Pxy(), it.get_Pyy());

                            if (fifmm.get_zeroedyigraphicsRES()[2] > 0)
                            {
                                u_array.Add(fifmm.get_hiRES()[0]);
                                v_array.Add(fifmm.get_hiRES()[1]);
                            }
                        }
                    }
                }

                //if (Camera_Constants.DEBUGDUMP) cout << "FNOLRNP timer after functions: " << timerlocal << endl;

                bool feature_found = false;
                i = 0;
                while (i < NUMBER_OF_RANDOM_INIT_FEATURE_SEARCH_REGION_TRIES)
                {
                    int u_offset = (int)((safe_feature_search_ufinish - safe_feature_search_ustart
                                          - INIT_FEATURE_SEARCH_WIDTH) * (rnd.Next(10000) / 10000.0f));
                    int v_offset = (int)((safe_feature_search_vfinish - safe_feature_search_vstart
                                          - INIT_FEATURE_SEARCH_HEIGHT) * (rnd.Next(10000) / 10000.0f));

                    init_feature_search_ustart  = safe_feature_search_ustart + u_offset;
                    init_feature_search_ufinish = init_feature_search_ustart + INIT_FEATURE_SEARCH_WIDTH;
                    init_feature_search_vstart  = safe_feature_search_vstart + v_offset;
                    init_feature_search_vfinish = init_feature_search_vstart + INIT_FEATURE_SEARCH_HEIGHT;

                    bool found_a_feature_in_region_flag = false;

                    // These arrays will be the same size
                    float uit, vit;
                    for (j = 0; j < u_array.Count; j++)
                    {
                        uit = (float)u_array[j];
                        vit = (float)v_array[j];

                        if ((uit >= init_feature_search_ustart - FEATURE_SEPARATION_MINIMUM) &&
                            (uit < init_feature_search_ufinish + FEATURE_SEPARATION_MINIMUM) &&
                            (vit >= init_feature_search_vstart - FEATURE_SEPARATION_MINIMUM) &&
                            (vit < init_feature_search_vfinish + FEATURE_SEPARATION_MINIMUM))
                        {
                            found_a_feature_in_region_flag = true;
                            feature_found = true;
                            break;
                        }
                    }

                    if (!found_a_feature_in_region_flag)
                    {
                        break;
                    }

                    i++;
                }

                if (!feature_found)
                {
                    return(false);
                }
            }
            else
            {
                return(false);
            }

            return(true);
        }