Merge pull request #37 from nilsbore/hang_fixes

Added new methods for fixing problems that Chang was having

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

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<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd> 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;

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
 

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<attitude_entry, Eigen::aligned_allocator<attitude_entry> >;
+
+    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 <class Archive>
+    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<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> >;
@@ -194,6 +216,8 @@ void write_data_from_str(T& data, const std::string& path)
     write_data<T>(data, boost::filesystem::path(path));
 }
 
+std::string time_string_from_time_stamp(long long time_stamp_);
+
 } // namespace std_data
 
 #endif // DATA_STRUCTURES_H

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
 

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 {

src/data_tools/src/std_data.cpp

@@ -11,6 +11,10 @@
 
 #include <data_tools/std_data.h>
 
+#define BOOST_NO_CXX11_SCOPED_ENUMS
+#include <boost/date_time.hpp>
+#undef BOOST_NO_CXX11_SCOPED_ENUMS
+
 using namespace std;
 
 namespace std_data {
@@ -25,5 +29,16 @@ template void write_data<nav_entry::EntriesT>(nav_entry::EntriesT& data, const b
 template void write_data<mbes_ping::PingsT>(mbes_ping::PingsT& data, const boost::filesystem::path& path);
 template void write_data<pt_submaps>(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
 

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 {

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")

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")

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

src/pydata_tools/src/pystd_data.cpp

@@ -49,7 +49,19 @@ PYBIND11_MODULE(std_data, m) {
         .def_static("parse_folder", &parse_folder_from_str<nav_entry>, "Parse nav_entry from folder of ASCII files exported from NaviEdit")
         .def_static("read_data", &read_data_from_str<nav_entry::EntriesT>, "Read nav_entry::Entries from .cereal file");
 
+    py::class_<attitude_entry>(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<attitude_entry::EntriesT>, "Read attitude_entry::Entries from .cereal file");
+
     m.def("write_data", &write_data_from_str<mbes_ping::PingsT>, "Write mbes_ping::PingsT to .cereal file");
     m.def("write_data", &write_data_from_str<nav_entry::EntriesT>, "Write nav_entry::EntriesT to .cereal file");
+    m.def("write_data", &write_data_from_str<attitude_entry::EntriesT>, "Write attitude_entry::EntriesT to .cereal file");
 
 }

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