public List <VelodynePoint> ReadNextFrame()
{
List <VelodynePoint> points = new List <VelodynePoint>();
bool isGo = true;
while (isGo)
{
VelodynePacket packet = ReadNext();
if (packet is VelodynePacket)
{
VelodynePointPacket pointPacket = packet as VelodynePointPacket;
foreach (VelodynePoint pt in pointPacket.Points)
{
points.Add(pt);
if ((lastPoint != null) && ((pt.Hz - lastPoint.Hz) < 0))
{
isGo = false;
break;
}
}
lastPoint = pointPacket.Points.Last();
}
}
return(points);
}
/// <summary>
/// Copy constructor
/// </summary>
/// <param name="obj">Copy object</param>
protected VelodynePoint(VelodynePoint obj) : base(obj)
{
this.InternalTime = obj.InternalTime;
this.Timestamp = obj.Timestamp;
this.Distance = obj.Distance;
this.Hz = obj.Hz;
this.Vz = obj.Vz;
this.Intensity = obj.Intensity;
this.ReturnType = obj.ReturnType;
}
// ref: http://velodynelidar.com/docs/manuals/VLP-16%20User%20Manual%20and%20Programming%20Guide%2063-9243%20Rev%20A.pdf
public override VelodynePacket ReadRecord(byte[] packet, DateTime?baseTime = null)
{
if (packet.Length == 554)
{
return(ReadRecordNMEA(packet));
}
else if (packet.Length == 1248)
{
DateTime?dateTopOfTheHour = null;
if (baseTime != null)
{
dateTopOfTheHour = new DateTime(baseTime.Value.Year, baseTime.Value.Month, baseTime.Value.Day, baseTime.Value.Hour, 0, 0);
}
//byte[] packet = reader.ReadBytes(1248);
VelodynePointPacket pointPacket = new VelodynePointPacket();
// Get timestamp and factory bytes
int l = packet.Length - 6; // this is the end of the pack!
double ts = BitConverter.ToUInt32(new byte[] { packet[l], packet[l + 1], packet[l + 2], packet[l + 3] }, 0) / 1000000.00;
factoryByte1 = packet[l + 4];
factoryByte2 = packet[l + 5];
if (ConvertFactroyByteToString(factoryByte2) != "VLP-16")
{
throw new Exception("Packet seems to come from other than VLP-16 sensor!");
}
if ((factoryByte1 == 0x38) && (this.ReturnMode == ReturnMode.StrongestReturnOnly))
{
throw new ArgumentException("Asked for StrongestReturnOnly but the factory byte indicates that the sensor was set to last return mode!");
}
if ((factoryByte1 == 0x37) && (this.ReturnMode == ReturnMode.LastReturnOnly))
{
throw new ArgumentException("Asked for StrongestReturnOnly but the factory byte indicates that the sensor was set to last return mode!");
}
// settigns for all returns
int i = 42; // header bytes
int k_iter = 2;
int j_iter = 16 * 3;
double[] firingAngle = firingAngleVLP16; // angles for vertical firing sequence
// Data packet
for (int datai = 0; datai < 12; datai++)
{
if ((packet[i] == 0xFF) && (packet[i + 1] == 0xEE))
{
byte lower_azimuth = packet[i + 2];
byte upper_azimuth = packet[i + 3];
double azimuth = BitConverter.ToUInt16(new byte[] { lower_azimuth, upper_azimuth }, 0) / 100.00;
//Console.WriteLine(i + " Azimuth: " + azimuth);
i = i + 4;
ReturnType returnType = datai % 2 == 0 ? ReturnType.StrongestReturn : ReturnType.LastReturn;
if (factoryByte1 == 0x39) // sensor in dual return, but we need only strongest or last returns
{
// skip all odd data block for strongest return
if ((ReturnMode == ReturnMode.StrongestReturnOnly) && (returnType == ReturnType.LastReturn))
{
i += j_iter * k_iter;
continue;
}
else if ((ReturnMode == ReturnMode.LastReturnOnly) && (returnType == ReturnType.StrongestReturn)) // skip all even data block for strongest return
{
i += j_iter * k_iter;
continue;
}
}
else if (factoryByte1 == 0x37) // strongest return
{
returnType = ReturnType.StrongestReturn;
}
else if (factoryByte1 == 0x38) // last return
{
returnType = ReturnType.LastReturn;
}
for (int k = 0; k < k_iter; k++)
{
for (int j = 0; j < j_iter; j = j + 3)
{
byte lower_distance = packet[i + j];
byte upper_distance = packet[i + j + 1];
double distance = (double)BitConverter.ToUInt16(new byte[] { lower_distance, upper_distance }, 0) * 2.0 / 1000.00;
byte intensity = packet[i + j + 2];
double omega = firingAngle[j / 3];
double omega_rad = omega / 180.0 * Math.PI;
double azimuth_rad = azimuth / 180.0 * Math.PI;
// VLP-16
double x = Math.Cos(azimuth_rad) * Math.Sin(omega_rad) * distance;
double y = Math.Cos(azimuth_rad) * Math.Cos(omega_rad) * distance;
double z = Math.Sin(azimuth_rad) * distance;
VelodynePoint pt = new VelodynePoint(x, y, z, distance, azimuth_rad, omega_rad, intensity, returnType);
pt.InternalTime = ts;
if (dateTopOfTheHour != null)
{
pt.Timestamp = dateTopOfTheHour.Value.AddSeconds(pt.InternalTime);
}
pointPacket.Points.Add(pt);
//if (factoryByte1 != 0x39)
//if (distance != 0)
//if (azimuth == 318.78)
//if (k == 0)
{
/*Console.WriteLine("Time (past the hour): " + ts + " s " + ts / 60 + "min \nFactory bytes: " + factoryByte1.ToString("X") + ": " + ConvertFactroyByteToString(factoryByte1) + ", "
+ factoryByte2.ToString("X") + ": " + ConvertFactroyByteToString(factoryByte2));
+ Console.WriteLine("Azimuth: " + azimuth + " Omega: " + omega + " Distance: " + distance);*/
//Console.WriteLine(datai + " " + k + " " + (j / 3) + " Azimuth: " + azimuth.ToString("0.00000") + " Omega: " + omega + " Distance: " + distance + " " + returnType + " " + ConvertFactroyByteToString(factoryByte1));
}
}
i += j_iter;
}
}
else
{
throw new Exception("Error in the point data packet structure!");
}
}
pointPacket.PacketTimestamp = pointPacket.Points[0].Timestamp;
return(pointPacket);
}
return(null);
}
/// <summary>
/// Render view with a given sampling
/// </summary>
/// <param name="sampling">Number of skipped objects</param>
private void Render(int sampling)
{
if ((viewPoints == null) || (viewPoints.Count == 0))
{
return;
}
DateTime startTime = DateTime.Now;
int width = viewPort.Width;
int height = viewPort.Height;
cam.cx = width / 2.0;
cam.cy = height / 2.0;
double R11 = Math.Cos(cam.phi) * Math.Cos(cam.kappa);
double R12 = -Math.Cos(cam.phi) * Math.Sin(cam.kappa);
double R13 = Math.Sin(cam.phi);
double R21 = Math.Cos(cam.omega) * Math.Sin(cam.kappa) + Math.Sin(cam.omega) * Math.Sin(cam.phi) * Math.Cos(cam.kappa);
double R22 = Math.Cos(cam.omega) * Math.Cos(cam.kappa) - Math.Sin(cam.omega) * Math.Sin(cam.phi) * Math.Sin(cam.kappa);
double R23 = -Math.Sin(cam.omega) * Math.Cos(cam.phi);
double R31 = Math.Sin(cam.omega) * Math.Sin(cam.kappa) - Math.Cos(cam.omega) * Math.Sin(cam.phi) * Math.Cos(cam.kappa);
double R32 = Math.Sin(cam.omega) * Math.Cos(cam.kappa) + Math.Cos(cam.omega) * Math.Sin(cam.phi) * Math.Sin(cam.kappa);
double R33 = Math.Cos(cam.omega) * Math.Cos(cam.phi);
Bitmap bmp = new Bitmap(width, height);
long ptk = 0;
using (Graphics g = Graphics.FromImage(bmp))
{
g.Clear(Color.Black);
for (int i = 0; i < viewPoints.Count(); i += sampling)
{
ptk++;
if ((worker != null) && (worker.CancellationPending))
{
bmp.Dispose();
workerDone.Set();
g.Dispose();
return;
}
ViewPoint pti = viewPoints[i];
if (pti == null)
{
continue;
}
PointLikeObject pt = Transfrom(pti.PointObject, cloudTransform);
double pti_x = cam.f * (R11 * (pt.X - cam.x) + R12 * (pt.Y - cam.y) + R13 * (pt.Z - cam.z)) / (R31 * (pt.X - cam.x) + R32 * (pt.Y - cam.y) + R33 * (pt.Z - cam.z)) + cam.cx;
double pti_y = -cam.f * (R21 * (pt.X - cam.x) + R22 * (pt.Y - cam.y) + R23 * (pt.Z - cam.z)) / (R31 * (pt.X - cam.x) + R32 * (pt.Y - cam.y) + R33 * (pt.Z - cam.z)) + cam.cy;
if ((pti_x > 0) && (pti_x < width) && (pti_y > 0) && (pti_y < height))
{
if (pti.PointObject is VelodynePoint)
{
VelodynePoint veloPoint = pti.PointObject as VelodynePoint;
SimpleVelodyneViewerColorMap colorMap = pti.ColorMap;
if (pti.IsSelected)
{
colorMap = SimpleVelodyneViewerColorMap.Selection;
}
Color clr = default(Color);
switch (colorMap)
{
case SimpleVelodyneViewerColorMap.Default:
clr = MapRainbowColor(255 - veloPoint.Intensity, 0, 255);
break;
case SimpleVelodyneViewerColorMap.Selection:
clr = MapRainbowColor(255 - veloPoint.Intensity, 0, 255);
clr = Color.FromArgb(255, clr.G, clr.B);
break;
default:
break;
}
Brush brush = new System.Drawing.SolidBrush(clr);
g.FillEllipse(brush, (int)pti_x, (int)pti_y, PointSize, PointSize);
brush.Dispose();
}
else if (pti.PointObject is VelodyneAnnotation)
{
VelodyneAnnotation annot = pti.PointObject as VelodyneAnnotation;
Brush brush = new System.Drawing.SolidBrush(Color.Red);
g.FillEllipse(brush, (int)pti_x, (int)pti_y, PointSize * 2, PointSize * 2);
var str = annot.ID.ToString();
if (annot.Text != "")
{
str += ": " + annot.Text;
}
g.DrawString(str, new Font("Arial", 12), brush, new PointF((float)pti_x + 3, (float)pti_y + 3));
brush.Dispose();
}
else
{
Brush brush = new System.Drawing.SolidBrush(Color.Red);
g.FillEllipse(brush, (int)pti_x, (int)pti_y, PointSize * 2, PointSize * 2);
brush.Dispose();
}
// update view coordinates
pti.ViewX = pti_x;
pti.ViewY = pti_y;
}
int refreshRate = Convert.ToInt32((double)viewPoints.Count() / 20.0);
if ((sampling == 1) && (ptk % 10 == 0))
{
lblStatus.Invoke((MethodInvoker)(() => lblStatus.Text = ((double)ptk / (double)viewPoints.Count() * 100.0).ToString("0.0") + "%"));
}
}
}
if (viewPort.Image != null)
{
viewPort.Image.Dispose();
}
viewPort.Invoke((MethodInvoker)(() => viewPort.Image = bmp));
if (viewPoints[0].PointObject is VelodynePoint)
{
VelodynePoint veloPoint = viewPoints[0].PointObject as VelodynePoint;
DateTime endTime = DateTime.Now;
TimeSpan tsPan = new TimeSpan(Convert.ToInt64(veloPoint.InternalTime * TimeSpan.TicksPerSecond));
String timeStamp = "???";
if (veloPoint.Timestamp != null)
{
timeStamp = veloPoint.Timestamp.Value.ToString("yyy-MM-dd HH:mm:ss.fff");
}
String its = tsPan.ToString(@"mm\:ss\.fff");
lblStatus.Invoke((MethodInvoker)(() => lblStatus.Text = "" + (endTime - startTime).TotalSeconds + "s (" + ptk / 1000 + "k)" +
" x: " + (cloudTransform.alpha / Math.PI * 180.0).ToString("0.0") + " y: " + (cloudTransform.beta / Math.PI * 180.0).ToString("0.0") + " z: " + (cloudTransform.kappa / Math.PI * 180.0).ToString("0.0") + " (" + its + "s) " + timeStamp));
}
workerDone.Set();
}
// Ref: http://velodynelidar.com/docs/manuals/VLP-16%20User%20Manual%20and%20Programming%20Guide%2063-9243%20Rev%20A.pdf
public override VelodynePacket ReadRecord(byte[] packet, DateTime?baseTime = null)
{
if (packet.Length == 554)
{
return(ReadRecordNMEA(packet));
}
else if (packet.Length == 1248)
{
// Get timestamp and factory bytes
int l = packet.Length - 6; // this is the end of the pack!
double ts = BitConverter.ToUInt32(new byte[] { packet[l], packet[l + 1], packet[l + 2], packet[l + 3] }, 0) / 1000000.00;
factoryByte1 = packet[l + 4];
factoryByte2 = packet[l + 5];
if (ConvertFactroyByteToString(factoryByte2) != "HDL-32E")
{
throw new Exception("Packet seems to come from other then HDL sensor!");
}
//byte[] packet = reader.ReadBytes(1248);
VelodynePointPacket pointPacket = new VelodynePointPacket();
// Data packet
int i = 42;
for (int datai = 0; datai < 12; datai++)
{
if ((packet[i] == 0xFF) && (packet[i + 1] == 0xEE))
{
//i += 0;
byte lower_azimuth = packet[i + 2];
byte upper_azimuth = packet[i + 3];
double azimuth = BitConverter.ToUInt16(new byte[] { lower_azimuth, upper_azimuth }, 0) / 100.00;
i = i + 4;
int k_iter = 2;
int j_iter = 16 * 3;
int i_jump = 16;
double[] firingAngle = firingAngleHDL32;
k_iter = 1;
j_iter = 32 * 3;
i_jump = 32;
firingAngle = firingAngleHDL32;
for (int k = 0; k < k_iter; k++)
{
for (int j = 0; j < j_iter; j = j + 3)
{
byte lower_distance = packet[i + j];
byte upper_distance = packet[i + j + 1];
double distance = (double)BitConverter.ToUInt16(new byte[] { lower_distance, upper_distance }, 0) * 2.0 / 1000.00;
byte intensity = packet[i + j + 2];
double omega = firingAngle[j / 3];
double omega_rad = omega / 180.0 * Math.PI;
double azimuth_rad = azimuth / 180.0 * Math.PI;
double x = Math.Cos(omega_rad) * Math.Sin(azimuth_rad) * distance;
double y = Math.Cos(omega_rad) * Math.Cos(azimuth_rad) * distance;
double z = Math.Sin(omega_rad) * distance;
VelodynePoint pt = new VelodynePoint(x, y, z, distance, azimuth_rad, omega_rad, intensity, ReturnType.StrongestReturn);
pt.InternalTime = ts;
if (baseTime != null)
{
pt.Timestamp = baseTime.Value.AddSeconds(pt.InternalTime);
}
pointPacket.Points.Add(pt);
//if (ConvertFactroyByteToString(factoryByte2) == "HDL-32E")
//{
// Console.WriteLine("Time (past the hour): " + ts + " s " + ts / 60 + "min \nFactory bytes: " + factoryByte1.ToString("X") + ": " + ConvertFactroyByteToString(factoryByte1) + ", "
// + factoryByte2.ToString("X") + ": " + ConvertFactroyByteToString(factoryByte2));
//Console.WriteLine("Azimuth: " + azimuth + " Omega: " + omega + " Distance: " + distance);
//}
//Console.WriteLine(pt.intensity);
}
i = i + i_jump * 3;
}
}
else
{
throw new Exception("Error in the point data packet structure!");
}
}
pointPacket.PacketTimestamp = pointPacket.Points[0].Timestamp;
return(pointPacket);
}
return(null);
}
