Velodyne HDL-64E S3 - Page 44

HDL-64E S3
High Definition LiDAR Sensor
Page 44
Velodyne LiDAR, Inc.
©
2019
63-HDL64ES3 REV K
float xyDistance = distance * cosVertAngle;// — vOffsetCorr * sinVertAngle;
float xx = xyDistance * sinRotAngle;// — hOffsetCorr * cosRotAngle + pos.getX();
float yy = xyDistance * cosRotAngle;// + hOffsetCorr * sinRotAngle + pos.getY();
if (xx<0) xx=—xx;
if (yy<0) yy=—yy;
float distanceCorrX = (cal—>getDistCorrection()—cal—>getDistCorrectionX())*(xx—
240)/(2504—240)+cal—>getDistCorrectionX();
float distanceCorrY = (cal—>getDistCorrection()—cal—>getDistCorrectionY())*(yy—
l93)/(2504—l93)+cal—>getDistCorrectionY();
if (distancel > 2500) // if larger than 25m, no interpolation.
{ distanceCorrX = cal—>getDistCorrection();
distanceCorrY = distanceCorrX ;
}
distancel /= VLS_DIM_SCALE;
distanceCorrX /= VLS_DIM_SCALE;
distanceCorrY /= VLS_DIM_SCALE;
distance = distancel+distanceCorrX;
xyDistance = distance * cosVertAngle;// — vOffsetCorr * sinVertAngle;
coords[idx].setX(xyDistance * sinRotAngle — hOffsetCorr * cosRotAngle +
pos.getX()/VLS_DIM_SCALE);
distance = distancel+distanceCorrY;
xyDistance = distance * cosVertAngle;// — vOffsetCorr * sinVertAngle;
coords[idx].setY(xyDistance * cosRotAngle + hOffsetCorr * sinRotAngle +
pos.getY()/VLS_DIM_SCALE);
//coords[idx].setX(xyDistance * sinRotAngle — hOffsetCorr * cosRotAngle +
pos.getX()/VLS_DIM_SCALE);
//coords[idx].setY(xyDistance * cosRotAngle + hOffsetCorr * sinRotAngle +
pos.getY()/VLS_DIM_SCALE);
// coords[idx].setZ(distance * sinVertAngle + vOffsetCorr * cosVertAngle +
pos.getZ()/VLS_DIM_SCALE);
coords[idx].setZ(distance * sinVertAngle + vOffsetCorr + pos.getZ()/VLS_DIM_SCALE);
}