Sign of pitch (NED to ENU)

src/data_tools/src/all_data.cpp

@@ -244,7 +244,7 @@ all_mbes_ping read_datagram<all_mbes_ping, all_xyz88_datagram>(std::istream& inp
 	new_ping.id_ = header.ping_count;
 	//new_ping.heading_ = header.heading;
     new_ping.heading_ = M_PI/180.*double(header.heading)*0.01;
-    new_ping.heading_ = 0.5*M_PI-new_ping.heading_; // this basically converts to yaw, should have that as marker instead
+    new_ping.heading_ = 0.5*M_PI-new_ping.heading_; // NED to ENU
 	new_ping.sound_vel_ = header.sound_vel;
 	new_ping.transducer_depth_ = header.transducer_depth;
     tie(new_ping.time_stamp_, new_ping.time_string_) = parse_all_time(header.date, header.time);
@@ -290,7 +290,7 @@ all_nav_entry read_datagram<all_nav_entry, all_position_datagram>(std::istream&
 	entry.long_ = double(header.longitude)/10000000.;
     entry.depth_ = stof(strs[6]);
 	entry.heading_ = M_PI/180.*double(header.heading)*0.01;
-    entry.heading_ = 0.5*M_PI-entry.heading_; // this basically converts to yaw
+    entry.heading_ = 0.5*M_PI-entry.heading_; // NED to ENU
 	entry.course_over_ground_ = double(header.course_over_ground)*0.01;
     tie(entry.time_stamp_, entry.time_string_) = parse_all_time(header.date, header.time);
 
@@ -327,10 +327,10 @@ all_nav_attitude read_datagram<all_nav_attitude, all_attitude_datagram>(std::ist
 	for (int i = 0; i < header.nbr_entries; ++i) {
 		input.read(reinterpret_cast<char*>(&meas), sizeof(meas));
         sample.ms_since_start = meas.ms_since_start;
-        sample.pitch = M_PI/180.*0.01*double(meas.pitch);
+        sample.pitch = -M_PI/180.*0.01*double(meas.pitch); // NED to ENU
         sample.roll = M_PI/180.*0.01*double(meas.roll);
         sample.heading = M_PI/180.*0.01*double(meas.heading);
-        //sample.heading = 0.5*M_PI-sample.heading; // this basically converts to yaw, should have that as marker instead
+        sample.heading = 0.5*M_PI-sample.heading; // NED to ENU
         sample.heave = 0.01*double(meas.heave);
         entry.samples.push_back(sample);
 	}

src/data_tools/src/csv_data.cpp

@@ -189,8 +189,8 @@ csv_nav_entry::EntriesT parse_file(const boost::filesystem::path& file)
         entry.pos_ = Eigen::Vector3d(x, y, z);
         entry.vel_ = Eigen::Vector3d(vx, vy, vz);
         entry.heading_ = M_PI/180.*heading;
-        entry.heading_ = 0.5*M_PI-entry.heading_; // TODO: need to keep this for old data. basically heading -> yaw
-        entry.pitch_ = M_PI/180.*pitch;
+        entry.heading_ = 0.5*M_PI-entry.heading_; // NED to ENU
+        entry.pitch_ = -M_PI/180.*pitch; // NED to ENU
         entry.roll_ = M_PI/180.*roll;
         entry.heading_std_ = M_PI/180.*heading_std;
         entry.pitch_std_ = M_PI/180.*pitch_std;

src/data_tools/src/gsf_data.cpp

@@ -177,10 +177,7 @@ mbes_ping::PingsT convert_pings(gsf_mbes_ping::PingsT& pings)
         //ping.pos_ = new_ping.pos_;
         int i = 0;
         for (const Eigen::Vector3d& beam : ping.beams) {
-            if (beam(2) > -5. || beam(2) < -25.) {
-                ++i;
-                continue;
-            }
+
             Eigen::Matrix3d Rz = Eigen::AngleAxisd(new_ping.heading_, Eigen::Vector3d::UnitZ()).matrix();
             /*Eigen::Matrix3d Ry = Eigen::AngleAxisd(new_ping.pitch_, Eigen::Vector3d::UnitY()).matrix();
             Eigen::Matrix3d Rx = Eigen::AngleAxisd(new_ping.roll_, Eigen::Vector3d::UnitX()).matrix();
@@ -435,9 +432,9 @@ gsf_mbes_ping::PingsT parse_file(const boost::filesystem::path& file)
             }
             if (records.mb_ping.heading != 0) {
                 ping.heading_ = M_PI/180.*records.mb_ping.heading;
-                ping.heading_ = 0.5*M_PI-ping.heading_; // TODO: need to keep this for old data
+                ping.heading_ = 0.5*M_PI-ping.heading_; // NED to ENU
                 ping.roll_ = M_PI/180.*records.mb_ping.roll;
-                ping.pitch_ = M_PI/180.*records.mb_ping.pitch;
+                ping.pitch_ = -M_PI/180.*records.mb_ping.pitch; // NED to ENU
             }
             else {
                 ping.heading_ = ping.roll_ = ping.pitch_ = 0.;

src/data_tools/src/jsf_data.cpp

@@ -186,9 +186,9 @@ jsf_sss_ping read_datagram<jsf_sss_ping, jsf_sonar_data_msg_header>(std::ifstrea
     jsf_sss_ping_side ping_side;
     ping.frequency = jsf_sonar_data_hdr.spl_freq_in_hz;
     ping.sound_vel = jsf_sonar_data_hdr.sound_speed_in_m_per_s;
-    ping.rpy = Eigen::Vector3d(jsf_sonar_data_hdr.roll, jsf_sonar_data_hdr.pitch, jsf_sonar_data_hdr.compass_heading);
+    ping.rpy = Eigen::Vector3d(jsf_sonar_data_hdr.roll, -jsf_sonar_data_hdr.pitch, jsf_sonar_data_hdr.compass_heading); // minus sign for pitch: NED to ENU
     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];
+    ping.rpy[2] = 0.5*M_PI - M_PI/180.*0.01*ping.rpy[2]; //  NED to ENU
 
     // NOTE: this is only valid if coord_units == 2
     ping.lat_ = 0.0001/60.*double(jsf_sonar_data_hdr.y_coord);

src/data_tools/src/xtf_data.cpp

@@ -124,9 +124,9 @@ xtf_sss_ping process_side_scan_ping(XTFPINGHEADER *PingHeader, XTFFILEHEADER *XT
    tie(northing, easting, utm_zone) = lat_long_utm::lat_long_to_UTM(ping.lat_, ping.long_);
    ping.pos_ = Eigen::Vector3d(easting, northing, -PingHeader->SensorDepth);
    ping.heading_ = M_PI/180.*PingHeader->SensorHeading;
-   ping.heading_ = 0.5*M_PI-ping.heading_; // TODO: need to keep this for old data
+   ping.heading_ = 0.5*M_PI-ping.heading_; // NED to ENU
    ping.roll_ = M_PI/180.*PingHeader->SensorRoll;
-   ping.pitch_ =  M_PI/180.*PingHeader->SensorPitch;
+   ping.pitch_ =  -M_PI/180.*PingHeader->SensorPitch; // NED to ENU
    ping.sound_vel_ = PingHeader->SoundVelocity;
 
    boost::posix_time::ptime data_time(boost::gregorian::date(PingHeader->Year, PingHeader->Month, PingHeader->Day), boost::posix_time::hours(PingHeader->Hour)+boost::posix_time::minutes(PingHeader->Minute)+boost::posix_time::seconds(PingHeader->Second)+boost::posix_time::milliseconds(10*int(PingHeader->HSeconds)));