bool
solvetestFixedSize(void)
{
Size s(n, n);
for (unsigned i = 0; i < 100; ++i)
if (!solvetest(rMatrix(s), rVec(s(0))))
return false;
return true;
}
bool
solvetestVariableSize(unsigned n)
{
Size s(n, n);
for (unsigned i = 0; i < 100; ++i)
if (!solvetest(rMatrix(s), rnVec(s(0))))
return false;
return true;
}
void convert(Rcpp::List& samples, const std::vector<chordalGraph::stochasticEnumerationArgs::matrixType>& boostMatrices)
{
samples = Rcpp::List(boostMatrices.size());
for (int i = 0; i < (int)boostMatrices.size(); i++)
{
const chordalGraph::stochasticEnumerationArgs::matrixType& currentMatrix = boostMatrices[i];
int size1 = currentMatrix.size1();
Rcpp::LogicalMatrix rMatrix(size1);
for (int j = 0; j < size1; j++)
{
for (int k = 0; k < size1; k++)
{
rMatrix(j, k) = currentMatrix(j, k);
}
}
samples[i] = rMatrix;
}
}
Vec3f Vec3f::rotateY(float radians) {
Vec3f tempVec = (*this);
float rotationArray[4][4] = { {cos(radians), 0, -sin(radians), 0},
{0, 1, 0, 0},
{sin(radians), 0, cos(radians), 0},
{0, 0, 0, 1}
};
Matrix44f rMatrix(rotationArray);
tempVec *= rMatrix;
return tempVec;
}
Matrix4 Matrix4::rigidInverse(void)
{
Matrix4 b;
//Project 2
//Calculate the inverse of this matrix with the assumption that it is a rigid transformation
//This will be useful when implementing cameras!
Matrix4 rMatrix( m[0][0], m[0][1], m[0][2],0, m[1][0], m[1][1], m[1][2], 0, m[2][0], m[2][1], m[2][2], 0, 0, 0, 0, 1);
Matrix4 tInvMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, -m[3][0], -m[3][1], -m[3][2], 1);
b = rMatrix.transpose().multiply(tInvMatrix);
return b;
}
// Impose the constraints system to the equation system
// the prefit residuals vector, hMatrix and rMatrix will be appended.
void EquationSystem::imposeConstraints()
{
if(!equationConstraints.hasConstraints()) return;
ConstraintList destList;
ConstraintList tempList = equationConstraints.getConstraintList();
for(ConstraintList::iterator it = tempList.begin();
it != tempList.end();
++it )
{
VariableDataMap dataMapOk;
bool validConstraint(true);
try
{
VariableDataMap dataMap = it->body;
for(VariableDataMap::iterator itv = dataMap.begin();
itv != dataMap.end();
++itv )
{
bool isFound(false);
VariableSet::iterator itv2 = varUnknowns.find(itv->first);
if(itv2!=varUnknowns.end())
{
isFound = true;
dataMapOk[*itv2] = dataMap[itv->first];
}
else
{
for(itv2 = varUnknowns.begin();
itv2 != varUnknowns.end();
++itv2 )
{
if( (itv->first.getType() == itv2->getType()) &&
(itv->first.getSource() == itv2->getSource()) &&
(itv->first.getSatellite() == itv2->getSatellite()) )
{
isFound = true;
dataMapOk[*itv2] = dataMap[itv->first];
break;
}
}
}
if( !isFound ) validConstraint = false;
}
}
catch(...)
{
validConstraint = false;
}
if(validConstraint)
{
destList.push_back(Constraint(it->header,dataMapOk));
}
else
{
// we discard all constraints
return;
}
}
// Update the equation system
equationConstraints.setConstraintList(destList);
// Now, we can append the matrix(prefit design and weight)
try
{
Vector<double> meas;
Matrix<double> design;
Matrix<double> cov;
equationConstraints.constraintMatrix(varUnknowns,
meas, design, cov);
const int oldSize = measVector.size();
const int newSize = oldSize + meas.size();
const int colSize = hMatrix.cols();
Vector<double> tempPrefit(newSize,0.0);
Matrix<double> tempGeometry(newSize,colSize,0.0);
Matrix<double> tempWeight(newSize,newSize,0.0);
for(int i=0; i< newSize; i++)
{
// prefit
if(i<oldSize) tempPrefit(i) = measVector(i);
else tempPrefit(i) = meas(i-oldSize);
// geometry
for(int j=0;j<colSize;j++)
{
if(i<oldSize) tempGeometry(i,j) = hMatrix(i,j);
else tempGeometry(i,j) = design(i-oldSize,j);
}
// weight
if(i<oldSize) tempWeight(i,i) = rMatrix(i,i);
else tempWeight(i,i) = 1.0/cov(i-oldSize,i-oldSize);
}
// Update these matrix
measVector = tempPrefit;
hMatrix = tempGeometry;
rMatrix = tempWeight;
}
catch(...)
{
return;
}
} // End of method 'EquationSystem::imposeConstraints()'
// 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()'
