aPPendix f: duaL tWo Point caLiBration MethodoLoGy
float cosRotCorrection =
float cosRotAngle =
float sinRotAngle =
float hOffsetCorr =
;//Convert distance to
// 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)
if (yy<0)
//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 =
float distanceCorrY =
//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
//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
//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 ]