mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-15 12:40:35 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez 2018-11-09 12:25:11 +01:00
commit 82d0f305b9
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
5 changed files with 196 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/algorithms/PVT/libs/gpx_printer.cc
View File

@ -138,13 +138,15 @@ bool Gpx_Printer::set_headers(std::string filename, bool time_tag_name)
gpx_file << std::setprecision(14); gpx_file << std::setprecision(14);
gpx_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl gpx_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
<< "<gpx version=\"1.1\" creator=\"GNSS-SDR\"" << std::endl << "<gpx version=\"1.1\" creator=\"GNSS-SDR\"" << std::endl
<< "xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd\"" << std::endl << indent << "xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd http://www.garmin.com/xmlschemas/GpxExtensions/v3 http://www.garmin.com/xmlschemas/GpxExtensionsv3.xsd http://www.garmin.com/xmlschemas/TrackPointExtension/v2 http://www.garmin.com/xmlschemas/TrackPointExtensionv2.xsd\"" << std::endl
<< "xmlns=\"http://www.topografix.com/GPX/1/1\"" << std::endl << indent << "xmlns=\"http://www.topografix.com/GPX/1/1\"" << std::endl
<< "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">" << std::endl << indent << "xmlns:gpxx=\"http://www.garmin.com/xmlschemas/GpxExtensions/v3\"" << std::endl
<< "<trk>" << std::endl << indent << "xmlns:gpxtpx=\"http://www.garmin.com/xmlschemas/TrackPointExtension/v2\"" << std::endl
<< indent << "<name>Position fixes computed by GNSS-SDR v" << GNSS_SDR_VERSION << "</name>" << std::endl << indent << "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">" << std::endl
<< indent << "<desc>GNSS-SDR position log generated at " << pt << " (local time)</desc>" << std::endl << indent << "<trk>" << std::endl
<< indent << "<trkseg>" << std::endl; << indent << indent << "<name>Position fixes computed by GNSS-SDR v" << GNSS_SDR_VERSION << "</name>" << std::endl
<< indent << indent << "<desc>GNSS-SDR position log generated at " << pt << " (local time)</desc>" << std::endl
<< indent << indent << "<trkseg>" << std::endl;
return true; return true;
} }
else else
@ -164,6 +166,9 @@ bool Gpx_Printer::print_position(const std::shared_ptr<rtklib_solver>& position,
positions_printed = true; positions_printed = true;
std::shared_ptr<rtklib_solver> position_ = position; std::shared_ptr<rtklib_solver> position_ = position;
double speed_over_ground = position_->get_speed_over_ground(); // expressed in m/s
double course_over_ground = position_->get_course_over_ground(); // expressed in deg
double hdop = position_->get_hdop(); double hdop = position_->get_hdop();
double vdop = position_->get_vdop(); double vdop = position_->get_vdop();
double pdop = position_->get_pdop(); double pdop = position_->get_pdop();
@ -187,9 +192,13 @@ bool Gpx_Printer::print_position(const std::shared_ptr<rtklib_solver>& position,
if (gpx_file.is_open()) if (gpx_file.is_open())
{ {
gpx_file << indent << indent << "<trkpt lon=\"" << longitude << "\" lat=\"" << latitude << "\"><ele>" << height << "</ele>" gpx_file << indent << indent << indent << "<trkpt lon=\"" << longitude << "\" lat=\"" << latitude << "\"><ele>" << height << "</ele>"
<< "<time>" << utc_time << "</time>" << "<time>" << utc_time << "</time>"
<< "<hdop>" << hdop << "</hdop><vdop>" << vdop << "</vdop><pdop>" << pdop << "</pdop></trkpt>" << std::endl; << "<hdop>" << hdop << "</hdop><vdop>" << vdop << "</vdop><pdop>" << pdop << "</pdop>"
<< "<extensions><gpxtpx:TrackPointExtension>"
<< "<gpxtpx:speed>" << speed_over_ground << "</gpxtpx:speed>"
<< "<gpxtpx:course>" << course_over_ground << "</gpxtpx:course>"
<< "</gpxtpx:TrackPointExtension></extensions></trkpt>" << std::endl;
return true; return true;
} }
else else
@ -203,8 +212,8 @@ bool Gpx_Printer::close_file()
{ {
if (gpx_file.is_open()) if (gpx_file.is_open())
{ {
gpx_file << indent << "</trkseg>" << std::endl gpx_file << indent << indent << "</trkseg>" << std::endl
<< "</trk>" << std::endl << indent << "</trk>" << std::endl
<< "</gpx>"; << "</gpx>";
gpx_file.close(); gpx_file.close();
return true; return true;

171
src/algorithms/PVT/libs/kml_printer.cc
View File

@ -2,6 +2,7 @@
* \file kml_printer.cc * \file kml_printer.cc
* \brief Implementation of a class that prints PVT information to a kml file * \brief Implementation of a class that prints PVT information to a kml file
* \author Javier Arribas, 2011. jarribas(at)cttc.es * \author Javier Arribas, 2011. jarribas(at)cttc.es
* Álvaro Cebrián Juan, 2018. acebrianjuan(at)gmail.com
* *
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
@ -43,6 +44,7 @@ using google::LogMessage;
Kml_Printer::Kml_Printer(const std::string& base_path) Kml_Printer::Kml_Printer(const std::string& base_path)
{ {
positions_printed = false; positions_printed = false;
indent = " ";
kml_base_path = base_path; kml_base_path = base_path;
boost::filesystem::path full_path(boost::filesystem::current_path()); boost::filesystem::path full_path(boost::filesystem::current_path());
const boost::filesystem::path p(kml_base_path); const boost::filesystem::path p(kml_base_path);
@ -74,6 +76,14 @@ Kml_Printer::Kml_Printer(const std::string& base_path)
} }
kml_base_path = kml_base_path + boost::filesystem::path::preferred_separator; kml_base_path = kml_base_path + boost::filesystem::path::preferred_separator;
boost::filesystem::path tmp_base_path = boost::filesystem::temp_directory_path();
boost::filesystem::path tmp_filename = boost::filesystem::unique_path();
boost::filesystem::path tmp_file = tmp_base_path / tmp_filename;
tmp_file_str = tmp_file.string();
point_id = 0;
} }
@ -127,36 +137,73 @@ bool Kml_Printer::set_headers(std::string filename, bool time_tag_name)
kml_filename = kml_base_path + kml_filename; kml_filename = kml_base_path + kml_filename;
kml_file.open(kml_filename.c_str()); kml_file.open(kml_filename.c_str());
if (kml_file.is_open()) tmp_file.open(tmp_file_str.c_str());
if (kml_file.is_open() && tmp_file.is_open())
{ {
DLOG(INFO) << "KML printer writing on " << filename.c_str(); DLOG(INFO) << "KML printer writing on " << filename.c_str();
// Set iostream numeric format and precision // Set iostream numeric format and precision
kml_file.setf(kml_file.fixed, kml_file.floatfield); kml_file.setf(kml_file.fixed, kml_file.floatfield);
kml_file << std::setprecision(14); kml_file << std::setprecision(14);
tmp_file.setf(tmp_file.fixed, tmp_file.floatfield);
tmp_file << std::setprecision(14);
kml_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl kml_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
<< "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl << "<kml xmlns=\"http://www.opengis.net/kml/2.2\" xmlns:gx=\"http://www.google.com/kml/ext/2.2\">" << std::endl
<< " <Document>" << std::endl << indent << "<Document>" << std::endl
<< " <name>GNSS Track</name>" << std::endl << indent << indent << "<name>GNSS Track</name>" << std::endl
<< " <description>GNSS-SDR Receiver position log file created at " << pt << indent << indent << "<description><![CDATA[" << std::endl
<< " </description>" << std::endl << indent << indent << indent << "<table>" << std::endl
<< "<Style id=\"yellowLineGreenPoly\">" << std::endl << indent << indent << indent << indent << "<tr><td>GNSS-SDR Receiver position log file created at " << pt << "</td></tr>" << std::endl
<< " <LineStyle>" << std::endl << indent << indent << indent << indent << "<tr><td>https://gnss-sdr.org/</td></tr>" << std::endl
<< " <color>7f00ffff</color>" << std::endl << indent << indent << indent << "</table>" << std::endl
<< " <width>1</width>" << std::endl << indent << indent << "]]></description>" << std::endl
<< " </LineStyle>" << std::endl << indent << indent << "<!-- Normal track style -->" << std::endl
<< "<PolyStyle>" << std::endl << indent << indent << "<Style id=\"track_n\">" << std::endl
<< " <color>7f00ff00</color>" << std::endl << indent << indent << indent << "<IconStyle>" << std::endl
<< "</PolyStyle>" << std::endl << indent << indent << indent << indent << "<color>ff00ffff</color>" << std::endl
<< "</Style>" << std::endl << indent << indent << indent << indent << "<scale>0.3</scale>" << std::endl
<< "<Placemark>" << std::endl << indent << indent << indent << indent << "<Icon>" << std::endl
<< "<name>GNSS-SDR PVT</name>" << std::endl << indent << indent << indent << indent << indent << "<href>http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png</href>" << std::endl
<< "<description>GNSS-SDR position log</description>" << std::endl << indent << indent << indent << indent << "</Icon>" << std::endl
<< "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl << indent << indent << indent << "</IconStyle>" << std::endl
<< "<LineString>" << std::endl << indent << indent << indent << "<LabelStyle>" << std::endl
<< "<extrude>0</extrude>" << std::endl << indent << indent << indent << indent << "<scale>0</scale>" << std::endl
<< "<tessellate>1</tessellate>" << std::endl << indent << indent << indent << "</LabelStyle>" << std::endl
<< "<altitudeMode>absolute</altitudeMode>" << std::endl << indent << indent << "</Style>" << std::endl
<< "<coordinates>" << std::endl; << indent << indent << "<!-- Highlighted track style -->" << std::endl
<< indent << indent << "<Style id=\"track_h\">" << std::endl
<< indent << indent << indent << "<IconStyle>" << std::endl
<< indent << indent << indent << indent << "<color>ff00ffff</color>" << std::endl
<< indent << indent << indent << indent << "<scale>1</scale>" << std::endl
<< indent << indent << indent << indent << "<Icon>" << std::endl
<< indent << indent << indent << indent << indent << "<href>http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png</href>" << std::endl
<< indent << indent << indent << indent << "</Icon>" << std::endl
<< indent << indent << indent << "</IconStyle>" << std::endl
<< indent << indent << "</Style>" << std::endl
<< indent << indent << "<StyleMap id=\"track\">" << std::endl
<< indent << indent << indent << "<Pair>" << std::endl
<< indent << indent << indent << indent << "<key>normal</key>" << std::endl
<< indent << indent << indent << indent << "<styleUrl>#track_n</styleUrl>" << std::endl
<< indent << indent << indent << "</Pair>" << std::endl
<< indent << indent << indent << "<Pair>" << std::endl
<< indent << indent << indent << indent << "<key>highlight</key>" << std::endl
<< indent << indent << indent << indent << "<styleUrl>#track_h</styleUrl>" << std::endl
<< indent << indent << indent << "</Pair>" << std::endl
<< indent << indent << "</StyleMap>" << std::endl
<< indent << indent << "<Style id=\"yellowLineGreenPoly\">" << std::endl
<< indent << indent << indent << "<LineStyle>" << std::endl
<< indent << indent << indent << indent << "<color>7f00ffff</color>" << std::endl
<< indent << indent << indent << indent << "<width>1</width>" << std::endl
<< indent << indent << indent << "</LineStyle>" << std::endl
<< indent << indent << indent << "<PolyStyle>" << std::endl
<< indent << indent << indent << indent << "<color>7f00ff00</color>" << std::endl
<< indent << indent << indent << "</PolyStyle>" << std::endl
<< indent << indent << "</Style>" << std::endl
<< indent << indent << "<Folder>" << std::endl
<< indent << indent << indent << "<name>Points</name>" << std::endl;
return true; return true;
} }
else else
@ -167,7 +214,7 @@ bool Kml_Printer::set_headers(std::string filename, bool time_tag_name)
} }
bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values) bool Kml_Printer::print_position(const std::shared_ptr<rtklib_solver>& position, bool print_average_values)
{ {
double latitude; double latitude;
double longitude; double longitude;
@ -175,7 +222,18 @@ bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position,
positions_printed = true; positions_printed = true;
std::shared_ptr<Pvt_Solution> position_ = position; std::shared_ptr<rtklib_solver> position_ = position;
double speed_over_ground = position_->get_speed_over_ground(); // expressed in m/s
double course_over_ground = position_->get_course_over_ground(); // expressed in deg
double hdop = position_->get_hdop();
double vdop = position_->get_vdop();
double pdop = position_->get_pdop();
std::string utc_time = to_iso_extended_string(position_->get_position_UTC_time());
if (utc_time.length() < 23) utc_time += ".";
utc_time.resize(23, '0'); // time up to ms
utc_time.append("Z"); // UTC time zone
if (print_average_values == false) if (print_average_values == false)
{ {
@ -190,9 +248,38 @@ bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position,
height = position_->get_avg_height(); height = position_->get_avg_height();
} }
if (kml_file.is_open()) if (kml_file.is_open() && tmp_file.is_open())
{ {
kml_file << longitude << "," << latitude << "," << height << std::endl; point_id++;
kml_file << indent << indent << indent << "<Placemark>" << std::endl
<< indent << indent << indent << indent << "<name>" << point_id << "</name>" << std::endl
<< indent << indent << indent << indent << "<snippet/>" << std::endl
<< indent << indent << indent << indent << "<description><![CDATA[" << std::endl
<< indent << indent << indent << indent << indent << "<table>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Time:</td><td>" << utc_time << "</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Longitude:</td><td>" << longitude << "</td><td>deg</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Latitude:</td><td>" << latitude << "</td><td>deg</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Altitude:</td><td>" << height << "</td><td>m</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Speed:</td><td>" << speed_over_ground << "</td><td>m/s</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Course:</td><td>" << course_over_ground << "</td><td>deg</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>HDOP:</td><td>" << hdop << "</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>VDOP:</td><td>" << vdop << "</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>PDOP:</td><td>" << pdop << "</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << "</table>" << std::endl
<< indent << indent << indent << indent << "]]></description>" << std::endl
<< indent << indent << indent << indent << "<TimeStamp>" << std::endl
<< indent << indent << indent << indent << indent << "<when>" << utc_time << "</when>" << std::endl
<< indent << indent << indent << indent << "</TimeStamp>" << std::endl
<< indent << indent << indent << indent << "<styleUrl>#track</styleUrl>" << std::endl
<< indent << indent << indent << indent << "<Point>" << std::endl
<< indent << indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>" << std::endl
<< indent << indent << indent << indent << indent << "<coordinates>" << longitude << "," << latitude << "," << height << "</coordinates>" << std::endl
<< indent << indent << indent << indent << "</Point>" << std::endl
<< indent << indent << indent << "</Placemark>" << std::endl;
tmp_file << indent << indent << indent << indent << indent
<< longitude << "," << latitude << "," << height << std::endl;
return true; return true;
} }
else else
@ -204,14 +291,32 @@ bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position,
bool Kml_Printer::close_file() bool Kml_Printer::close_file()
{ {
if (kml_file.is_open()) if (kml_file.is_open() && tmp_file.is_open())
{ {
kml_file << "</coordinates>" << std::endl tmp_file.close();
<< "</LineString>" << std::endl
<< "</Placemark>" << std::endl kml_file << indent << indent << "</Folder>"
<< "</Document>" << std::endl << indent << indent << "<Placemark>" << std::endl
<< indent << indent << indent << "<name>Path</name>" << std::endl
<< indent << indent << indent << "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl
<< indent << indent << indent << "<LineString>" << std::endl
<< indent << indent << indent << indent << "<extrude>0</extrude>" << std::endl
<< indent << indent << indent << indent << "<tessellate>1</tessellate>" << std::endl
<< indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>" << std::endl
<< indent << indent << indent << indent << "<coordinates>" << std::endl;
// Copy the contents of tmp_file into kml_file
std::ifstream src(tmp_file_str, std::ios::binary);
kml_file << src.rdbuf();
kml_file << indent << indent << indent << indent << "</coordinates>" << std::endl
<< indent << indent << indent << "</LineString>" << std::endl
<< indent << indent << "</Placemark>" << std::endl
<< indent << "</Document>" << std::endl
<< "</kml>"; << "</kml>";
kml_file.close(); kml_file.close();
return true; return true;
} }
else else

9
src/algorithms/PVT/libs/kml_printer.h
View File

@ -2,7 +2,7 @@
* \file kml_printer.h * \file kml_printer.h
* \brief Interface of a class that prints PVT information to a kml file * \brief Interface of a class that prints PVT information to a kml file
* \author Javier Arribas, 2011. jarribas(at)cttc.es * \author Javier Arribas, 2011. jarribas(at)cttc.es
* * Álvaro Cebrián Juan, 2018. acebrianjuan(at)gmail.com
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
@ -34,6 +34,7 @@
#define GNSS_SDR_KML_PRINTER_H_ #define GNSS_SDR_KML_PRINTER_H_
#include "pvt_solution.h" #include "pvt_solution.h"
#include "rtklib_solver.h"
#include <fstream> #include <fstream>
#include <memory> #include <memory>
#include <string> #include <string>
@ -48,15 +49,19 @@ class Kml_Printer
{ {
private: private:
std::ofstream kml_file; std::ofstream kml_file;
std::ofstream tmp_file;
bool positions_printed; bool positions_printed;
std::string kml_filename; std::string kml_filename;
std::string kml_base_path; std::string kml_base_path;
std::string tmp_file_str;
unsigned int point_id;
std::string indent;
public: public:
Kml_Printer(const std::string& base_path = std::string(".")); Kml_Printer(const std::string& base_path = std::string("."));
~Kml_Printer(); ~Kml_Printer();
bool set_headers(std::string filename, bool time_tag_name = true); bool set_headers(std::string filename, bool time_tag_name = true);
bool print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values); bool print_position(const std::shared_ptr<rtklib_solver>& position, bool print_average_values);
bool close_file(); bool close_file();
}; };

1
src/tests/system-tests/libs/position_test_flags.h
View File

@ -37,7 +37,6 @@
DEFINE_string(config_file_ptest, std::string(""), "File containing the configuration parameters for the position test."); DEFINE_string(config_file_ptest, std::string(""), "File containing the configuration parameters for the position test.");
DEFINE_bool(plot_position_test, false, "Plots results of with gnuplot"); DEFINE_bool(plot_position_test, false, "Plots results of with gnuplot");
DEFINE_bool(static_scenario, true, "Compute figures of merit for static user position (DRMS, CEP, etc..)"); DEFINE_bool(static_scenario, true, "Compute figures of merit for static user position (DRMS, CEP, etc..)");
DEFINE_bool(use_pvt_solver_dump, false, "Use PVT solver binary dump or fall back to KML PVT file (contains only position information)");
DEFINE_bool(use_ref_motion_file, false, "Enable or disable the use of a reference file containing the true receiver position, velocity and acceleration."); DEFINE_bool(use_ref_motion_file, false, "Enable or disable the use of a reference file containing the true receiver position, velocity and acceleration.");
DEFINE_int32(ref_motion_file_type, 1, "Type of reference motion file: 1- Spirent CSV motion file"); DEFINE_int32(ref_motion_file_type, 1, "Type of reference motion file: 1- Spirent CSV motion file");
DEFINE_string(ref_motion_filename, std::string("motion.csv"), "Path and filename for the reference motion file"); DEFINE_string(ref_motion_filename, std::string("motion.csv"), "Path and filename for the reference motion file");

127
src/tests/system-tests/position_test.cc
View File

@ -455,107 +455,42 @@ void PositionSystemTest::check_results()
ref_R_eb_e.insert_cols(0, true_r_eb_e); ref_R_eb_e.insert_cols(0, true_r_eb_e);
arma::vec ref_r_enu = {0, 0, 0}; arma::vec ref_r_enu = {0, 0, 0};
cart2utm(true_r_eb_e, utm_zone, ref_r_enu); cart2utm(true_r_eb_e, utm_zone, ref_r_enu);
if (!FLAGS_use_pvt_solver_dump)
rtklib_solver_dump_reader pvt_reader;
pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename);
int64_t n_epochs = pvt_reader.num_epochs();
R_eb_e = arma::zeros(3, n_epochs);
V_eb_e = arma::zeros(3, n_epochs);
LLH = arma::zeros(3, n_epochs);
receiver_time_s = arma::zeros(n_epochs, 1);
int64_t current_epoch = 0;
while (pvt_reader.read_binary_obs())
{ {
//fall back to read receiver KML output (position only) receiver_time_s(current_epoch) = pvt_reader.RX_time - pvt_reader.clk_offset_s;
std::fstream myfile(PositionSystemTest::generated_kml_file, std::ios_base::in); R_eb_e(0, current_epoch) = pvt_reader.rr[0];
ASSERT_TRUE(myfile.is_open()) << "No valid kml file could be opened"; R_eb_e(1, current_epoch) = pvt_reader.rr[1];
std::string line; R_eb_e(2, current_epoch) = pvt_reader.rr[2];
// Skip header V_eb_e(0, current_epoch) = pvt_reader.rr[3];
std::getline(myfile, line); V_eb_e(1, current_epoch) = pvt_reader.rr[4];
bool is_header = true; V_eb_e(2, current_epoch) = pvt_reader.rr[5];
while (is_header) LLH(0, current_epoch) = pvt_reader.latitude;
{ LLH(1, current_epoch) = pvt_reader.longitude;
std::getline(myfile, line); LLH(2, current_epoch) = pvt_reader.height;
ASSERT_FALSE(myfile.eof()) << "No valid kml file found.";
std::size_t found = line.find("<coordinates>");
if (found != std::string::npos) is_header = false;
}
bool is_data = true;
//read data
int64_t current_epoch = 0;
while (is_data)
{
if (!std::getline(myfile, line))
{
is_data = false;
break;
}
std::size_t found = line.find("</coordinates>");
if (found != std::string::npos)
is_data = false;
else
{
std::string str2;
std::istringstream iss(line);
double value;
double lat = 0.0;
double longitude = 0.0;
double h = 0.0;
for (int i = 0; i < 3; i++)
{
std::getline(iss, str2, ',');
value = std::stod(str2);
if (i == 0) longitude = value;
if (i == 1) lat = value;
if (i == 2) h = value;
}
arma::vec tmp_v_ecef; arma::vec tmp_r_enu = {0, 0, 0};
arma::vec tmp_r_ecef; cart2utm(R_eb_e.col(current_epoch), utm_zone, tmp_r_enu);
pv_Geo_to_ECEF(degtorad(lat), degtorad(longitude), h, arma::vec{0, 0, 0}, tmp_r_ecef, tmp_v_ecef); R_eb_enu.insert_cols(current_epoch, tmp_r_enu);
R_eb_e.insert_cols(current_epoch, tmp_r_ecef);
arma::vec tmp_r_enu = {0, 0, 0}; // debug check
cart2utm(tmp_r_ecef, utm_zone, tmp_r_enu); // std::cout << "t1: " << pvt_reader.RX_time << std::endl;
R_eb_enu.insert_cols(current_epoch, tmp_r_enu); // std::cout << "t2: " << pvt_reader.TOW_at_current_symbol_ms << std::endl;
// std::cout << "lat = " << lat << ", longitude = " << longitude << " h = " << h << std::endl; // std::cout << "offset: " << pvt_reader.clk_offset_s << std::endl;
// std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl; // getchar();
// getchar(); current_epoch++;
}
}
myfile.close();
ASSERT_FALSE(R_eb_e.n_cols == 0) << "KML file is empty";
}
else
{
//use complete binary dump from pvt solver
rtklib_solver_dump_reader pvt_reader;
pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename);
int64_t n_epochs = pvt_reader.num_epochs();
R_eb_e = arma::zeros(3, n_epochs);
V_eb_e = arma::zeros(3, n_epochs);
LLH = arma::zeros(3, n_epochs);
receiver_time_s = arma::zeros(n_epochs, 1);
int64_t current_epoch = 0;
while (pvt_reader.read_binary_obs())
{
receiver_time_s(current_epoch) = pvt_reader.RX_time - pvt_reader.clk_offset_s;
R_eb_e(0, current_epoch) = pvt_reader.rr[0];
R_eb_e(1, current_epoch) = pvt_reader.rr[1];
R_eb_e(2, current_epoch) = pvt_reader.rr[2];
V_eb_e(0, current_epoch) = pvt_reader.rr[3];
V_eb_e(1, current_epoch) = pvt_reader.rr[4];
V_eb_e(2, current_epoch) = pvt_reader.rr[5];
LLH(0, current_epoch) = pvt_reader.latitude;
LLH(1, current_epoch) = pvt_reader.longitude;
LLH(2, current_epoch) = pvt_reader.height;
arma::vec tmp_r_enu = {0, 0, 0};
cart2utm(R_eb_e.col(current_epoch), utm_zone, tmp_r_enu);
R_eb_enu.insert_cols(current_epoch, tmp_r_enu);
//debug check
// std::cout << "t1: " << pvt_reader.RX_time << std::endl;
// std::cout << "t2: " << pvt_reader.TOW_at_current_symbol_ms << std::endl;
// std::cout << "offset: " << pvt_reader.clk_offset_s << std::endl;
// getchar();
current_epoch++;
}
ASSERT_FALSE(current_epoch == 0) << "PVT dump is empty";
} }
ASSERT_FALSE(current_epoch == 0) << "PVT dump is empty";
// compute results // compute results
if (FLAGS_static_scenario) if (FLAGS_static_scenario)
{ {
double sigma_E_2_precision = arma::var(R_eb_enu.row(0)); double sigma_E_2_precision = arma::var(R_eb_enu.row(0));