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