Velodyne HD HDL-64E S2.1 - Page 34

float cosRotCorrection = cal->getCosRotCorrection();
float sinRotCorrection = cal->getSinRotCorrection();
float cosRotAngle = rotCosTable[data->position]*cosRotCorrection +
rotSinTable[data->position]*sinRotCorrection;
float sinRotAngle = rotSinTable[data->position]*cosRotCorrection -
rotCosTable[data->position]*sinRotCorrection;
float hOffsetCorr = cal->getHorizOffsetCorrection()/VLS_DIM_SCALE;
float vOffsetCorr = cal->getVertOffsetCorrection()/VLS_DIM_SCALE;
; //Convert distance to X-Y plane, formular is: xyDistance = distance * cosVertAngle
float xyDistance = distance * cosVertAngle
// Calculate temporal X, use absolute value.
float xx = xyDistance * sinRotAngle - hOffsetCorr * cosRotAngle + pos.getX();
// Calculate temporal Y, use absolute value
float yy = xyDistance * cosRotAngle + hOffsetCorr * sinRotAngle + pos.getY();
if (xx<0) xx=-xx;
if (yy<0) yy=-yy;
//Get 2points calibration values,Linear interpolation to get distance
correction for X and Y, that means distance correction use different value at
different distance
float distanceCorrX = (cal->getDistCorrection()-cal->getDistCorrectionX())*(xx-
240)/(2504-240)+cal->getDistCorrectionX();
float distanceCorrY = (cal->getDistCorrection()-cal->getDistCorrectionY())*(yy-
193)/(2504-193)+cal->getDistCorrectionY(); //fix in V2.0
// Unit convert: cm converts to meter
distance1 /= VLS_DIM_SCALE;
distanceCorrX /= VLS_DIM_SCALE;
distanceCorrY /= VLS_DIM_SCALE;
// Measured distance add distance correction in X.
distance = distance1+distanceCorrX;
xyDistance = distance * cosVertAngle; // Convert to X-Y plane
// Calculate X coordinate
coords[idx].setX(xyDistance * sinRotAngle - hOffsetCorr * cosRotAngle +
pos.getX()/VLS_DIM_SCALE);
// Measured distance add distance correction in Y.
distance = distance1+distanceCorrY;
xyDistance = distance * cosVertAngle; //Convert to X-Y plane
// Calculate Y coordinate
coords[idx].setY(xyDistance * cosRotAngle + hOffsetCorr * sinRotAngle +
pos.getY()/VLS_DIM_SCALE);
//Calculate Z coordinate, formula is : setZ(distance * sinVertAngle +
vOffsetCorr
coords[idx].setZ(distance * sinVertAngle + vOffsetCorr +
pos.getZ()/VLS_DIM_SCALE);
}
[ 31 ]
HDL-64E S2 and S2.1 User’s Manual
                