VariableSet GeneralConstraint::getVariables( const SatID& sat, const TypeIDSet& typeSet ) { VariableSet vset; VariableSet varSet = getVariables(sat); for(VariableSet::iterator itv=varSet.begin(); itv!=varSet.end(); ++itv) { TypeIDSet::const_iterator it = typeSet.find(itv->getType()); if( (it!=typeSet.end()) ) vset.insert(*itv); } return vset; } // End of method 'GeneralConstraint::getVariables(...'
VariableSet GeneralConstraint::getVariables( const SourceID& source, const TypeIDSet& typeSet ) { VariableSet vset; VariableSet varSet = getVariables(source); for(VariableSet::iterator itv=varSet.begin(); itv!=varSet.end(); ++itv) { TypeIDSet::const_iterator it = typeSet.find(itv->getType()); if( (it!=typeSet.end()) && itv->getSourceIndexed() ) { vset.insert(*itv); } } return vset; } // End of method 'GeneralConstraint::getVariables'
// Compute hMatrix and rMatrix void EquationSystem::getGeometryWeights( gnssDataMap& gdsMap ) { // Resize hMatrix and rMatrix hMatrix.resize( measVector.size(), varUnknowns.size(), 0.0); rMatrix.resize( measVector.size(), measVector.size(), 0.0); // Let's work with the first element of the data structure gnssDataMap gds2( gdsMap.frontEpoch() ); // Let's fill weights and geometry matrices int row(0); // Declare a counter for row number for( std::list<Equation>::const_iterator itRow = currentEquationsList.begin(); itRow != currentEquationsList.end(); ++itRow ) { // Create temporal GDS objects SourceID source( (*itRow).header.equationSource ); SatID sat( (*itRow).header.equationSat ); // Get a TypeIDSet with all the data types present in current GDS // Declare an appropriate object TypeIDSet typeSet; // We need a flag bool found( false ); // Iterate through data structure for( gnssDataMap::const_iterator itGDS = gds2.begin(); itGDS != gds2.end() && !found; ++itGDS ) { // Look for source sourceDataMap::const_iterator itSDM = (*itGDS).second.find(source); if( itSDM != (*itGDS).second.end() ) { // Get the types typeSet = (*itSDM).second.getTypeID(); found = true; } } // First, fill weights matrix // Check if current GDS has weight info. If you don't want those // weights to get into equations, please don't put them in GDS if( typeSet.find(TypeID::weight) != typeSet.end() ) { // Weights matrix = Equation weight * observation weight rMatrix(row,row) = (*itRow).header.constWeight * gds2.getValue(source, sat, TypeID::weight); } else { // Weights matrix = Equation weight rMatrix(row,row) = (*itRow).header.constWeight; } // Second, fill geometry matrix: Look for equation coefficients int col(0); // Declare a counter for column number for( VariableSet::const_iterator itCol = varUnknowns.begin(); itCol != varUnknowns.end(); ++itCol ) { // Check if unknown is in current equation and also is marked // as a current unknown if( (*itRow).body.find( (*itCol) ) != (*itRow).body.end() && currentUnknowns.find( (*itCol) ) != currentUnknowns.end() ) { // Check if '(*itCol)' unknown variable enforces a specific // coefficient if( (*itCol).isDefaultForced() ) { // Use default coefficient hMatrix(row,col) = (*itCol).getDefaultCoefficient(); } else { // Look the coefficient in provided data // Get type of current varUnknown TypeID type( (*itCol).getType() ); // Check if this type has an entry in current GDS type set if( typeSet.find(type) != typeSet.end() ) { // If type was found, insert value into hMatrix hMatrix(row,col) = gds2.getValue(source, sat, type); } else { // If value for current type is not in gdsMap, then // insert default coefficient for this variable hMatrix(row,col) = (*itCol).getDefaultCoefficient(); } } // End of 'if( (*itCol).isDefaultForced() ) ...' } // End of 'if( (*itRow).body.find( (*itCol) ) != ...' // Increment column counter ++col; } // End of 'for( VariableSet::const_iterator itCol = ...' // Handle type index variable for( VariableSet::const_iterator itCol = (*itRow).body.begin(); itCol != (*itRow).body.end(); ++itCol ) { VariableSet::const_iterator itr = rejectUnknowns.find( (*itCol) ); if( itr == rejectUnknowns.end() || (*itr).getTypeIndexed()) continue; Variable var(*itr); col = 0; for( VariableSet::const_iterator it = varUnknowns.begin(); it != varUnknowns.end(); it++) { if(((*itCol).getType() == (*it).getType()) && ((*itCol).getModel() == (*it).getModel()) && ((*itCol).getSourceIndexed() == (*it).getSourceIndexed())&& ((*itCol).getSatIndexed() == (*it).getSatIndexed()) && ((*itCol).getSource() == (*it).getSource()) && ((*itCol).getSatellite() == (*it).getSatellite()) ) { break; } col++; } // Check if '(*itCol)' unknown variable enforces a specific // coefficient if( (*itCol).isDefaultForced() ) { // Use default coefficient hMatrix(row,col) = (*itCol).getDefaultCoefficient(); } else { // Look the coefficient in provided data // Get type of current varUnknown TypeID type( (*itCol).getType() ); // Check if this type has an entry in current GDS type set if( typeSet.find(type) != typeSet.end() ) { // If type was found, insert value into hMatrix hMatrix(row,col) = gds2.getValue(source, sat, type); } else { // If value for current type is not in gdsMap, then // insert default coefficient for this variable hMatrix(row,col) = (*itCol).getDefaultCoefficient(); } } // End of 'if( (*itCol).isDefaultForced() ) ...' } // Increment row number ++row; } // End of 'std::list<Equation>::const_iterator itRow = ...' } // End of method 'EquationSystem::getGeometryWeights()'