Skip to content

Apply noise to lidar point cloud #86

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
nkoenig merged 3 commits into from
Feb 8, 2021
+34 −13
Merged

Apply noise to lidar point cloud #86

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
63ffc47
Apply noise to lidar point cloud
Jan 27, 2021
7fa30a2
Use ray count instead of range count
Feb 3, 2021
5d3be6d
Fix range to ray
Feb 5, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions include/ignition/sensors/Lidar.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,11 @@ namespace ignition
/// \return true if the update was successfull
public: virtual bool Update(const common::Time &_now) override;

/// \brief Apply noise to the laser buffer, if noise has been
/// configured. This should be called before PublishLidarScan if you
/// want the scan data to contain noise.
public: void ApplyNoise();

/// \brief Publish LaserScan message
/// \param[in] _now The current time
/// \return true if the update was successfull
Expand Down
14 changes: 9 additions & 5 deletions src/GpuLidarSensor.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,8 @@ using namespace ignition::sensors;
class ignition::sensors::GpuLidarSensorPrivate
{
/// \brief Fill the point cloud packed message
public: void FillPointCloudMsg();
/// \param[in] _laserBuffer Lidar data buffer.
public: void FillPointCloudMsg(const float *_laserBuffer);

/// \brief Rendering camera
public: ignition::rendering::GpuRaysPtr gpuRays;
Expand Down Expand Up @@ -232,6 +233,9 @@ bool GpuLidarSensor::Update(const ignition::common::Time &_now)
/// \todo(anyone) It would be nice to remove this copy.
this->dataPtr->gpuRays->Copy(this->laserBuffer);

// Apply noise before publishing the data.
this->ApplyNoise();

this->PublishLidarScan(_now);

if (this->dataPtr->pointPub.HasConnections())
Expand All @@ -244,7 +248,7 @@ bool GpuLidarSensor::Update(const ignition::common::Time &_now)

this->dataPtr->pointMsg.set_is_dense(true);

this->dataPtr->FillPointCloudMsg();
this->dataPtr->FillPointCloudMsg(this->laserBuffer);

{
this->AddSequence(this->dataPtr->pointMsg.mutable_header());
Expand Down Expand Up @@ -289,7 +293,7 @@ ignition::math::Angle GpuLidarSensor::VFOV() const
}

//////////////////////////////////////////////////
void GpuLidarSensorPrivate::FillPointCloudMsg()
void GpuLidarSensorPrivate::FillPointCloudMsg(const float *_laserBuffer)
{
IGN_PROFILE("GpuLidarSensorPrivate::FillPointCloudMsg");
uint32_t width = this->pointMsg.width();
Expand Down Expand Up @@ -322,8 +326,8 @@ void GpuLidarSensorPrivate::FillPointCloudMsg()
{
// Index of current point, and the depth value at that point
auto index = j * width * channels + i * channels;
float depth = this->gpuRays->Data()[index];
float intensity = this->gpuRays->Data()[index + 1];
float depth = _laserBuffer[index];
float intensity = _laserBuffer[index + 1];
uint16_t ring = j;

int fieldIndex = 0;
Expand Down
28 changes: 20 additions & 8 deletions src/Lidar.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -192,6 +192,26 @@ bool Lidar::Update(const ignition::common::Time &/*_now*/)
return false;
}

//////////////////////////////////////////////////
void Lidar::ApplyNoise()
{
if (this->dataPtr->noises.find(LIDAR_NOISE) != this->dataPtr->noises.end())
{
for (unsigned int j = 0; j < this->VerticalRayCount(); ++j)
{
for (unsigned int i = 0; i < this->RayCount(); ++i)
{
int index = j * this->RangeCount() + i;
Copy link
Contributor

@iche033 iche033 Feb 4, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

one more this->RangeCount() to this->RayCount() here

Copy link
Contributor Author

@nkoenig nkoenig Feb 5, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

5d3be6d

double range = this->laserBuffer[index*3];
range = this->dataPtr->noises[LIDAR_NOISE]->Apply(range);
range = ignition::math::clamp(range,
this->RangeMin(), this->RangeMax());
this->laserBuffer[index*3] = range;
}
}
}
}

//////////////////////////////////////////////////
bool Lidar::PublishLidarScan(const ignition::common::Time &_now)
{
Expand Down Expand Up @@ -236,14 +256,6 @@ bool Lidar::PublishLidarScan(const ignition::common::Time &_now)
int index = j * this->RangeCount() + i;
double range = this->laserBuffer[index*3];

if (this->dataPtr->noises.find(LIDAR_NOISE) !=
this->dataPtr->noises.end())
{
range = this->dataPtr->noises[LIDAR_NOISE]->Apply(range);
range = ignition::math::clamp(range,
this->RangeMin(), this->RangeMax());
}

range = ignition::math::isnan(range) ? this->RangeMax() : range;
this->dataPtr->laserMsg.set_ranges(index, range);
this->dataPtr->laserMsg.set_intensities(index,
Expand Down