Skip to content

Commit

Permalink
Remove all performance tricks, use points, increase camera clip distance
Browse files Browse the repository at this point in the history 
Signed-off-by: Louise Poubel <[email protected]>
  • Loading branch information
@chapulina
chapulina committed Nov 30, 2021
1 parent b7b859f commit 6a353c1
Show file tree
Hide file tree
Showing 6 changed files with 128 additions and 172 deletions.
60 changes: 30 additions & 30 deletions lrauv_ignition_plugins/data/simple_test.csv
View file
Original file line number Diff line number Diff line change
@@ -1,31 +1,31 @@
elapsed_time_second,latitude_degree,longitude_degree,depth_meter,sea_water_temperature_degC,sea_water_salinity_psu,mass_concentration_of_chlorophyll_in_sea_water_ugram_per_liter,eastward_sea_water_velocity_meter_per_sec,northward_sea_water_velocity_meter_per_sec
0,0.0000001,0.0000000,0,-5.0,0.001,NaN,-1,0.5
0,0.0000002,0.0000000,0,-5.1,0.002,NaN,-1,1.5
0,0.0000003,0.0000000,0,-5.2,0.001,NaN,-1,0.5
0,0.0000004,0.0000000,0,-5.3,0.002,0.1,-1,0.5
0,0.0000005,0.0000000,0,-5.4,0.001,0.1,-1,0.5
0,0.0000001,0.0000001,0,-5.5,0.002,0.1,-1,0.5
0,0.0000002,0.0000001,0,-5.6,0.001,0.1,-1,0.5
0,0.0000003,0.0000001,0,-5.7,0.002,0.1,-1,0.5
0,0.0000004,0.0000001,0,-5.8,0.001,0.1,-1,1.5
0,0.0000005,0.0000001,0,-5.9,0.002,0.1,-1,0.5
0,0.0000001,0.0000002,0,-6.0,0.001,0.1,-1,0.5
0,0.0000002,0.0000002,0,-6.1,0.002,NaN,-1,0.5
0,0.0000003,0.0000002,0,-6.2,0.001,NaN,-1,0.5
0,0.0000004,0.0000002,0,-6.3,0.002,NaN,-1,0.5
0,0.0000005,0.0000002,0,-6.4,0.001,NaN,-1,1.5
0,0.0000001,0.0000003,10,-6.5,0.002,NaN,-1,0.5
0,0.0000002,0.0000003,10,-6.6,0.001,NaN,-1,0.5
0,0.0000003,0.0000003,10,-6.7,0.002,NaN,-1,0.5
0,0.0000004,0.0000003,10,-6.8,0.001,NaN,-1,0.5
0,0.0000005,0.0000003,10,-6.9,0.002,NaN,-1,0.5
0,0.0000001,0.0000004,10,-7.0,0.001,NaN,-1,0.5
0,0.0000002,0.0000004,10,-7.1,0.002,0.1,-1,0.5
0,0.0000003,0.0000004,10,-7.2,0.001,0.1,-1,1.5
0,0.0000004,0.0000004,10,-7.3,0.002,0.1,-1,0.5
0,0.0000005,0.0000004,10,-7.4,0.001,0.1,-1,0.5
0,0.0000001,0.0000005,10,-7.5,0.002,0.1,-1,0.5
0,0.0000002,0.0000005,10,-7.6,0.001,NaN,-1,0.5
0,0.0000003,0.0000005,10,-7.7,0.002,NaN,-1,1.5
0,0.0000004,0.0000005,10,-7.8,0.001,NaN,-1,0.5
0,0.0000005,0.0000005,10,-7.9,0.002,NaN,-1,0.5
0,0.000001,0.000000,0,-5.0,0.001,NaN,-1,0.5
0,0.000002,0.000000,0,-5.1,0.002,NaN,-1,1.5
0,0.000003,0.000000,0,-5.2,0.001,NaN,-1,0.5
0,0.000004,0.000000,0,-5.3,0.002,0.1,-1,0.5
0,0.000005,0.000000,0,-5.4,0.001,0.1,-1,0.5
0,0.000001,0.000001,0,-5.5,0.002,0.1,-1,0.5
0,0.000002,0.000001,0,-5.6,0.001,0.1,-1,0.5
0,0.000003,0.000001,0,-5.7,0.002,0.1,-1,0.5
0,0.000004,0.000001,0,-5.8,0.001,0.1,-1,1.5
0,0.000005,0.000001,0,-5.9,0.002,0.1,-1,0.5
0,0.000001,0.000002,0,-6.0,0.001,0.1,-1,0.5
0,0.000002,0.000002,0,-6.1,0.002,NaN,-1,0.5
0,0.000003,0.000002,0,-6.2,0.001,NaN,-1,0.5
0,0.000004,0.000002,0,-6.3,0.002,NaN,-1,0.5
0,0.000005,0.000002,0,-6.4,0.001,NaN,-1,1.5
0,0.000001,0.000003,0.5,-6.5,0.002,NaN,-1,0.5
0,0.000002,0.000003,0.5,-6.6,0.001,NaN,-1,0.5
0,0.000003,0.000003,0.5,-6.7,0.002,NaN,-1,0.5
0,0.000004,0.000003,0.5,-6.8,0.001,NaN,-1,0.5
0,0.000005,0.000003,0.5,-6.9,0.002,NaN,-1,0.5
0,0.000001,0.000004,0.5,-7.0,0.001,NaN,-1,0.5
0,0.000002,0.000004,0.5,-7.1,0.002,0.1,-1,0.5
0,0.000003,0.000004,0.5,-7.2,0.001,0.1,-1,1.5
0,0.000004,0.000004,0.5,-7.3,0.002,0.1,-1,0.5
0,0.000005,0.000004,0.5,-7.4,0.001,0.1,-1,0.5
0,0.000001,0.000005,0.5,-7.5,0.002,0.1,-1,0.5
0,0.000002,0.000005,0.5,-7.6,0.001,NaN,-1,0.5
0,0.000003,0.000005,0.5,-7.7,0.002,NaN,-1,1.5
0,0.000004,0.000005,0.5,-7.8,0.001,NaN,-1,0.5
0,0.000005,0.000005,0.5,-7.9,0.002,NaN,-1,0.5
39 changes: 0 additions & 39 deletions lrauv_ignition_plugins/src/ScienceSensorsSystem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,23 +190,6 @@ class tethys::ScienceSensorsSystemPrivate

/// \brief Publish a few more times for visualization plugin to get them
public: int repeatPubTimes = 1;

// 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.000001;
// 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;
};

/////////////////////////////////////////////////
Expand Down Expand Up @@ -445,30 +428,13 @@ 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},
ignition::math::SphericalCoordinates::SPHERICAL,
ignition::math::SphericalCoordinates::LOCAL2);
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(
Expand Down Expand Up @@ -855,11 +821,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;
Expand Down
142 changes: 49 additions & 93 deletions lrauv_ignition_plugins/src/VisualizePointCloud.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()};

Expand All @@ -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};
};
Expand Down Expand Up @@ -304,6 +299,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();
}

Expand Down Expand Up @@ -347,43 +346,30 @@ void VisualizePointCloud::PublishMarkers()
return;
}

std::lock_guard<std::recursive_mutex>(this->dataPtr->mutex);

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

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;
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
float dataVal = std::numeric_limits<float>::quiet_NaN();
// Value from float vector, if available
float dataVal = 1.0;
if (this->dataPtr->floatVMsg.data().size() > ptIdx)
{
dataVal = this->dataPtr->floatVMsg.data(ptIdx);
Expand All @@ -393,76 +379,32 @@ void VisualizePointCloud::PublishMarkers()
if (std::isnan(dataVal))
continue;

auto msg = markers.add_marker();

msg->set_ns(this->dataPtr->pointCloudTopic + "-" +
this->dataPtr->floatVTopic);
msg->set_id(nPtsViz + 1);

// TODO(chapulina) color per-point
// https://github.com/osrf/lrauv/issues/52
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:: math::Color color{
minC.R() + (maxC.R() - minC.R()) * ratio,
minC.G() + (maxC.G() - minC.G()) * ratio,
minC.B() + (maxC.B() - minC.B()) * 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);
msg->set_action(ignition::msgs::Marker::ADD_MODIFY);
ignition::msgs::Set(marker.mutable_material()->mutable_diffuse(), color);
ignition::msgs::Set(marker.add_materials()->mutable_diffuse(), color);

// 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.mutable_scale(),
ignition::math::Vector3d::One * this->dataPtr->pointSize);

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 [" << markers.marker().size() << "]"
<< 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);
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -535,6 +477,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)
19 changes: 19 additions & 0 deletions lrauv_ignition_plugins/src/VisualizePointCloud.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,14 @@ namespace tethys
NOTIFY MaxFloatVChanged
)

/// \brief Point size
Q_PROPERTY(
float pointSize
READ PointSize
WRITE SetPointSize
NOTIFY PointSizeChanged
)

/// \brief Constructor
public: VisualizePointCloud();

Expand Down Expand Up @@ -192,6 +200,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);
Expand Down
Loading

0 comments on commit 6a353c1

Please sign in to comment.