diff --git a/src/bathy_maps/include/bathy_maps/base_draper.h b/src/bathy_maps/include/bathy_maps/base_draper.h index 8213c55c..558a32ef 100644 --- a/src/bathy_maps/include/bathy_maps/base_draper.h +++ b/src/bathy_maps/include/bathy_maps/base_draper.h @@ -56,6 +56,7 @@ struct BaseDraper { csv_data::csv_asvp_sound_speed::EntriesT sound_speeds; double sensor_yaw; bool ray_tracing_enabled; // is snell ray tracing enabled? + double tracing_map_size; std::default_random_engine generator; // hopefully not same seed every time // NOTE: these are new style functions @@ -96,6 +97,7 @@ struct BaseDraper { void set_texture(const Eigen::MatrixXd& texture, const BoundsT& texture_bounds); void set_sidescan_yaw(double new_sensor_yaw) { sensor_yaw = new_sensor_yaw; } + void set_tracing_map_size(double new_tracing_map_size) { tracing_map_size = new_tracing_map_size; } void set_ray_tracing_enabled(bool enabled); void set_vehicle_mesh(const Eigen::MatrixXd& new_V2, const Eigen::MatrixXi& new_F2, const Eigen::MatrixXd& new_C2); diff --git a/src/bathy_maps/src/base_draper.cpp b/src/bathy_maps/src/base_draper.cpp index a2005134..ab976502 100644 --- a/src/bathy_maps/src/base_draper.cpp +++ b/src/bathy_maps/src/base_draper.cpp @@ -25,7 +25,7 @@ BaseDraper::BaseDraper(const Eigen::MatrixXd& V1, const Eigen::MatrixXi& F1, const csv_asvp_sound_speed::EntriesT& sound_speeds) : pings(pings), i(0), V1(V1), F1(F1), sound_speeds(sound_speeds), bounds(bounds), - sensor_yaw(0.), ray_tracing_enabled(false) + sensor_yaw(0.), ray_tracing_enabled(false), tracing_map_size(200.) { offset = Eigen::Vector3d(bounds(0, 0), bounds(0, 1), 0.); @@ -301,8 +301,8 @@ tuple BaseDr Eigen::VectorXd mod_right; Eigen::Vector3d offset_pos = pings[i].pos_ - offset; - if ((offset_pos - pos_small).norm() > 50.) { - tie(V1_small, F1_small) = mesh_map::cut_square_around_point(V1, F1, offset_pos.head<2>(), 200.); + if ((offset_pos - pos_small).norm() > tracing_map_size/4.) { + tie(V1_small, F1_small) = mesh_map::cut_square_around_point(V1, F1, offset_pos.head<2>(), tracing_map_size); if (V1_small.rows() == 0) { V1_small = V1; F1_small = F1; diff --git a/src/data_tools/include/data_tools/all_data.h b/src/data_tools/include/data_tools/all_data.h index 8a8304fc..835852af 100644 --- a/src/data_tools/include/data_tools/all_data.h +++ b/src/data_tools/include/data_tools/all_data.h @@ -150,6 +150,7 @@ struct all_echosounder_depth { std_data::mbes_ping::PingsT convert_matched_entries(all_mbes_ping::PingsT& pings, all_nav_entry::EntriesT& entries); std_data::mbes_ping::PingsT match_attitude(std_data::mbes_ping::PingsT& pings, all_nav_attitude::EntriesT& entries); csv_data::csv_asvp_sound_speed::EntriesT convert_sound_speeds(const all_mbes_ping::PingsT& pings); +std_data::attitude_entry::EntriesT convert_attitudes(const all_nav_attitude::EntriesT& attitudes); } // namespace all_data diff --git a/src/data_tools/include/data_tools/std_data.h b/src/data_tools/include/data_tools/std_data.h index 99a37eaf..05f68ccd 100644 --- a/src/data_tools/include/data_tools/std_data.h +++ b/src/data_tools/include/data_tools/std_data.h @@ -78,6 +78,28 @@ struct nav_entry EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; +struct attitude_entry +{ + using EntriesT = std::vector >; + + std::string time_string_; // readable time stamp string + long long time_stamp_; // posix time stamp + bool first_in_file_; // is first entry in a file? + double roll; + double pitch; + double yaw; + double heave; + + template + void serialize( Archive & ar ) + { + ar(CEREAL_NVP(time_string_), CEREAL_NVP(time_stamp_), CEREAL_NVP(first_in_file_), + CEREAL_NVP(roll), CEREAL_NVP(pitch), CEREAL_NVP(yaw), CEREAL_NVP(heave)); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + struct pt_submaps { using PointsT = std::vector >; @@ -194,6 +216,8 @@ void write_data_from_str(T& data, const std::string& path) write_data(data, boost::filesystem::path(path)); } +std::string time_string_from_time_stamp(long long time_stamp_); + } // namespace std_data #endif // DATA_STRUCTURES_H diff --git a/src/data_tools/include/data_tools/xtf_data.h b/src/data_tools/include/data_tools/xtf_data.h index ea8237a9..cc5bc015 100644 --- a/src/data_tools/include/data_tools/xtf_data.h +++ b/src/data_tools/include/data_tools/xtf_data.h @@ -69,6 +69,7 @@ Eigen::MatrixXd make_eigen_waterfall_image(const xtf_sss_ping::PingsT& pings); void show_waterfall_image(const xtf_sss_ping::PingsT& pings); xtf_sss_ping::PingsT correct_sensor_offset(const xtf_sss_ping::PingsT& pings, const Eigen::Vector3d& sensor_offset); +xtf_sss_ping::PingsT match_attitudes(const xtf_sss_ping::PingsT& pings, const std_data::attitude_entry::EntriesT& entries); } // namespace xtf_data diff --git a/src/data_tools/src/all_data.cpp b/src/data_tools/src/all_data.cpp index 9e82b988..cbdb725f 100644 --- a/src/data_tools/src/all_data.cpp +++ b/src/data_tools/src/all_data.cpp @@ -537,6 +537,32 @@ csv_data::csv_asvp_sound_speed::EntriesT convert_sound_speeds(const all_mbes_pin return csv_data::csv_asvp_sound_speed::EntriesT { sound_speed }; } +std_data::attitude_entry::EntriesT convert_attitudes(const all_nav_attitude::EntriesT& attitudes) +{ + std_data::attitude_entry::EntriesT entries; + std_data::attitude_entry entry; + for (const all_nav_attitude& att : attitudes) { + for (const all_nav_attitude_sample& sample : att.samples) { + entry.roll = sample.roll; + entry.pitch = sample.pitch; + entry.yaw = sample.heading; + entry.heave = sample.heave; + entry.time_stamp_ = att.time_stamp_ + sample.ms_since_start; + entry.time_string_ = std_data::time_string_from_time_stamp(entry.time_stamp_); + entries.push_back(entry); + } + if (!att.samples.empty()) { + entries[entries.size() - att.samples.size()].first_in_file_ = att.first_in_file_; + } + } + + std::stable_sort(entries.begin(), entries.end(), [](const std_data::attitude_entry& entry1, const std_data::attitude_entry& entry2) { + return entry1.time_stamp_ < entry2.time_stamp_; + }); + + return entries; +} + } // namespace all_data namespace std_data { diff --git a/src/data_tools/src/std_data.cpp b/src/data_tools/src/std_data.cpp index 6a73a117..be3a45b7 100644 --- a/src/data_tools/src/std_data.cpp +++ b/src/data_tools/src/std_data.cpp @@ -11,6 +11,10 @@ #include +#define BOOST_NO_CXX11_SCOPED_ENUMS +#include +#undef BOOST_NO_CXX11_SCOPED_ENUMS + using namespace std; namespace std_data { @@ -25,5 +29,16 @@ template void write_data(nav_entry::EntriesT& data, const b template void write_data(mbes_ping::PingsT& data, const boost::filesystem::path& path); template void write_data(pt_submaps& data, const boost::filesystem::path& path); +std::string time_string_from_time_stamp(long long time_stamp_) +{ + // TODO: can this be a static variable instead? we could also use that in other libraries + const boost::posix_time::ptime epoch = boost::posix_time::time_from_string("1970-01-01 00:00:00.000"); + boost::posix_time::ptime t = epoch + boost::posix_time::milliseconds(time_stamp_); + stringstream time_ss; + time_ss << t; + string time_string_ = time_ss.str(); + return time_string_; +} + } // namespace std_data diff --git a/src/data_tools/src/xtf_data.cpp b/src/data_tools/src/xtf_data.cpp index baf221fd..d1757f24 100644 --- a/src/data_tools/src/xtf_data.cpp +++ b/src/data_tools/src/xtf_data.cpp @@ -316,6 +316,38 @@ xtf_sss_ping::PingsT correct_sensor_offset(const xtf_sss_ping::PingsT& pings, co return new_pings; } +xtf_sss_ping::PingsT match_attitudes(const xtf_sss_ping::PingsT& pings, const std_data::attitude_entry::EntriesT& entries) +{ + xtf_sss_ping::PingsT new_pings = pings; + + auto pos = entries.begin(); + for (xtf_sss_ping& ping : new_pings) { + pos = std::find_if(pos, entries.end(), [&](const std_data::attitude_entry& entry) { + return entry.time_stamp_ > ping.time_stamp_; + }); + + ping.pitch_ = 0.; + ping.roll_ = 0.; + double heave; + if (pos == entries.end()) { + ping.pitch_ = entries.back().pitch; + ping.roll_ = entries.back().roll; + } + else if (pos == entries.begin()) { + ping.pitch_ = pos->pitch; + ping.roll_ = pos->roll; + } + else { + const std_data::attitude_entry& previous = *(pos - 1); + double ratio = double(ping.time_stamp_ - previous.time_stamp_)/double(pos->time_stamp_ - previous.time_stamp_); + ping.pitch_ = previous.pitch + ratio*(pos->pitch - previous.pitch); + ping.roll_ = previous.roll + ratio*(pos->roll - previous.roll); + } + } + + return new_pings; +} + } // namespace xtf_data namespace std_data { diff --git a/src/pybathy_maps/src/pymap_draper.cpp b/src/pybathy_maps/src/pymap_draper.cpp index c478d110..64415116 100644 --- a/src/pybathy_maps/src/pymap_draper.cpp +++ b/src/pybathy_maps/src/pymap_draper.cpp @@ -60,6 +60,7 @@ PYBIND11_MODULE(map_draper, m) { const xtf_sss_ping::PingsT&, const MapImageDraper::BoundsT&, const csv_asvp_sound_speed::EntriesT&>()) .def("set_sidescan_yaw", &MapImageDraper::set_sidescan_yaw, "Set yaw correction of sidescan with respect to nav frame") + .def("set_tracing_map_size", &MapImageDraper::set_tracing_map_size, "Set size of slice of map where we do ray tracing. Smaller makes it faster but you might cut off valid sidescan angles") .def("set_ray_tracing_enabled", &MapImageDraper::set_ray_tracing_enabled, "Set if ray tracing through water layers should be enabled. Takes more time but is recommended if there are large speed differences") .def("set_vehicle_mesh", &MapImageDraper::set_vehicle_mesh, "Provide the viewer with a vehicle model, purely for visualization") .def("show", &MapImageDraper::show, "Start the draping, and show the visualizer") @@ -75,6 +76,7 @@ PYBIND11_MODULE(map_draper, m) { const xtf_sss_ping::PingsT&, const MeasDataDraper::BoundsT&, const csv_asvp_sound_speed::EntriesT&>()) .def("set_sidescan_yaw", &MeasDataDraper::set_sidescan_yaw, "Set yaw correction of sidescan with respect to nav frame") + .def("set_tracing_map_size", &MeasDataDraper::set_tracing_map_size, "Set size of slice of map where we do ray tracing. Smaller makes it faster but you might cut off valid sidescan angles") .def("set_ray_tracing_enabled", &MeasDataDraper::set_ray_tracing_enabled, "Set if ray tracing through water layers should be enabled. Takes more time but is recommended if there are large speed differences") .def("set_vehicle_mesh", &MeasDataDraper::set_vehicle_mesh, "Provide the viewer with a vehicle model, purely for visualization") .def("show", &MeasDataDraper::show, "Start the draping, and show the visualizer") diff --git a/src/pybathy_maps/src/pypatch_draper.cpp b/src/pybathy_maps/src/pypatch_draper.cpp index b8b321b4..f5791811 100644 --- a/src/pybathy_maps/src/pypatch_draper.cpp +++ b/src/pybathy_maps/src/pypatch_draper.cpp @@ -50,6 +50,7 @@ PYBIND11_MODULE(patch_draper, m) { const xtf_sss_ping::PingsT&, const PatchDraper::BoundsT&, const csv_asvp_sound_speed::EntriesT&>()) .def("set_sidescan_yaw", &PatchDraper::set_sidescan_yaw, "Set yaw correction of sidescan with respect to nav frame") + .def("set_tracing_map_size", &PatchDraper::set_tracing_map_size, "Set size of slice of map where we do ray tracing. Smaller makes it faster but you might cut off valid sidescan angles") .def("set_ray_tracing_enabled", &PatchDraper::set_ray_tracing_enabled, "Set if ray tracing through water layers should be enabled. Takes more time but is recommended if there are large speed differences") .def("set_vehicle_mesh", &PatchDraper::set_vehicle_mesh, "Provide the viewer with a vehicle model, purely for visualization") .def("show", &PatchDraper::show, "Start the draping, and show the visualizer") diff --git a/src/pydata_tools/src/pyall_data.cpp b/src/pydata_tools/src/pyall_data.cpp index 4c3579c7..02ecb1d3 100644 --- a/src/pydata_tools/src/pyall_data.cpp +++ b/src/pydata_tools/src/pyall_data.cpp @@ -104,4 +104,5 @@ PYBIND11_MODULE(all_data, m) { m.def("convert_matched_entries", &convert_matched_entries, "Matches xtf_sss_ping::PingsT and csv_nav_entry::EntriesT and assign pos data to pings"); m.def("match_attitude", &match_attitude, "Match mbes_ping::PingsT and all_nav_attitude::EntriesT and assign attitude data to pings"); m.def("convert_sound_speeds", &convert_sound_speeds, "Convert all_mbes_ping::PingsT to csv_asvp_sound_speed::EntriesT"); + m.def("convert_attitudes", &convert_attitudes, "Convert all_nav_attitude::EntriesT to std_data::attitude_entry::EntriesT"); } diff --git a/src/pydata_tools/src/pystd_data.cpp b/src/pydata_tools/src/pystd_data.cpp index 3eff82f3..464988a1 100644 --- a/src/pydata_tools/src/pystd_data.cpp +++ b/src/pydata_tools/src/pystd_data.cpp @@ -49,7 +49,19 @@ PYBIND11_MODULE(std_data, m) { .def_static("parse_folder", &parse_folder_from_str, "Parse nav_entry from folder of ASCII files exported from NaviEdit") .def_static("read_data", &read_data_from_str, "Read nav_entry::Entries from .cereal file"); + py::class_(m, "attitude_entry", "Standard class interface for working with attitude data") + .def(py::init<>()) + .def_readwrite("time_string_", &attitude_entry::time_string_, "Member") + .def_readwrite("time_stamp_", &attitude_entry::time_stamp_, "Member") + .def_readwrite("first_in_file_", &attitude_entry::first_in_file_, "Member") + .def_readwrite("roll", &attitude_entry::roll, "Member") + .def_readwrite("pitch", &attitude_entry::pitch, "Member") + .def_readwrite("yaw", &attitude_entry::yaw, "Member") + .def_readwrite("heave", &attitude_entry::heave, "Member") + .def_static("read_data", &read_data_from_str, "Read attitude_entry::Entries from .cereal file"); + m.def("write_data", &write_data_from_str, "Write mbes_ping::PingsT to .cereal file"); m.def("write_data", &write_data_from_str, "Write nav_entry::EntriesT to .cereal file"); + m.def("write_data", &write_data_from_str, "Write attitude_entry::EntriesT to .cereal file"); } diff --git a/src/pydata_tools/src/pyxtf_data.cpp b/src/pydata_tools/src/pyxtf_data.cpp index 34bf2749..6b5dae3f 100644 --- a/src/pydata_tools/src/pyxtf_data.cpp +++ b/src/pydata_tools/src/pyxtf_data.cpp @@ -53,4 +53,5 @@ PYBIND11_MODULE(xtf_data, m) { m.def("make_waterfall_image", &make_eigen_waterfall_image, "Create a cv2 waterfall image from xtf_sss_ping::PingsT"); m.def("show_waterfall_image", &show_waterfall_image, "Show a waterfall image created from xtf_sss_ping::PingsT"); m.def("correct_sensor_offset", &correct_sensor_offset, "Move the sensor onboard the vehicle with a given translation"); + m.def("match_attitudes", &match_attitudes, "Get roll and pitch from std_data::attitude_entry by matching timestamps"); }