Velodyne Acoustics LIDAR HDL-32E - Manual (Page 25)

Coordinate Calculation Algorithm Sample Code
After removing all the correction parameters except vertical correction,
the calculation code is:
firingData::computeCoords(guint16 laserNum, boost::shared_ptr<CalibrationDB> db,
GLpos_t &pos)
{
guint16 idx = laserNum % VLS_LASER_PER_FIRING;
boost::shared_ptr<CalibrationPoint> cal = db->getCalibration(laserNum);
if (data->points[idx].distance == 0) {
coords[idx].setX(0.0);
coords[idx].setY(0.0);
coords[idx].setZ(0.0);
return;
}
float distance = db->getDistLSB() * (float)data->points[idx].distance;
// Get measured distance, distance1
float cosVertAngle = cal->getCosVertCorrection();
float sinVertAngle = cal->getSinVertCorrection();
float cosRotAngle = rotCosTable[data->position];
float sinRotAngle = rotSinTable[data->position];
float xyDistance = distance * cosVertAngle; // Convert to X-Y plane
coords[idx].setX(xyDistance * sinRotAngle + pos.getX()/VLS_DIM_SCALE);
// Calculate X coordinate
coords[idx].setY(xyDistance * cosRotAngle + pos.getY()/VLS_DIM_SCALE);
// Calculate Y coordinate
//Calculate Z coordinate
coords[idx].setZ(distance * sinVertAngle + pos.getZ()/VLS_DIM_SCALE);
}
[ 22 ]
HDL-32E User’s Manual aPPendix c: coordinate caLcuLation aLGoritHM saMPLe code