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); }