Skip to content

Commit

Permalink
Add support pcl::PointXYZRGBA to pcl::VLPGrabber
Browse files Browse the repository at this point in the history 
Add support pcl::PointXYZRGBA to pcl::VLPGrabber.
This change will be able to attach color to point cloud using
pcl::visualization::PointCloudColorHandlerRGBField.
  • Loading branch information
@UnaNancyOwen
UnaNancyOwen committed Nov 22, 2017
1 parent dd532fe commit 0dc4b89
Show file tree
Hide file tree
Showing 2 changed files with 67 additions and 23 deletions.
17 changes: 16 additions & 1 deletion io/include/pcl/io/vlp_grabber.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ namespace pcl
* \param[in] port UDP Port that should be used to listen for VLP packets
*/
VLPGrabber (const boost::asio::ip::address& ipAddress,
const unsigned short port);
const uint16_t port);

/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
virtual
Expand All @@ -80,13 +80,28 @@ namespace pcl
virtual std::string
getName () const;

/** \brief Allows one to customize the colors used for each of the lasers. */
void
setLaserColorRGB (const pcl::RGB& color,
uint8_t laserNumber);

protected:
static const uint8_t VLP_MAX_NUM_LASERS = 16;
static const uint8_t VLP_DUAL_MODE = 0x39;

private:
/** \brief Lookup table of Colors for coloring each of the lasers. */
pcl::RGB laser_rgb_mapping_[VLP_MAX_NUM_LASERS];

virtual void
toPointClouds (HDLDataPacket *dataPacket);

boost::asio::ip::address
getDefaultNetworkAddress ();

void
initializeLaserMapping ();

void
loadVLP16Corrections ();

Expand Down
73 changes: 51 additions & 22 deletions io/src/vlp_grabber.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,21 +40,20 @@

using boost::asio::ip::udp;

#define VLP_MAX_NUM_LASERS 16
#define VLP_DUAL_MODE 0x39

/////////////////////////////////////////////////////////////////////////////
pcl::VLPGrabber::VLPGrabber (const std::string& pcapFile) :
HDLGrabber ("", pcapFile)
{
initializeLaserMapping ();
loadVLP16Corrections ();
}

/////////////////////////////////////////////////////////////////////////////
pcl::VLPGrabber::VLPGrabber (const boost::asio::ip::address& ipAddress,
const unsigned short int port) :
const uint16_t port) :
HDLGrabber (ipAddress, port)
{
initializeLaserMapping ();
loadVLP16Corrections ();
}

Expand All @@ -65,12 +64,21 @@ pcl::VLPGrabber::~VLPGrabber () throw ()

/////////////////////////////////////////////////////////////////////////////
void
pcl::VLPGrabber::loadVLP16Corrections ()
pcl::VLPGrabber::initializeLaserMapping ()
{
double vlp16_vertical_corrections[] = { -15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15
for (uint8_t i = 0; i < VLP_MAX_NUM_LASERS / 2u; i++)
{
laser_rgb_mapping_[i * 2].b = static_cast<uint8_t> (i * 6 + 64);
laser_rgb_mapping_[i * 2 + 1].b = static_cast<uint8_t> ((i + 16) * 6 + 64);
}
}

};
for (int i = 0; i < VLP_MAX_NUM_LASERS; i++)
/////////////////////////////////////////////////////////////////////////////
void
pcl::VLPGrabber::loadVLP16Corrections ()
{
double vlp16_vertical_corrections[] = { -15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15 };
for (uint8_t i = 0; i < VLP_MAX_NUM_LASERS; i++)
{
HDLGrabber::laser_corrections_[i].azimuthCorrection = 0.0;
HDLGrabber::laser_corrections_[i].distanceCorrection = 0.0;
Expand Down Expand Up @@ -106,25 +114,25 @@ pcl::VLPGrabber::toPointClouds (HDLDataPacket *dataPacket)

double interpolated_azimuth_delta;

int index = 1;
uint8_t index = 1;
if (dataPacket->mode == VLP_DUAL_MODE)
{
index = 2;
}
if (dataPacket->firingData[index].rotationalPosition < dataPacket->firingData[0].rotationalPosition)
{
interpolated_azimuth_delta = ( (dataPacket->firingData[index].rotationalPosition + 36000) - dataPacket->firingData[0].rotationalPosition) / 2.0;
interpolated_azimuth_delta = ((dataPacket->firingData[index].rotationalPosition + 36000) - dataPacket->firingData[0].rotationalPosition) / 2.0;
}
else
{
interpolated_azimuth_delta = (dataPacket->firingData[index].rotationalPosition - dataPacket->firingData[0].rotationalPosition) / 2.0;
}

for (int i = 0; i < HDL_FIRING_PER_PKT; ++i)
for (uint8_t i = 0; i < HDL_FIRING_PER_PKT; ++i)
{
HDLFiringData firing_data = dataPacket->firingData[i];

for (int j = 0; j < HDL_LASER_PER_FIRING; j++)
for (uint8_t j = 0; j < HDL_LASER_PER_FIRING; j++)
{
double current_azimuth = firing_data.rotationalPosition;
if (j >= VLP_MAX_NUM_LASERS)
Expand All @@ -139,54 +147,64 @@ pcl::VLPGrabber::toPointClouds (HDLDataPacket *dataPacket)
{
if (current_sweep_xyz_->size () > 0)
{
current_sweep_xyz_->is_dense = current_sweep_xyzi_->is_dense = false;
current_sweep_xyz_->is_dense = current_sweep_xyzrgb_->is_dense = current_sweep_xyzi_->is_dense = false;
current_sweep_xyz_->header.stamp = velodyne_time;
current_sweep_xyzrgb_->header.stamp = velodyne_time;
current_sweep_xyzi_->header.stamp = velodyne_time;
current_sweep_xyz_->header.seq = sweep_counter;
current_sweep_xyzrgb_->header.seq = sweep_counter;
current_sweep_xyzi_->header.seq = sweep_counter;

sweep_counter++;

HDLGrabber::fireCurrentSweep ();
}
current_sweep_xyz_.reset (new pcl::PointCloud<pcl::PointXYZ> ());
current_sweep_xyzrgb_.reset (new pcl::PointCloud<pcl::PointXYZRGBA> ());
current_sweep_xyzi_.reset (new pcl::PointCloud<pcl::PointXYZI> ());
}

PointXYZ xyz;
PointXYZI xyzi;
PointXYZRGBA xyzrgb;
PointXYZ dual_xyz;
PointXYZI dual_xyzi;
PointXYZRGBA dual_xyzrgb;

HDLGrabber::computeXYZI (xyzi, current_azimuth, firing_data.laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);

xyz.x = xyzi.x;
xyz.y = xyzi.y;
xyz.z = xyzi.z;
xyz.x = xyzrgb.x = xyzi.x;
xyz.y = xyzrgb.y = xyzi.y;
xyz.z = xyzrgb.z = xyzi.z;

xyzrgb.rgba = laser_rgb_mapping_[j % VLP_MAX_NUM_LASERS].rgba;

if (dataPacket->mode == VLP_DUAL_MODE)
{
HDLGrabber::computeXYZI (dual_xyzi, current_azimuth, dataPacket->firingData[i + 1].laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);

dual_xyz.x = dual_xyzi.x;
dual_xyz.y = dual_xyzi.y;
dual_xyz.z = dual_xyzi.z;
dual_xyz.x = dual_xyzrgb.x = dual_xyzi.x;
dual_xyz.y = dual_xyzrgb.y = dual_xyzi.y;
dual_xyz.z = dual_xyzrgb.z = dual_xyzi.z;

dual_xyzrgb.rgba = laser_rgb_mapping_[j % VLP_MAX_NUM_LASERS].rgba;
}

if (! (boost::math::isnan (xyz.x) || boost::math::isnan (xyz.y) || boost::math::isnan (xyz.z)))
if (! (pcl_isnan (xyz.x) || pcl_isnan (xyz.y) || pcl_isnan (xyz.z)))
{
current_sweep_xyz_->push_back (xyz);
current_sweep_xyzrgb_->push_back (xyzrgb);
current_sweep_xyzi_->push_back (xyzi);

last_azimuth_ = current_azimuth;
}
if (dataPacket->mode == VLP_DUAL_MODE)
{
if ( (dual_xyz.x != xyz.x || dual_xyz.y != xyz.y || dual_xyz.z != xyz.z)
&& ! (boost::math::isnan (dual_xyz.x) || boost::math::isnan (dual_xyz.y) || boost::math::isnan (dual_xyz.z)))
if ((dual_xyz.x != xyz.x || dual_xyz.y != xyz.y || dual_xyz.z != xyz.z)
&& ! (pcl_isnan (dual_xyz.x) || pcl_isnan (dual_xyz.y) || pcl_isnan (dual_xyz.z)))
{
current_sweep_xyz_->push_back (dual_xyz);
current_sweep_xyzrgb_->push_back (dual_xyzrgb);
current_sweep_xyzi_->push_back (dual_xyzi);
}
}
Expand All @@ -205,3 +223,14 @@ pcl::VLPGrabber::getName () const
return (std::string ("Velodyne LiDAR (VLP) Grabber"));
}

/////////////////////////////////////////////////////////////////////////////
void
pcl::VLPGrabber::setLaserColorRGB (const pcl::RGB& color,
uint8_t laserNumber)
{
if (laserNumber >= VLP_MAX_NUM_LASERS)
return;

laser_rgb_mapping_[laserNumber] = color;
}

0 comments on commit 0dc4b89

Please sign in to comment.