void grafo::CHRISTOFIDES(void) {
// Aplica o KRUSKAL para determinar a minima espassão em árvore
KRUSKAL();
// encontrar vertices com grau impar
vector<point> odd_degree;
for( num i=0; i<size(); i++) {
if( degre(i)%2==1) {
odd_degree.push_back(R[i]);
}
}
for( num k=0; k<size()-1; k++)
for( num t=k+1; t<size(); t++)
M.V[k][t]=M.V[t][k]=false;
// char out[]="output.dat";
// ofstream saida( out);
// for( num k=0; k<odd_degree.size(); k++)
// saida << odd_degree[k].real() << " " << odd_degree[k].imag() << endl;
// this->grafo(odd_degree);
// encontrar o "perfect matching" com peso mínimo dos vértices impares
// obs: usar o mesmo passo do KRUSKAL
// madj Aux(odd_degree);
vector<bool> visited(size());
for( num k=0; k<odd_degree.size()*(odd_degree.size()-1)/2; k++) {
double aux=INF;
vertex v=0, w=0;
for( num i=0; i<odd_degree.size()-1; i++) {
vertex p=enu(odd_degree[i]);
for( num j=i+1; j<odd_degree.size(); j++) {
vertex q=enu(odd_degree[j]);
if( connected(p,q)) break;
if(getw(p,q) < aux & !visited[q] & !visited[p]) {
v=p; w=q; aux=getw(q,p);
}
}
}
// cout << v << "<->" << w << endl;
if(aux==INF) break;
if( !visited[v] & !visited[w]) {
connect(v, w);
visited[v]=visited[w]=true;
}
}
// Selecionar um circuito de Euler
// Usar pesquisa usando DFS para encontrar um circuito de Euler, uma dúvida trivial é se pode
// existir mais de um circuito no grafo.
// for( num k=0; k<size(); k++) {
// }
}
//*****************************************************************************
// METHOD: ossimHgtRef::getSurfaceNormalCovMatrix()
//
// Form surface normal ECF covariance matrix from ENU surface covariance.
//
//*****************************************************************************
bool ossimHgtRef::
getSurfaceNormalCovMatrix
(const ossimGpt& pg,
const NEWMAT::Matrix& surfCov,
NEWMAT::Matrix& normCov) const
{
// Set the local frame
ossimLsrSpace enu(pg);
// Propagate the reference covariance matrix to ECF
NEWMAT::Matrix covEcf;
covEcf = enu.lsrToEcefRotMatrix() * surfCov * enu.lsrToEcefRotMatrix().t();
// Get surface normal vector in ENU
ossimColumnVector3d sn = getLocalTerrainNormal(pg);
NEWMAT::Matrix tnU(3,1);
tnU << sn[0] << sn[1] << sn[2];
// Rotate surface normal to ECF
NEWMAT::Matrix tnUecf(3,1);
tnUecf = enu.lsrToEcefRotMatrix() * tnU;
// Propagate to terrain normal
NEWMAT::Matrix ptn;
NEWMAT::SymmetricMatrix ptns;
ptn = tnUecf.t() * covEcf * tnUecf;
ptns << ptn;
// And back to ECF
normCov = tnUecf * ptns * tnUecf.t();
return true;
}
QVector3D Conversions::xyz2enu(const QVector3D &xyz, qreal reflat, qreal reflon, qreal refalt)
{
QVector3D refxyz = Conversions::lla2xyz(reflat,reflon,refalt);
QVector3D diffxyz = xyz - refxyz;
QTransform R1 = Conversions::rot(90.0 + reflon,3);
QTransform R2 = Conversions::rot(90.0-reflat,1);
QTransform R = R2*R1;
//MAKE THIS MATRIX MULTIPLY
qreal x = R.m11()*diffxyz.x() + R.m12()*diffxyz.y() + R.m13()*diffxyz.z();
qreal y = R.m21()*diffxyz.x() + R.m22()*diffxyz.y() + R.m23()*diffxyz.z();
qreal z = R.m31()*diffxyz.x() + R.m32()*diffxyz.y() + R.m33()*diffxyz.z();
QVector3D enu(x,y,z);
return enu;
}
/*!
* Actual function which does the conversion from ENU coords to LLA coords.
*
* @param ea: East coords.
* @param no: North coords.
* @param up: Up coords.
* @param lat: Latitude coords.
* @param lon: Longitude coords.
* @param alt: Altitude coords.
*
* @return true if conversion is possible else false.
*/
AREXPORT bool
ArMapGPSCoords::convertMap2LLACoords(const double ea, const double no, const double up,
double& lat, double& lon, double& alt) const
{
if(!myOriginSet)
return false;
ArENUCoords enu(ea, no, alt);
ArECEFCoords ecef = enu.ENU2ECEF(*myOriginLLA);
ArLLACoords lla = ecef.ECEF2LLA();
// ArLog::log(ArLog::Normal, "GPSLocaLog: convertMap2LLACoords: ENU %g %g %g",
// enu.getX(), enu.getY(), enu.getZ());
lat = lla.getX();//lla(0);
lon = lla.getY();//lla(1);
alt = lla.getZ();//lla(2);
// ArLog::log(ArLog::Normal, "GPSLocaLog: convertMap2LLACoords: %g %g %g",
// lat, lon, alt);
return true;
}
ReceiverOutput Calculate_Position::GetPosition(const long double elevation_mask, int weight_method)
{
ECEF_Frame position = ECEF_Frame(0.0L, 0.0L, 0.0L);
Matrix G;
Matrix dr;
Matrix W;
int j = 0;
long double r;
Matrix cov;
int number_of_satellites = 0;
std::vector<ECEF_Frame> satellites;
std::map<int, PsudoRange> psudodistance;
GPS_Time modifiedCurrent;
std::vector<long double> original_distance;
std::vector<long double> distance;
std::vector<long double> clockdiff;
std::vector<long double> sat_weight;
long double diff;
diff = calc_pos::max_diff;
long double receiver_clockdiff = 0.0L;
while(diff > calc_pos::min_diff)
{
int i = 0;
satellites.clear();
original_distance.clear();
clockdiff.clear();
distance.clear();
sat_weight.clear();
psudodistance.clear();
modifiedCurrent = GPS_Time(current.GetWeek(), current.GetSecond() - receiver_clockdiff / IS_GPS_200::C_velocity, current.GetLeapSecond());
std::map<int, PsudoRange>::iterator current_dist_it = originalpsudodistance.begin();
for (std::map <int, Ephemeris>::iterator it = ephemeris.begin(); it != ephemeris.end(); it++)
{
for (std::map<int, PsudoRange>::iterator dist_it = current_dist_it; dist_it != originalpsudodistance.end(); dist_it++)
{
if (dist_it->first == it->first)
{
long double r = dist_it->second.GetPsudoRange(type) - receiver_clockdiff;
long double satellite_clock = (it->second).GetClock(modifiedCurrent, r);
r = dist_it->second.GetPsudoRange(type) - receiver_clockdiff + satellite_clock * IS_GPS_200::C_velocity;
dist_it->second.SetCalculateRange(r);
satellite_clock = (it->second).GetClock(modifiedCurrent, r);
ECEF_Frame satellite_position = (it->second).GetPosition(modifiedCurrent, r);
//
if (weight_method == 1)
{
if (j < 1)
{
sat_weight.push_back(1.0L);
}
else
{
long double elevation = ENU_Frame(satellite_position, position).GetElevation();
long double sin_e = sin(elevation);
long double weight = sin_e * sin_e / calc_pos::VAR_ZENITH;
if (weight < calc_pos::MIN_WEIGHT)
{
weight = calc_pos::MIN_WEIGHT;
}
else
{
// Do nothing
}
}
}
else
{
if (j < 3 || (ENU_Frame(satellite_position, position).GetElevation() > elevation_mask))
{
sat_weight.push_back(1.0L);
}
else
{
continue;
}
}
original_distance.push_back(dist_it->second.GetPsudoRange(type));
satellites.push_back(satellite_position);
dist_it->second.SetSatellitePosition(satellite_position);
psudodistance.insert(std::pair<int, PsudoRange>(dist_it->first, dist_it->second));
clockdiff.push_back(satellite_clock);
distance.push_back(satellite_position.Distance(position));
current_dist_it = dist_it;
i++;
break;
}
else
{
continue;
}
}
}
number_of_satellites = i;
G.SetSize(number_of_satellites, 4);
dr.SetSize(number_of_satellites, 1);
W.SetSize(number_of_satellites, number_of_satellites);
for (int k = 0; k < number_of_satellites; k++)
{
for (int l = 0; l < number_of_satellites; l++)
{
W.SetData(0.0L, l, k);
}
}
// Create Observation matrix
for (i = 0; i < number_of_satellites; i++)
{
r = satellites[i].Distance(position);
ENU_Frame enu(satellites[i], position);
G.SetData(-enu.GetE()/r, i, 0);
G.SetData(-enu.GetN()/r, i, 1);
G.SetData(-enu.GetU()/r, i, 2);
G.SetData(1.0, i, 3);
dr.SetData(original_distance[i] + clockdiff[i] * IS_GPS_200::C_velocity - r - receiver_clockdiff, i, 0);
W.SetData(sat_weight[i], i, i);
if (j > 3)
{
long double atomospheric_delay = TropoSphere::GetTropoSphereCollection(satellites[i], position);
if (ionosphere.IsValid())
{
atomospheric_delay += ionosphere.GetIonoSphereDelayCorrection(satellites[i], position, current);
}
else
{
// Do nothing
}
dr.SetData(dr.GetData(i, 0) + atomospheric_delay, i, 0);
}
}
Matrix Gt = G.Tranposed();
Matrix Gtdr = Gt * dr;
Gaussian_Elimination gauss;
Matrix GtGd;
if (j == 1)
{
GtGd = Gt;
}
else
{
GtGd = Gt * W;
}
Matrix GtG = GtGd * G;
if (j == 1)
{
cov = gauss.GetAnswer(GtG, Gtdr);
}
else
{
gauss.GetAnswer(GtG, Gtdr);
}
position= ENU_Frame(Gtdr.GetData(0, 0), Gtdr.GetData(1, 0), Gtdr.GetData(2, 0), WGS84_Frame(position)).GetPosition();
receiver_clockdiff += Gtdr.GetData(3, 0);
diff = ECEF_Frame(Gtdr).Distance(ECEF_Frame(0, 0, 0));
if (j > calc_pos::max_loop)
{
break;
}
else
{
//Do nothing
}
j++;
}
ReceiverOutput ret = ReceiverOutput(modifiedCurrent, position, psudodistance, cov, type);
return ret;
}
bool rspfSensorModelTuple::computeSingleInterCov(
const rspf_int32& img,
const rspfDpt& obs,
const rspfGpt& ptG,
HeightRefType_t cRefType,
NEWMAT::Matrix& covMat)
{
NEWMAT::SymmetricMatrix BtWB(3);
NEWMAT::Matrix B(2,3);
NEWMAT::SymmetricMatrix W(2);
NEWMAT::Matrix surfCovENU(3,3);
rspfDpt resid;
bool tCovOK;
bool covOK;
rspfHgtRef hgtRef(cRefType);
if (PTR_CAST(rspfRpcModel, theImages[img]))
{
rspfRpcModel* model = PTR_CAST(rspfRpcModel, theImages[img]);
rspfGpt ptObs(obs.samp,obs.line);
theImages[img]->getForwardDeriv(OBS_INIT, ptObs);
resid = theImages[img]->getForwardDeriv(EVALUATE, ptG);
rspfDpt pWRTx = theImages[img]->getForwardDeriv(P_WRT_X, ptG);
rspfDpt pWRTy = theImages[img]->getForwardDeriv(P_WRT_Y, ptG);
rspfDpt pWRTz = theImages[img]->getForwardDeriv(P_WRT_Z, ptG);
rspfLsrSpace enu(ptG);
NEWMAT::Matrix jECF(3,2);
jECF(1,1) = pWRTx.u;
jECF(1,2) = pWRTx.v;
jECF(2,1) = pWRTy.u;
jECF(2,2) = pWRTy.v;
jECF(3,1) = pWRTz.u;
jECF(3,2) = pWRTz.v;
NEWMAT::Matrix jLSR(3,2);
jLSR = enu.ecefToLsrRotMatrix()*jECF;
rspf_float64 dU_dx = jLSR(1,1);
rspf_float64 dU_dy = jLSR(2,1);
rspf_float64 dU_dz = jLSR(3,1);
rspf_float64 dV_dx = jLSR(1,2);
rspf_float64 dV_dy = jLSR(2,2);
rspf_float64 dV_dz = jLSR(3,2);
rspf_float64 den = dU_dy*dV_dx - dV_dy*dU_dx;
rspf_float64 dY = dU_dx*dV_dz - dV_dx*dU_dz;
rspf_float64 dX = dV_dy*dU_dz - dU_dy*dV_dz;
rspf_float64 dy_dH = dY / den;
rspf_float64 dx_dH = dX / den;
rspf_float64 tAz = atan2(dx_dH, dy_dH);
rspf_float64 tEl = atan2(1.0, sqrt(dy_dH*dy_dH+dx_dH*dx_dH));
rspfColumnVector3d surfN = hgtRef.getLocalTerrainNormal(ptG);
rspf_float64 surfCE;
rspf_float64 surfLE;
if (theSurfAccRepresentsNoDEM)
{
surfN = surfN.zAligned();
rspfRpcModel::rpcModelStruct rpcPar;
model->getRpcParameters(rpcPar);
rspf_float64 hgtRng = rpcPar.hgtScale;
surfCE = 0.0;
rspf_float64 scaledHgtRng = abs(hgtRng/theSurfCE90);
rspf_float64 scaled1SigmaHgtRng = abs(scaledHgtRng/theSurfLE90);
surfLE = scaled1SigmaHgtRng*1.6449;
rspfNotify(rspfNotifyLevel_INFO)
<< "\n computeSingleInterCov() RPC NoDEM state selected..."
<< "\n RPC Height Scale = " << rpcPar.hgtScale <<" m"
<< "\n Scale Divisor = " <<abs(theSurfCE90)
<< "\n 1-Sigma Divisor = "<<abs(theSurfLE90)
<< std::endl;
}
else
{
surfCE = theSurfCE90;
surfLE = theSurfLE90;
}
tCovOK = hgtRef.getSurfaceCovMatrix(surfCE, surfLE, surfCovENU);
if (tCovOK)
{
theRpcPqeInputs.theRpcElevationAngle = tEl*DEG_PER_RAD;
theRpcPqeInputs.theRpcAzimuthAngle = tAz*DEG_PER_RAD;
theRpcPqeInputs.theRpcBiasError = model->getBiasError();
theRpcPqeInputs.theRpcRandError = model->getRandError();
theRpcPqeInputs.theSurfaceNormalVector = surfN;
theRpcPqeInputs.theSurfaceCovMatrix = surfCovENU;
rspfNotify(rspfNotifyLevel_INFO)
<< "\n RPC error prop parameters..."
<< "\n Elevation Angle = " << tEl*DEG_PER_RAD<< " deg"
<< "\n Azimuth Angle = " << tAz*DEG_PER_RAD<<" deg"
<< "\n RPC Bias Error = " <<model->getBiasError() <<" m"
<< "\n RPC Random Error = " <<model->getRandError()<<" m"
<< "\n surfN = " <<surfN
<< "\n surfCovENU = \n" <<surfCovENU
<< std::endl;
rspfEcefPoint pt(ptG);
rspfPositionQualityEvaluator qev
(pt,model->getBiasError(),model->getRandError(),
tEl,tAz,surfN,surfCovENU);
NEWMAT::Matrix covENU(3,3);
covOK = qev.getCovMatrix(covENU);
if (covOK)
{
covMat = enu.lsrToEcefRotMatrix()*covENU*enu.ecefToLsrRotMatrix();
}
}
else
{
covOK = false;
}
}
else
{
covOK = getGroundObsEqComponents(img, 0, obs, ptG, resid, B, W);
BtWB << B.t() * W * B;
NEWMAT::Matrix St(3,3);
if (theSurfAccRepresentsNoDEM)
{
rspfNotify(rspfNotifyLevel_INFO)
<< "\n computeSingleInterCov() RPC NoDEM state selected..."
<< " Not valid for this sensor model" << std::endl;
}
if (hgtRef.getSurfaceCovMatrix(theSurfCE90, theSurfLE90, surfCovENU))
{
tCovOK = hgtRef.getSurfaceNormalCovMatrix(ptG, surfCovENU, St);
}
else
{
tCovOK = false;
}
if (tCovOK)
{
NEWMAT::Matrix Sti = invert(St);
covMat = invert(BtWB + Sti);
}
else
{
covMat = invert(BtWB);
}
}
return covOK;
}
