Visualize science data as points

lrauv_ignition_plugins/src/ScienceSensorsSystem.cc

@@ -295,26 +295,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.
-  // This is only for visualization, so that MAX_PTS_VIS can calculate close
-  // to the actual number of points visualized.
-  // Sensors shouldn't use this.
-  public: const float SKIP_Z_BELOW = -20;
 };
 
 /////////////////////////////////////////////////
@@ -553,12 +533,6 @@ void ScienceSensorsSystemPrivate::ReadData(
       // Check validity of spatial coordinates
       if (!std::isnan(latitude) && !std::isnan(longitude) && !std::isnan(depth))
       {
-        // Performance trick. Skip points below a certain depth
-        if (-depth < this->SKIP_Z_BELOW)
-        {
-          continue;
-        }
-
         // Convert lat / lon / elevation to Cartesian ENU
         auto cart = this->world.SphericalCoordinates(_ecm).value()
             .PositionTransform({IGN_DTOR(latitude), IGN_DTOR(longitude), 0.0},
@@ -567,17 +541,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)
-        {
-          continue;
-        }
-
         // Gather spatial coordinates, 3 fields in the line, into point cloud
         // for indexing this time slice of data.
         this->timeSpaceCoords[lineTimeIdx]->push_back(
@@ -906,7 +869,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 +889,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 +906,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 +954,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 +1375,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;

lrauv_ignition_plugins/src/VisualizePointCloud.cc

@@ -76,14 +76,6 @@ namespace tethys
     /// \brief Message holding a float vector.
     public: ignition::msgs::Float_V floatVMsg;
 
-    /// \brief Performance trick. Cap number of points to visualize, to save
-    /// memory.
-    public: const int MAX_PTS_VIS = 1000;
-
-    /// \brief Performance trick. Render only every other n. Increase to render
-    /// fewer markers (faster performance). Recalulated in function.
-    public: int renderEvery = 1;
-
     /// \brief Minimum value in latest float vector
     public: float minFloatV{std::numeric_limits<float>::max()};
 
@@ -96,6 +88,9 @@ namespace tethys
     /// \brief Color for maximum value
     public: ignition::math::Color maxColor{0, 255, 0, 255};
 
+    /// \brief Size of each point
+    public: float pointSize{20};
+
     /// \brief True if showing
     public: bool showing{true};
   };
@@ -305,6 +300,10 @@ void VisualizePointCloud::OnFloatV(const ignition::msgs::Float_V &_msg)
       this->SetMaxFloatV(data);
   }
 
+  // TODO(chapulina) Publishing whenever we get a new point cloud and a new
+  // floatV is good in case these topics are out of sync. But here they're
+  // synchronized, so in practice we're publishing markers twice for each
+  // PC+float that we get.
   this->PublishMarkers();
 }
 
@@ -349,40 +348,29 @@ void VisualizePointCloud::PublishMarkers()
   }
 
   std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);
+  ignition::msgs::Marker marker;
+  marker.set_ns(this->dataPtr->pointCloudTopic + this->dataPtr->floatVTopic);
+  marker.set_id(1);
+  marker.set_action(ignition::msgs::Marker::ADD_MODIFY);
+  marker.set_type(ignition::msgs::Marker::POINTS);
+  marker.set_visibility(ignition::msgs::Marker::GUI);
 
-  // Used to calculate cap of number of points to visualize, to save memory
-  int nPts = this->dataPtr->pointCloudMsg.height() *
-      this->dataPtr->pointCloudMsg.width();
-  // If there are more points than we can render, render every n
-  if (nPts > this->dataPtr->MAX_PTS_VIS)
-  {
-    this->dataPtr->renderEvery = (int) round(
-      nPts / (double) this->dataPtr->MAX_PTS_VIS);
-    ignwarn << "Only rendering one science data point for each "
-            << this->dataPtr->renderEvery << std::endl;
-  }
-
-  ignition::msgs::Marker_V markers;
+  ignition::msgs::Set(marker.mutable_scale(),
+    ignition::math::Vector3d::One * this->dataPtr->pointSize);
 
   PointCloudPackedIterator<float> iterX(this->dataPtr->pointCloudMsg, "x");
   PointCloudPackedIterator<float> iterY(this->dataPtr->pointCloudMsg, "y");
   PointCloudPackedIterator<float> iterZ(this->dataPtr->pointCloudMsg, "z");
 
   // Index of point in point cloud, visualized or not
   int ptIdx{0};
-  // Number of points actually visualized
-  int nPtsViz{0};
+  auto minC = this->dataPtr->minColor;
+  auto maxC = this->dataPtr->maxColor;
+  auto floatRange = this->dataPtr->maxFloatV - this->dataPtr->minFloatV;
   for (;iterX != iterX.end() &&
         iterY != iterY.end() &&
         iterZ != iterZ.end(); ++iterX, ++iterY, ++iterZ, ++ptIdx)
   {
-    // Performance trick. Only publish every nth. Skip z below some depth
-    if (this->dataPtr->renderEvery == 0 ||
-        ptIdx % this->dataPtr->renderEvery != 0)
-    {
-      continue;
-    }
-
     // Value from float vector, if available. Otherwise publish all data as
     // zeroes.
     float dataVal = 0.0;
@@ -391,82 +379,30 @@ void VisualizePointCloud::PublishMarkers()
       dataVal = this->dataPtr->floatVMsg.data(ptIdx);
     }
 
-    auto msg = markers.add_marker();
-
-    msg->set_ns(this->dataPtr->pointCloudTopic + this->dataPtr->floatVTopic);
-    msg->set_id(ptIdx + 1);
-
     // Don't visualize NaN
     if (std::isnan(dataVal))
-    {
-      msg->set_action(ignition::msgs::Marker::DELETE_MARKER);
       continue;
-    }
-    msg->set_action(ignition::msgs::Marker::ADD_MODIFY);
-
-    auto ratio = (dataVal - this->dataPtr->minFloatV) /
-        (this->dataPtr->maxFloatV - this->dataPtr->minFloatV);
-    auto color = this->dataPtr->minColor +
-        (this->dataPtr->maxColor - this->dataPtr->minColor) * ratio;
 
-    ignition::msgs::Set(msg->mutable_material()->mutable_ambient(), color);
-    ignition::msgs::Set(msg->mutable_material()->mutable_diffuse(), color);
-    msg->mutable_material()->mutable_diffuse()->set_a(0.5);
+    auto ratio = floatRange > 0 ?
+        (dataVal - this->dataPtr->minFloatV) / floatRange : 0.0f;
+    ignition:: math::Color color{
+      minC.R() + (maxC.R() - minC.R()) * ratio,
+      minC.G() + (maxC.G() - minC.G()) * ratio,
+      minC.B() + (maxC.B() - minC.B()) * ratio
+    };
 
-    // TODO: Use POINTS or LINE_LIST, but need per-vertex color
-    msg->set_type(ignition::msgs::Marker::BOX);
-    msg->set_visibility(ignition::msgs::Marker::GUI);
-    ignition::msgs::Set(msg->mutable_scale(),
-      ignition::math::Vector3d(0.2, 0.2, 0.2));
+    ignition::msgs::Set(marker.add_materials()->mutable_diffuse(), color);
 
-    ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
+    ignition::msgs::Set(marker.add_point(), ignition::math::Vector3d(
       *iterX,
       *iterY,
-      *iterZ,
-      0, 0, 0));
-
-    /*
-    // Use POINTS type and array for better performance, pending per-point
-    // color.
-    // One marker per point cloud, many points.
-    // TODO Implement in ign-gazebo per-point color like RViz point arrays,
-    // so can have just 1 marker, many points in it, each with a specified
-    // color, to improve performance. Color is the limiting factor that
-    // requires us to use many markers here, 1 point per marker.
-    // https://github.com/osrf/lrauv/issues/52
-    ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d(
-      0, 0, 0, 0, 0, 0));
-    auto pt = msg->add_point();
-    pt->set_x(*iterX);
-    pt->set_y(*iterY);
-    pt->set_z(*iterZ);
-    */
-
-    if (nPtsViz < 10)
-    {
-      igndbg << "Added point " << nPtsViz << " at "
-             << msg->pose().position().x() << ", "
-             << msg->pose().position().y() << ", "
-             << msg->pose().position().z() << ", "
-             << "value " << dataVal << ", "
-             << std::endl;
-    }
-    ++nPtsViz;
+      *iterZ));
   }
 
-  igndbg << "Received [" << nPts
-         << "] markers, visualizing [" << nPtsViz << "]"
-         << std::endl;
-
-  ignition::msgs::Boolean res;
-  bool result;
-  unsigned int timeout = 5000;
-  this->dataPtr->node.Request("/marker_array", markers, timeout, res, result);
+  igndbg << "Visualizing " << marker.point_size() << " points"
+    << std::endl;
 
-  if (!result || !res.data())
-  {
-    ignerr << "Failed to create markers on /marker_array" << std::endl;
-  }
+  this->dataPtr->node.Request("/marker", marker);
 }
 
 //////////////////////////////////////////////////
@@ -542,6 +478,20 @@ void VisualizePointCloud::SetMaxFloatV(float _maxFloatV)
   this->MaxFloatVChanged();
 }
 
+/////////////////////////////////////////////////
+float VisualizePointCloud::PointSize() const
+{
+  return this->dataPtr->pointSize;
+}
+
+/////////////////////////////////////////////////
+void VisualizePointCloud::SetPointSize(float _pointSize)
+{
+  this->dataPtr->pointSize = _pointSize;
+  this->PointSizeChanged();
+  this->PublishMarkers();
+}
+
 // Register this plugin
 IGNITION_ADD_PLUGIN(tethys::VisualizePointCloud,
                     ignition::gui::Plugin)

lrauv_ignition_plugins/src/VisualizePointCloud.hh

@@ -91,6 +91,14 @@ namespace tethys
       NOTIFY MaxFloatVChanged
     )
 
+    /// \brief Point size
+    Q_PROPERTY(
+      float pointSize
+      READ PointSize
+      WRITE SetPointSize
+      NOTIFY PointSizeChanged
+    )
+
     /// \brief Constructor
     public: VisualizePointCloud();
 
@@ -196,6 +204,17 @@ namespace tethys
     /// \brief Notify that maximum value has changed
     signals: void MaxFloatVChanged();
 
+    /// \brief Get the point size
+    /// \return Maximum value
+    public: Q_INVOKABLE float PointSize() const;
+
+    /// \brief Set the point size
+    /// \param[ax] _pointSize Maximum value.
+    public: Q_INVOKABLE void SetPointSize(float _pointSize);
+
+    /// \brief Notify that point size has changed
+    signals: void PointSizeChanged();
+
     /// \brief Set whether to show the point cloud.
     /// \param[in] _show Boolean value for displaying the points.
     public: Q_INVOKABLE void Show(bool _show);