From f221ca569ca912f4b54de695e074f5dac87f7412 Mon Sep 17 00:00:00 2001 From: xyp8023 Date: Tue, 31 Mar 2020 12:54:39 +0200 Subject: [PATCH 1/3] Added small things for jsf_data library 1. Added sample_interval for jsf_sss_ping 2. Added python interface for jsf_dvl_ping --- src/data_tools/include/data_tools/jsf_data.h | 3 ++- src/data_tools/src/jsf_data.cpp | 13 ++++++++--- src/pydata_tools/src/pyjsf_data.cpp | 24 ++++++++++++++++++++ 3 files changed, 36 insertions(+), 4 deletions(-) diff --git a/src/data_tools/include/data_tools/jsf_data.h b/src/data_tools/include/data_tools/jsf_data.h index 6a51439e..6b8a2a56 100644 --- a/src/data_tools/include/data_tools/jsf_data.h +++ b/src/data_tools/include/data_tools/jsf_data.h @@ -57,11 +57,12 @@ struct jsf_sss_ping double sound_vel; uint16_t frequency; Eigen::Vector3d pos_; + double sample_interval; template void serialize( Archive & ar ) { - ar(CEREAL_NVP(time_string_), CEREAL_NVP(time_stamp_), CEREAL_NVP(port), CEREAL_NVP(stbd), CEREAL_NVP(first_in_file_), CEREAL_NVP(rpy), CEREAL_NVP(lat_), CEREAL_NVP(long_), CEREAL_NVP(utm_zone), CEREAL_NVP(sound_vel), CEREAL_NVP(frequency), CEREAL_NVP(pos_)); + ar(CEREAL_NVP(time_string_), CEREAL_NVP(time_stamp_), CEREAL_NVP(port), CEREAL_NVP(stbd), CEREAL_NVP(first_in_file_), CEREAL_NVP(rpy), CEREAL_NVP(lat_), CEREAL_NVP(long_), CEREAL_NVP(utm_zone), CEREAL_NVP(sound_vel), CEREAL_NVP(frequency), CEREAL_NVP(pos_), CEREAL_NVP(sample_interval)); } EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/src/data_tools/src/jsf_data.cpp b/src/data_tools/src/jsf_data.cpp index 7207450c..44ce4d2c 100644 --- a/src/data_tools/src/jsf_data.cpp +++ b/src/data_tools/src/jsf_data.cpp @@ -154,6 +154,8 @@ vector > parse_file_impl(const } while (!input.eof()) { jsf_msg_header jsf_hdr; + int16_t prot_ver = jsf_hdr.prot_ver; + // cout << "in while Protocol Version: " << prot_ver << endl; input.read(reinterpret_cast(&jsf_hdr),sizeof(jsf_hdr)); if (jsf_hdr.start_marker != SONAR_MESSAGE_HEADER_START) { cout << "Invalid file format! start marker: " << jsf_hdr.start_marker << endl; @@ -189,7 +191,11 @@ jsf_sss_ping read_datagram(std::ifstrea ping.rpy = Eigen::Vector3d(jsf_sonar_data_hdr.roll, jsf_sonar_data_hdr.pitch, jsf_sonar_data_hdr.compass_heading); ping.rpy.head<2>() = M_PI/32768.*ping.rpy.head<2>(); ping.rpy[2] = .5*M_PI - M_PI/180.*0.01*ping.rpy[2]; - + double interval = jsf_sonar_data_hdr.spl_intvl_in_ns*0.000000001; + ping.sample_interval = interval; + // cout << "samples interval in s: " << ping.sample_interval << endl; + // cout << "Num of samples in this pocket: "<< jsf_sonar_data_hdr.spls_num_in_pkt << endl; + if (ping.sound_vel>0) cout << "Sound velocity: " << ping.sound_vel << endl; // NOTE: this is only valid if coord_units == 2 ping.lat_ = 0.0001/60.*double(jsf_sonar_data_hdr.y_coord); ping.long_ = 0.0001/60.*double(jsf_sonar_data_hdr.x_coord); @@ -200,13 +206,14 @@ jsf_sss_ping read_datagram(std::ifstrea ping.utm_zone = utm_zone; ping.pos_ = Eigen::Vector3d(easting, northing, -0.001*jsf_sonar_data_hdr.depth_in_mm); ping_side = process_side_scan_ping_side(input, jsf_hdr, jsf_sonar_data_hdr); - + // cout << "Protocol Version: " << std::hex << jsf_hdr.prot_ver << endl; //cout << "Coord units: " << jsf_sonar_data_hdr.coord_units << endl; - + // cout << "Starting depth: " << jsf_sonar_data_hdr.starting_depth << "depth in mm: " << jsf_sonar_data_hdr.depth_in_mm << endl; const boost::posix_time::ptime epoch = boost::posix_time::time_from_string("1970-01-01 00:00:00.000"); boost::posix_time::ptime data_time; if (jsf_hdr.prot_ver >= 8) { + data_time = epoch + boost::posix_time::seconds(jsf_sonar_data_hdr.ping_time_in_sec) + boost::posix_time::milliseconds(jsf_sonar_data_hdr.today_in_ms%1000); } else { diff --git a/src/pydata_tools/src/pyjsf_data.cpp b/src/pydata_tools/src/pyjsf_data.cpp index bddd39e6..1268c311 100644 --- a/src/pydata_tools/src/pyjsf_data.cpp +++ b/src/pydata_tools/src/pyjsf_data.cpp @@ -43,11 +43,35 @@ PYBIND11_MODULE(jsf_data, m) { .def_readwrite("sound_vel_", &jsf_sss_ping::sound_vel, "Sound speed in m/s") .def_readwrite("frequency", &jsf_sss_ping::frequency, "Frequency of sampling") .def_readwrite("pos_", &jsf_sss_ping::pos_, "Position in ENU coordinates") + .def_readwrite("sample_interval_", &jsf_sss_ping::sample_interval, "Sample interval in s") .def_static("parse_file", &parse_file_from_str, "Parse jsf_sss_ping from .jsf file") .def_static("parse_folder", &parse_folder_from_str, "Parse jsf_sss_ping from folder of .jsf files") .def_static("read_data", &read_data_from_str, "Read jsf_sss_ping::PingsT from .cereal file"); + py::class_(m, "jsf_dvl_ping", "Class for jsf dvl type") + .def(py::init<>()) + .def_readwrite("time_string_", &jsf_dvl_ping::time_string_, "Readable date of measurement") + .def_readwrite("time_stamp_", &jsf_dvl_ping::time_stamp_, "UNIX timestamp") + .def_readwrite("dist_to_bottom_", &jsf_dvl_ping::dist_to_bottom_, "Disctance to bottom in meter for up to 4 beams") + .def_readwrite("vel_wrt_bottom_", &jsf_dvl_ping::vel_wrt_bottom_, "Velocity with respect to the bottom in m/s") + .def_readwrite("vel_wrt_water_", &jsf_dvl_ping::vel_wrt_water_, "Velocity with respect to the water layer in m/s") + .def_readwrite("first_in_file_", &jsf_dvl_ping::first_in_file_, "Is first measurement in file?") + .def_readwrite("ship_coord_", &jsf_dvl_ping::ship_coord_, "Is velocity in ship coordinates?") + .def_readwrite("depth_", &jsf_dvl_ping::depth_, "Depth from depth sensor in meters") + .def_readwrite("pitch_", &jsf_dvl_ping::pitch_, "Radian pitch in ENU coordinates") + .def_readwrite("roll_", &jsf_dvl_ping::roll_, "Radian roll in ENU coordinatese") + .def_readwrite("heading_", &jsf_dvl_ping::heading_, "Radian heading in ENU coordinates") + .def_readwrite("sound_vel_", &jsf_dvl_ping::sound_vel_, "Sound speed in m/s") + .def_readwrite("temp_", &jsf_dvl_ping::temp_, "Temperature in degree Celsius") + .def_readwrite("salinity_", &jsf_dvl_ping::salinity_, "Salinity in 1 part per thousand") + .def_readwrite("flag_", &jsf_dvl_ping::flag_, "Flag indicates which values present") + .def_readwrite("error_", &jsf_dvl_ping::error_, "Is error detected?") + .def_static("parse_file", &parse_file_from_str, "Parse jsf_dvl_ping from .jsf file") + .def_static("parse_folder", &parse_folder_from_str, "Parse jsf_dvl_ping from folder of .jsf files") + .def_static("read_data", &read_data_from_str, "Read jsf_dvl_ping::PingsT from .cereal file"); + m.def("write_data", &write_data_from_str, "Write jsf pings to .cereal file"); + m.def("write_data", &write_data_from_str, "Write dvl pings to .cereal file"); m.def("make_waterfall_image", &make_waterfall_image, "Create a cv2 waterfall image from jsf_sss_ping::PingsT"); m.def("show_waterfall_image", &show_waterfall_image, "Show a waterfall image created from jsf_sss_ping::PingsT"); m.def("filter_frequency", &filter_frequency, "Filter to keep only jsf_sss_ping::PingsT with certain frequency"); From 038ad962d1e5235691fe9957cd986809fc37eb2f Mon Sep 17 00:00:00 2001 From: xyp8023 Date: Wed, 1 Apr 2020 17:42:59 +0200 Subject: [PATCH 2/3] Added match_sound_vel for jsf_data 1. Added altitude, validity flag, sample interval for jsf_sss_ping 2. Added function match_sound_vel to match sound velocity from jsf_dvl_ping to jsf_sss_ping and calculate the max slant range --- src/data_tools/include/data_tools/jsf_data.h | 6 +- src/data_tools/src/jsf_data.cpp | 94 +++++++++++++++++++- src/data_tools/src/test_jsf.cpp | 52 ++++++++--- src/pydata_tools/src/pyjsf_data.cpp | 6 ++ 4 files changed, 145 insertions(+), 13 deletions(-) diff --git a/src/data_tools/include/data_tools/jsf_data.h b/src/data_tools/include/data_tools/jsf_data.h index 6b8a2a56..99fec240 100644 --- a/src/data_tools/include/data_tools/jsf_data.h +++ b/src/data_tools/include/data_tools/jsf_data.h @@ -58,11 +58,14 @@ struct jsf_sss_ping uint16_t frequency; Eigen::Vector3d pos_; double sample_interval; + std::unordered_map flag_; // flag indicates which values are valid + double altitude_; + double slant_; template void serialize( Archive & ar ) { - ar(CEREAL_NVP(time_string_), CEREAL_NVP(time_stamp_), CEREAL_NVP(port), CEREAL_NVP(stbd), CEREAL_NVP(first_in_file_), CEREAL_NVP(rpy), CEREAL_NVP(lat_), CEREAL_NVP(long_), CEREAL_NVP(utm_zone), CEREAL_NVP(sound_vel), CEREAL_NVP(frequency), CEREAL_NVP(pos_), CEREAL_NVP(sample_interval)); + ar(CEREAL_NVP(time_string_), CEREAL_NVP(time_stamp_), CEREAL_NVP(port), CEREAL_NVP(stbd), CEREAL_NVP(first_in_file_), CEREAL_NVP(rpy), CEREAL_NVP(lat_), CEREAL_NVP(long_), CEREAL_NVP(utm_zone), CEREAL_NVP(sound_vel), CEREAL_NVP(frequency), CEREAL_NVP(pos_), CEREAL_NVP(sample_interval), CEREAL_NVP(flag_), CEREAL_NVP(altitude_), CEREAL_NVP(slant_)); } EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -106,6 +109,7 @@ cv::Mat make_waterfall_image(const jsf_sss_ping::PingsT& pings); void show_waterfall_image(const jsf_sss_ping::PingsT& pings); jsf_sss_ping::PingsT filter_frequency(const jsf_sss_ping::PingsT& pings, int desired_freq); std_data::sss_ping::PingsT convert_to_xtf_pings(const jsf_sss_ping::PingsT& pings); +jsf_sss_ping::PingsT match_sound_vel(jsf_sss_ping::PingsT& sss_pings, jsf_dvl_ping::PingsT& dvl_pings); } // namespace jsf_data diff --git a/src/data_tools/src/jsf_data.cpp b/src/data_tools/src/jsf_data.cpp index 44ce4d2c..bc5efd3a 100644 --- a/src/data_tools/src/jsf_data.cpp +++ b/src/data_tools/src/jsf_data.cpp @@ -68,7 +68,7 @@ cv::Mat make_waterfall_image(const jsf_sss_ping::PingsT& pings) } cv::Mat resized_swath_img;//dst image cv::resize(swath_img, resized_swath_img, cv::Size(512, rows));//resize image - + return resized_swath_img; } @@ -199,6 +199,10 @@ jsf_sss_ping read_datagram(std::ifstrea // NOTE: this is only valid if coord_units == 2 ping.lat_ = 0.0001/60.*double(jsf_sonar_data_hdr.y_coord); ping.long_ = 0.0001/60.*double(jsf_sonar_data_hdr.x_coord); + + ping.altitude_ = 0.001*double(jsf_sonar_data_hdr.al_in_mm); + // cout << "altitude in m: " << ping.altitude_ << endl; + double easting, northing; string utm_zone; tie(northing, easting, utm_zone) = lat_long_utm::lat_long_to_UTM(ping.lat_, ping.long_); @@ -206,9 +210,76 @@ jsf_sss_ping read_datagram(std::ifstrea ping.utm_zone = utm_zone; ping.pos_ = Eigen::Vector3d(easting, northing, -0.001*jsf_sonar_data_hdr.depth_in_mm); ping_side = process_side_scan_ping_side(input, jsf_hdr, jsf_sonar_data_hdr); + + + // initialize the flag + ping.flag_["latlon_XY"] = false; // bit 0 + ping.flag_["course"] = false; // bit 1 + + ping.flag_["speed"] = false; // bit 2 + ping.flag_["heading"] = false; // bit 3 + ping.flag_["pressure"] = false; // bit 4 + ping.flag_["pitchroll"] = false; // bit 5 + ping.flag_["altitude"] = false; // bit 6 + // bit 7 reserved + ping.flag_["water_temp"] = false; // bit 8 + ping.flag_["depth"] = false; // bit 9 + ping.flag_["annotation"] = false; // bit 10 + ping.flag_["cable_counter"] = false; // bit 11 + ping.flag_["kp"] = false; // bit 12 + ping.flag_["position_interpolated"] = false; // bit 13 + ping.flag_["sound_vel"] = false; // bit 14 + + // bit 0 + if (jsf_sonar_data_hdr.val_flag & 0x01) ping.flag_["latlon_XY"] = true; + + // bit 1 + if (jsf_sonar_data_hdr.val_flag & 0x02) ping.flag_["course"] = true; + + + // bit 2 + if (jsf_sonar_data_hdr.val_flag & 0x04) ping.flag_["speed"] = true; + + // bit 3 + if (jsf_sonar_data_hdr.val_flag & 0x08) ping.flag_["heading"] = true; + + // bit 4 + if (jsf_sonar_data_hdr.val_flag & 0x10) ping.flag_["pressure"] = true; + + // bit 5 + if (jsf_sonar_data_hdr.val_flag & 0x20) ping.flag_["pitchroll"] = true; + + // bit 6 + if (jsf_sonar_data_hdr.val_flag & 0x40) ping.flag_["altitude"] = true; + + // bit 7 + // if (jsf_sonar_data_hdr.val_flag & 0x80) ping.flag_["pitch"] = true; + + // bit 8 + if (jsf_sonar_data_hdr.val_flag & 0x100) ping.flag_["water_temp"] = true; + + // bit 9 + if (jsf_sonar_data_hdr.val_flag & 0x200) ping.flag_["depth"] = true; + + // bit 10 + if (jsf_sonar_data_hdr.val_flag & 0x400) ping.flag_["annotation"] = true; + + // bit 11 + if (jsf_sonar_data_hdr.val_flag & 0x800) ping.flag_["cable_counter"] = true; + + // bit 12 + if (jsf_sonar_data_hdr.val_flag & 0x1000) ping.flag_["kp"] = true; + + // bit 13 + if (jsf_sonar_data_hdr.val_flag & 0x2000) ping.flag_["position_interpolated"] = true; + + // bit 14 + if (jsf_sonar_data_hdr.val_flag & 0x4000) ping.flag_["sound_vel"] = true; + + // cout << "Protocol Version: " << std::hex << jsf_hdr.prot_ver << endl; //cout << "Coord units: " << jsf_sonar_data_hdr.coord_units << endl; - // cout << "Starting depth: " << jsf_sonar_data_hdr.starting_depth << "depth in mm: " << jsf_sonar_data_hdr.depth_in_mm << endl; + // cout << "Starting depth: " << jsf_sonar_data_hdr.starting_depth << "depth in m: " << 0.001*jsf_sonar_data_hdr.depth_in_mm << endl; const boost::posix_time::ptime epoch = boost::posix_time::time_from_string("1970-01-01 00:00:00.000"); boost::posix_time::ptime data_time; @@ -370,6 +441,25 @@ std_data::sss_ping::PingsT convert_to_xtf_pings(const jsf_sss_ping::PingsT& ping } +jsf_sss_ping::PingsT match_sound_vel(jsf_sss_ping::PingsT& sss_pings, jsf_dvl_ping::PingsT& dvl_pings) +{ + std::stable_sort(dvl_pings.begin(), dvl_pings.end(), [](const jsf_dvl_ping& dvl_ping1, const jsf_dvl_ping& dvl_ping2) { + return dvl_ping1.time_stamp_ < dvl_ping2.time_stamp_; + }); + std::stable_sort(sss_pings.begin(), sss_pings.end(), [](const jsf_sss_ping& sss_ping1, const jsf_sss_ping& sss_ping2) { + return sss_ping1.time_stamp_ < sss_ping2.time_stamp_; + }); + auto pos = dvl_pings.begin(); + for (jsf_sss_ping& sss_ping: sss_pings){ + pos = std::find_if(pos, dvl_pings.end(),[&](const jsf_dvl_ping& dvl_ping){ + return dvl_ping.time_stamp_ > sss_ping.time_stamp_; + }); + sss_ping.sound_vel = pos->sound_vel_; + sss_ping.slant_ = sss_ping.sound_vel * sss_ping.sample_interval* sss_ping.port.pings.size()/2; + } + return sss_pings; +} + } // namespace jsf_data diff --git a/src/data_tools/src/test_jsf.cpp b/src/data_tools/src/test_jsf.cpp index 08294369..2570e727 100644 --- a/src/data_tools/src/test_jsf.cpp +++ b/src/data_tools/src/test_jsf.cpp @@ -20,27 +20,27 @@ using namespace std; using namespace jsf_data; using namespace std_data; -void test_parse_sss(const boost::filesystem::path& path) +jsf_sss_ping::PingsT test_parse_sss(const boost::filesystem::path& path) { jsf_sss_ping::PingsT pings = parse_file(path); int rows = pings.size(); int cols = pings[0].port.pings.size() + pings[0].stbd.pings.size(); for (const jsf_data::jsf_sss_ping& ping : pings) { - cout << "Ping duration:" << ping.port.time_duration << endl; - cout << "Ping pos: " << ping.pos_.transpose() << endl; - cout << "Ping rpy: " << ping.rpy.transpose() << endl; + // cout << "Ping duration:" << ping.port.time_duration << endl; + // cout << "Ping pos: " << ping.pos_.transpose() << endl; + // cout << "Ping rpy: " << ping.rpy.transpose() << endl; } - jsf_sss_ping::PingsT filtered_pings = filter_frequency(pings, 21269); + // jsf_sss_ping::PingsT filtered_pings = filter_frequency(pings, 21269); printf("rows num: %d\n", rows); printf("cols num: %d\n", cols); - show_waterfall_image(filtered_pings); - + // show_waterfall_image(filtered_pings); + return pings; } -void test_parse_dvl(const boost::filesystem::path& path) +jsf_dvl_ping::PingsT test_parse_dvl(const boost::filesystem::path& path) { jsf_dvl_ping::PingsT pings = parse_file(path); cout << "Number of dvl pings: " << pings.size() << endl; @@ -58,15 +58,47 @@ void test_parse_dvl(const boost::filesystem::path& path) if (!pings.empty()) { cout << "Sound velocity from the first dvl data: " << pings[0].sound_vel_ << " m/s" << endl; } + return pings; +} + + +void test_match_sound_vel(const boost::filesystem::path& path){ + jsf_sss_ping::PingsT sss_pings = parse_file(path); + jsf_dvl_ping::PingsT dvl_pings = parse_file(path); + + + sss_pings = match_sound_vel(sss_pings, dvl_pings); + + for (auto i : sss_pings) { + // if (i.sound_vel==1477) + cout << "Sound velocity from the sss data at time_stamp: " << i.time_string_ << ", "<< i.sound_vel << " m/s" << endl; + } + for (auto i : dvl_pings) { + // if (i.sound_vel_==1477) + cout << "Sound velocity from the dvl data at time_stamp: " << i.time_string_ << ", "<, "Parse jsf_sss_ping from .jsf file") .def_static("parse_folder", &parse_folder_from_str, "Parse jsf_sss_ping from folder of .jsf files") .def_static("read_data", &read_data_from_str, "Read jsf_sss_ping::PingsT from .cereal file"); @@ -76,6 +80,8 @@ PYBIND11_MODULE(jsf_data, m) { m.def("show_waterfall_image", &show_waterfall_image, "Show a waterfall image created from jsf_sss_ping::PingsT"); m.def("filter_frequency", &filter_frequency, "Filter to keep only jsf_sss_ping::PingsT with certain frequency"); m.def("convert_to_xtf_pings", &convert_to_xtf_pings, "Convert jsf_sss_ping::PingsT to std_data::sss_ping::PingsT"); + m.def("match_sound_vel", &match_sound_vel, "Match sound velocity and slant range from jsf_dvl_ping::PingsT to jsf_sss_ping::PingsT"); + // from http://alexsm.com/pybind11-buffer-protocol-opencv-to-numpy/ pybind11::class_(m, "Image", pybind11::buffer_protocol()) From 388e9fd57bbea786541ccb570d6ace9790fd25f6 Mon Sep 17 00:00:00 2001 From: xyp8023 Date: Wed, 1 Apr 2020 18:14:19 +0200 Subject: [PATCH 3/3] Updated docs --- docs/data_tools.rst | 2 ++ docs/jsf_data.rst | 6 ++++++ 2 files changed, 8 insertions(+) create mode 100644 docs/jsf_data.rst diff --git a/docs/data_tools.rst b/docs/data_tools.rst index 2133e28a..2b8d4d91 100644 --- a/docs/data_tools.rst +++ b/docs/data_tools.rst @@ -38,3 +38,5 @@ bathymetric map as an image. csv_data xyz_data + + jsf_data diff --git a/docs/jsf_data.rst b/docs/jsf_data.rst new file mode 100644 index 00000000..e394d8fe --- /dev/null +++ b/docs/jsf_data.rst @@ -0,0 +1,6 @@ +jsf_data +======== + +.. automodule:: auvlib.data_tools.jsf_data + :members: + :undoc-members: