/// <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); }