From 1e6697da3cb1b0e13417e0105fb1e944cdad3d1f Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Tue, 21 Dec 2021 21:28:45 -0800
Subject: [PATCH 1/2] Remove miniature scale from science data and fix segfault

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 .../src/ScienceSensorsSystem.cc               | 42 +++++++---------
 .../worlds/buoyant_tethys.sdf                 | 48 ++++++++++++-------
 2 files changed, 50 insertions(+), 40 deletions(-)

diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
index da85aeb0..a3cfff44 100644
--- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
+++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
@@ -296,18 +296,6 @@ class tethys::ScienceSensorsSystemPrivate
   /// \brief Publish a few more times for visualization plugin to get them
   public: int repeatPubTimes = 1;
 
-  //////////////////////////////
-  // Constants for visualization
-
-  // TODO This is a workaround pending upstream Ignition orbit tool improvements
-  // \brief Scale down in order to see in view
-  // For 2003080103_mb_l3_las_1x1km.csv
-  //public: const float MINIATURE_SCALE = 0.01;
-  // For 2003080103_mb_l3_las.csv
-  public: const float MINIATURE_SCALE = 0.0001;
-  // For simple_test.csv
-  //public: const float MINIATURE_SCALE = 1.0;
-
   // TODO This is a workaround pending upstream Marker performance improvements.
   // \brief Performance trick. Skip depths below this z, so have memory to
   // visualize higher layers at higher resolution.
@@ -567,11 +555,6 @@ void ScienceSensorsSystemPrivate::ReadData(
         // Flip sign of z, because positive depth is negative z.
         cart.Z() = -depth;
 
-        // Performance trick. Scale down to see in view
-        cart *= this->MINIATURE_SCALE;
-        // Revert Z to the unscaled depth
-        cart.Z() = -depth;
-
         // Performance trick. Skip points beyond some distance from origin
         if (abs(cart.X()) > 1000 || abs(cart.Y()) > 1000)
         {
@@ -906,7 +889,8 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate(
   }
   SortIndices(xsSorted, xsSortedInds);
 
-  int ltPSortedIdx, gtPSortedIdx;
+  int ltPSortedIdx{-1};
+  int gtPSortedIdx{-1};
   float ltPDist, gtPDist;
 
   // Get the two closest positions in _xs that _p lies between.
@@ -925,6 +909,15 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate(
       break;
     }
   }
+  if (ltPSortedIdx < 0 || ltPSortedIdx >= xsSortedInds.size() ||
+      gtPSortedIdx < 0 || gtPSortedIdx >= xsSortedInds.size())
+  {
+    ignwarn << "Failed to find consecutive elements in sorted vector. Indices ["
+            << ltPSortedIdx << "] / [" << gtPSortedIdx
+            << "] for vector sized [" << xsSortedInds.size()
+            << "]. Cannot interpolate." << std::endl;
+    return std::numeric_limits<float>::quiet_NaN();
+  }
 
   // Normalize the distances to ratios between 0 and 1, to use as weights
   float ltPWeight = ltPDist / (gtPDist + ltPDist);
@@ -933,6 +926,12 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate(
   // Retrieve indices of sorted elements in original array
   int ltPIdx = xsSortedInds[ltPSortedIdx];
   int gtPIdx = xsSortedInds[gtPSortedIdx];
+
+  if (ltPIdx >= _values.size() || gtPIdx >= _values.size())
+  {
+    ignwarn << "Bad indices. Cannot interpolate." << std::endl;
+    return std::numeric_limits<float>::quiet_NaN();
+  }
   float result = ltPWeight * _values[ltPIdx] + gtPWeight * _values[gtPIdx];
 
   if (this->DEBUG_INTERPOLATE)
@@ -975,7 +974,7 @@ void ScienceSensorsSystemPrivate::SortIndices(
 
   // Sort indexes based on comparing values in v using std::stable_sort instead
   // of std::sort to avoid unnecessary index re-orderings when v contains
-  // elements of equal values 
+  // elements of equal values
   std::stable_sort(_idx.begin(), _idx.end(),
     [&_v](size_t _i1, size_t _i2) {return _v[_i1] < _v[_i2];});
 }
@@ -1396,11 +1395,6 @@ ignition::msgs::PointCloudPacked ScienceSensorsSystemPrivate::PointCloudMsg()
       {"xyz", ignition::msgs::PointCloudPacked::Field::FLOAT32},
     });
 
-  // TODO optimization for visualization:
-  // Use PCL methods to chop off points beyond some distance from sensor
-  // pose. Don't need to visualize beyond that. Might want to put that on a
-  // different topic specifically for visualization.
-
   msg.mutable_header()->mutable_stamp()->set_sec(this->timestamps[this->timeIdx]);
 
   pcl::PCLPointCloud2 pclPC2;
diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
index fdd64b08..6af05bea 100644
--- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
+++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf
@@ -101,7 +101,17 @@
         <scene>scene</scene>
         <ambient_light>0.4 0.4 0.4</ambient_light>
         <background_color>0.8 0.8 0.8</background_color>
+        <!-- looking at robot -->
         <camera_pose>0 6 6 0 0.5 -1.57</camera_pose>
+        <!-- looking at all science data for 2003080103_mb_l3_las.csv -->
+        <!--camera_pose>-50000 -30000 250000 0 1.1 1.58</camera_pose-->
+        <camera_clip>
+          <!-- ortho view needs low near clip -->
+          <!-- but a very low near clip messes orbit's far clip ?! -->
+          <near>0.1</near>
+          <!-- See 3000 km away -->
+          <far>3000000</far>
+        </camera_clip>
       </plugin>
 
       <!-- Plugins that add functionality to the scene -->
@@ -260,10 +270,25 @@
           <property type="string" key="state">docked_collapsed</property>
         </ignition-gui>
       </plugin>
+      <plugin filename="ViewAngle" name="Camera controls">
+        <ignition-gui>
+          <title>Camera controls</title>
+          <property type="string" key="state">docked_collapsed</property>
+        </ignition-gui>
+      </plugin>
       <plugin filename="GridConfig" name="Grid config">
         <ignition-gui>
           <property type="string" key="state">docked_collapsed</property>
         </ignition-gui>
+        <insert>
+          <!-- 300 km x 300 km -->
+          <cell_count>6</cell_count>
+          <vertical_cell_count>0</vertical_cell_count>
+          <!-- 50 km -->
+          <cell_length>50000</cell_length>
+          <pose>0 100000 0  0 0 0.32</pose>
+          <color>0 1 0 1</color>
+        </insert>
         <insert>
           <!-- 0.1 km x 0.1 km -->
           <cell_count>100</cell_count>
@@ -303,32 +328,23 @@
       <direction>-0.5 0.1 -0.9</direction>
     </light>
 
-    <!-- Uncomment if you need a ground plane -->
-    <!-- <model name="ground_plane">
+
+    <!-- This invisible plane helps with orbiting the camera, especially at large scales -->
+    <model name="horizontal_plane">
       <static>true</static>
       <link name="link">
-        <collision name="collision">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-            </plane>
-          </geometry>
-        </collision>
         <visual name="visual">
           <geometry>
             <plane>
               <normal>0 0 1</normal>
-              <size>100 100</size>
+              <!-- 300 km x 300 km -->
+              <size>300000 300000</size>
             </plane>
           </geometry>
-          <material>
-            <ambient>0.8 0.8 0.8 1</ambient>
-            <diffuse>0.8 0.8 0.8 1</diffuse>
-            <specular>0.8 0.8 0.8 1</specular>
-          </material>
+          <transparency>1.0</transparency>
         </visual>
       </link>
-    </model> -->
+    </model>
 
     <!-- Uncomment for particle effect
       Requires ParticleEmitter2 in ign-gazebo 4.8.0, which will be copied

From b8641f02224ef2b9172301234277e73871919dbe Mon Sep 17 00:00:00 2001
From: Mabel Zhang <mabel@openrobotics.org>
Date: Wed, 22 Dec 2021 23:16:13 -0500
Subject: [PATCH 2/2] actually store the sorted 1D neighbors

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
---
 .../src/ScienceSensorsSystem.cc               | 35 ++++++++++++++-----
 1 file changed, 27 insertions(+), 8 deletions(-)

diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
index a3cfff44..1b6f9616 100644
--- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
+++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
@@ -104,9 +104,9 @@ class tethys::ScienceSensorsSystemPrivate
     const std::vector<int> &_inds,
     std::vector<float> &_new);
 
-  /// \brief Sort vector in-place, keeping track of indices
+  /// \brief Sort vector, store indices to original vector after sorting.
+  /// Original vector unchanged.
   /// \param[in] _v Vector to sort
-  /// \param[out] _v Sorted vector
   /// \param[out] _idx Indices of original vector in sorted vector
   public: template<typename T>
   void SortIndices(
@@ -880,7 +880,7 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate(
     return std::numeric_limits<float>::quiet_NaN();
   }
 
-  // Sort points
+  // Sort points and store the indices
   std::vector<float> xsSorted;
   std::vector<size_t> xsSortedInds;
   for (int i = 0; i < _xs.size(); ++i)
@@ -888,6 +888,11 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate(
     xsSorted.push_back(_xs(i));
   }
   SortIndices(xsSorted, xsSortedInds);
+  // Access sorted indices to get the new sorted array
+  for (int i = 0; i < xsSortedInds.size(); ++i)
+  {
+    xsSorted[i] = _xs(xsSortedInds[i]);
+  }
 
   int ltPSortedIdx{-1};
   int gtPSortedIdx{-1};
@@ -909,13 +914,23 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate(
       break;
     }
   }
+
+  // Sanity check
   if (ltPSortedIdx < 0 || ltPSortedIdx >= xsSortedInds.size() ||
       gtPSortedIdx < 0 || gtPSortedIdx >= xsSortedInds.size())
   {
-    ignwarn << "Failed to find consecutive elements in sorted vector. Indices ["
-            << ltPSortedIdx << "] / [" << gtPSortedIdx
-            << "] for vector sized [" << xsSortedInds.size()
-            << "]. Cannot interpolate." << std::endl;
+    ignwarn << "1D linear interpolation: cannot find pair of consecutive "
+      << "neighbors that query point lies between. Cannot interpolate. "
+      << "(This should not happen!)"
+      << std::endl;
+    if (this->DEBUG_INTERPOLATE)
+    {
+      igndbg << "Neighbors: " << std::endl << _xs << std::endl;
+      igndbg << "Sorted neighbors: " << std::endl;
+      for (int i = 0; i < xsSorted.size(); ++i)
+        igndbg << xsSorted[i] << std::endl;
+      igndbg << "Query point: " << std::endl << _p << std::endl;
+    }
     return std::numeric_limits<float>::quiet_NaN();
   }
 
@@ -927,9 +942,13 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate(
   int ltPIdx = xsSortedInds[ltPSortedIdx];
   int gtPIdx = xsSortedInds[gtPSortedIdx];
 
+  // Sanity check
   if (ltPIdx >= _values.size() || gtPIdx >= _values.size())
   {
-    ignwarn << "Bad indices. Cannot interpolate." << std::endl;
+    ignwarn << "1D linear interpolation: mapping from sorted index to "
+      << "original index resulted in invalid index. Cannot interpolate. "
+      << "(This should not happen!)"
+      << std::endl;
     return std::numeric_limits<float>::quiet_NaN();
   }
   float result = ltPWeight * _values[ltPIdx] + gtPWeight * _values[gtPIdx];