public VelodyneXYZPoint(int laserNumber, BlockHeaderID block, double range, double sensorTheta, int intensity)
{
RCFVCFHCF angles;
if (block == BlockHeaderID.TOP)
{
angles = VelodyneAngles.topblockDegrees[laserNumber];
}
else
{
angles = VelodyneAngles.bottomblockDegrees[laserNumber];
}
double xv = range * angles.RCFCOS * angles.VCFCOS;
double yv = range * angles.RCFSIN * angles.VCFCOS;
//yv += angles.HCF;
double zv = range * angles.VCFSIN;
//now do sensor rotation
sensorTheta *= -(Math.PI / 180.0);
x = (xv * Math.Cos(sensorTheta)) - (yv * Math.Sin(sensorTheta));
y = (xv * Math.Sin(sensorTheta)) + (yv * Math.Cos(sensorTheta));
z = zv;
this.intensity = (double)intensity / 255.0;
this.laserNum = laserNumber + ((block == BlockHeaderID.TOP)?32:0);
}
public VelodyneXYZPoint(int laserNumber, BlockHeaderID block, double range, double sensorTheta, int intensity)
{
RCFVCFHCF angles;
if (block == BlockHeaderID.TOP)
angles = VelodyneAngles.topblockDegrees[laserNumber];
else
angles = VelodyneAngles.bottomblockDegrees[laserNumber];
double xv = range * angles.RCFCOS * angles.VCFCOS;
double yv = range * angles.RCFSIN * angles.VCFCOS;
//yv += angles.HCF;
double zv = range * angles.VCFSIN;
//now do sensor rotation
sensorTheta *= -(Math.PI / 180.0);
x = (xv * Math.Cos(sensorTheta)) - (yv * Math.Sin(sensorTheta));
y = (xv * Math.Sin(sensorTheta)) + (yv * Math.Cos(sensorTheta));
z = zv;
this.intensity = (double)intensity/255.0;
this.laserNum = laserNumber + ((block==BlockHeaderID.TOP)?32:0);
}