diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index 4c1f3448..f49b6038 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -121,6 +121,7 @@ class tethys::ScienceSensorsSystemPrivate /// Vector size: number of time slices. Indices correspond to those of /// this->timestamps. /// Point cloud: spatial coordinates to index science data by location + /// in the ENU world frame. public: std::vector::Ptr> timeSpaceCoords; /// \brief Science data. @@ -429,9 +430,10 @@ void ScienceSensorsSystemPrivate::ReadData( { // Convert lat / lon / elevation to Cartesian ENU auto cart = this->world.SphericalCoordinates(_ecm).value() - .PositionTransform({latitude, longitude, -depth}, + .PositionTransform({IGN_DTOR(latitude), IGN_DTOR(longitude), 0.0}, ignition::math::SphericalCoordinates::SPHERICAL, ignition::math::SphericalCoordinates::LOCAL2); + cart.Z() = -depth; // Gather spatial coordinates, 3 fields in the line, into point cloud // for indexing this time slice of data. @@ -819,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; diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.cc b/lrauv_ignition_plugins/src/VisualizePointCloud.cc index 99f10a42..0c38f183 100644 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.cc +++ b/lrauv_ignition_plugins/src/VisualizePointCloud.cc @@ -362,8 +362,6 @@ void VisualizePointCloud::PublishMarkers() // 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() && @@ -401,8 +399,6 @@ void VisualizePointCloud::PublishMarkers() *iterX, *iterY, *iterZ)); - - ++nPtsViz; } igndbg << "Visualizing " << marker.point_size() << " points"