Velodyne Acoustics LIDAR HDL-32E User Manual

Page 25

Advertising
background image

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

Advertising