C# Vector.SquaredMagnitude方法代码示例

        /// <summary>
        /// Project a point from an ideal (Euclidean) camera frame into image co-ordinates
        /// with radial distortion.
        ///
        /// This first transforms the input position vector  vct{y}  (relative to
        /// the camera position) into an undistorted image location  \vct{u}_c 
        /// (relative to an origin at the optical centre)
        /// 
        ///     vct{u}_c = evct{u_c \ v_c} = evct{-F_{ku} y_x / y_z \ -F_{kv} y_y / y_z} =
        ///     emat{-F_{ku} & 0 \ 0 & -F_{kv}}\evct{y_x/y_z \ y_y/y_z}
        /// 
        /// Then radial distortion is applied to give the final image location  h 
        /// (with the origin back in the normal location of of the top left of the image).
        /// 
        ///     vct{h} = frac{vct{u}_c}{sqrt{1 + 2 k_1 |vct{u}_c|^2}} + evct{u_0 \ v_0}
        /// </summary>
        /// <param name="camera"></param>
        /// <returns></returns>
        public virtual Vector Project(Vector camera)
        {
            // Remember this position in case we are asked about the Jacobians of this
            // transformation
            m_last_camera = camera;
  
            // First do perspective projection
            // This turns the position vector into an undistorted image location (with the
            // origin at the centre)
            // Use -Fkuv to swap the image co-ordinates (0,0) at the top to
            // camera co-ordinates (0,0) at the bottom

            Vector imagepos_centred = new Vector(2);
            imagepos_centred[0] = -m_Fku * camera[0] / camera[2];
            imagepos_centred[1] = -m_Fkv * camera[1] / camera[2];

            m_last_image_centred = imagepos_centred;

            // 1 distortion coefficient model
            float radius2 = imagepos_centred.SquaredMagnitude();
            float factor = (float)Math.Sqrt(1 + 2 * m_Kd1 * radius2);
            return imagepos_centred / factor + m_centre;
        }

//.........这里部分代码省略.........
                Vector xv = scene.get_xv();
                scene.get_motion_model().func_xp(xv);
                Vector prev_xp_pos = (scene.get_motion_model().get_xpRES()).Extract(3);

                // Prediction step
                if (currently_mapping_flag)
                    kalman.predict_filter_fast(scene, u, delta_t);

                // if features are not seen the first time try a few more times
                int tries = 0;
                number_of_visible_features = 0;
                while (((tries == 0) || (scene.get_no_selected() < 2)) && (tries < 5))
                {
                    number_of_visible_features = scene.auto_select_n_features(NUMBER_OF_FEATURES_TO_SELECT);

                    if (scene.get_no_selected() > 0)
                    {
                        //scene.predict_measurements(); // commented out in original code                        

                        number_of_matched_features = (uint)SceneLib.make_measurements(scene, sim_or_rob, rnd);

                        if (scene.get_successful_measurement_vector_size() != 0)
                        {
                            // this function is the slowest part of the algorithm
                            kalman.total_update_filter_slow(scene);

                            scene.normalise_state();
                        }

                    }

                    tries++;
                }


                if (currently_mapping_flag)
                    scene.delete_bad_features();

                // Let's enforce symmetry of covariance matrix...
                // Add to transpose and divide by 2                
                uint tot_state_size = scene.get_total_state_size();
                MatrixFixed Pxx = new MatrixFixed(tot_state_size, tot_state_size);
                scene.construct_total_covariance(ref Pxx);
                MatrixFixed PxxT = Pxx.Transpose();                

                Pxx.Update(Pxx * 0.5f + PxxT * 0.5f);
                scene.fill_covariances(Pxx);
                
                // Look at camera speed estimate
                // Get the current position and estimate the speed from it
                xv = scene.get_xv();
                scene.get_motion_model().func_xp(xv);
                Vector xp_pos = scene.get_motion_model().get_xpRES().Extract(3);
                velocity = (xp_pos - prev_xp_pos) / delta_t;
                speed = (float)Math.Sqrt(velocity.SquaredMagnitude());

                // This section of code is a workaround for a situation where
                // the camera suddenly shoots off at high speed, which must be
                // a bug perhaps in the state update.  If the camera suddenly accelerates
                // at a high rate then this just sets the state back to the previous one.
                if (prev_speed == 0) 
                    prev_speed = speed;
                else
                {
                    float speed_change = speed / prev_speed;
                    if ((speed > 1) && (speed_change > 1.2) && (prev_xv != null))
                    {
                        xv.Update(prev_xv);
                    }
                }
                prev_speed = speed;
                if (prev_xv != null)  // TODO: minor bug here with vector != operator
                    prev_xv.Update(xv);
                else
                    prev_xv = new Vector(xv);

                if (currently_mapping_flag)
                {
                    if (speed > 0.2)
                    {
                        if ((number_of_visible_features < NUMBER_OF_FEATURES_TO_KEEP_VISIBLE) &&
                            (scene.get_feature_init_info_vector().Count < (uint)(MAX_FEATURES_TO_INIT_AT_ONCE)))
                        {
                            // if the number of features is low make more attempts
                            // to initialise new ones
                            tries = 1;
                            if (number_of_visible_features < 8) tries = 2;
                            if (number_of_visible_features < 5) tries = 3;
                            for (int i = 0; i < tries; i++) AutoInitialiseFeature(u, delta_t);
                        }
                    }
                }

                MatchPartiallyInitialisedFeatures();                

                recordCameraHistory();
                 
            }
            return true;
        }