Velodyne Acoustics HDL-32E user manual APPendix c coordinate caLcuLation aLGoritHM saMPLe code

aPPendix c: coordinate caLcuLation aLGoritHM saMPLe code

HDL-32E User’s Manual

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 ]