public ViewPoint(PointLikeObject point, double x, double y, SimpleVelodyneViewerColorMap colorMap) { this.ViewX = x; this.ViewY = y; this.PointObject = point; this.IsSelected = false; this.ColorMap = colorMap; }
/// <summary> /// Transform a point based on the transformation object. Includes rotation and translation /// </summary> /// <param name="pt">Points object</param> /// <param name="trans">Transformation object</param> /// <returns></returns> private PointLikeObject Transfrom(PointLikeObject pt, ObjectTransfromation trans) { PointLikeObject ptt = Rotate(pt, trans); ptt.X += trans.dx; ptt.Y += trans.dy; ptt.Z += trans.dz; return(ptt); }
/// <summary> /// otate a point based on X, Y, Z (omega, phi, kappa) rotations /// </summary> /// <param name="pt"></param> /// <param name="omega"></param> /// <param name="phi"></param> /// <param name="kappa"></param> /// <returns></returns> private PointLikeObject Rotate(PointLikeObject pt, double omega, double phi, double kappa) { double R11 = Math.Cos(phi) * Math.Cos(kappa); double R12 = -Math.Cos(phi) * Math.Sin(kappa); double R13 = Math.Sin(phi); double R21 = Math.Cos(omega) * Math.Sin(kappa) + Math.Sin(omega) * Math.Sin(phi) * Math.Cos(kappa); double R22 = Math.Cos(omega) * Math.Cos(kappa) - Math.Sin(omega) * Math.Sin(phi) * Math.Sin(kappa); double R23 = -Math.Sin(omega) * Math.Cos(phi); double R31 = Math.Sin(omega) * Math.Sin(kappa) - Math.Cos(omega) * Math.Sin(phi) * Math.Cos(kappa); double R32 = Math.Sin(omega) * Math.Cos(kappa) + Math.Cos(omega) * Math.Sin(phi) * Math.Sin(kappa); double R33 = Math.Cos(omega) * Math.Cos(phi); PointLikeObject ret = pt.Clone() as PointLikeObject; ret.X = pt.X * R11 + pt.Y * R12 + pt.Z * R13; ret.Y = pt.X * R21 + pt.Y * R22 + pt.Z * R23; ret.Z = pt.X * R31 + pt.Y * R32 + pt.Z * R33; return(ret); }
/// <summary> /// Rotate a point based on the transformation object. Only rotation, translation is skipped /// </summary> /// <param name="pt">Points object</param> /// <param name="trans">Transformation object</param> /// <returns>Transformed object; the input object won't be copied</returns> private PointLikeObject Rotate(PointLikeObject pt, ObjectTransfromation trans) { PointLikeObject ptt = Rotate(pt, 0, 0, trans.kappa); return(Rotate(ptt, trans.alpha, trans.beta, trans.gamma)); }
public ViewPoint(PointLikeObject point, double x, double y) : this(point, x, y, SimpleVelodyneViewerColorMap.Default) { }
/// <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(); }
public ViewPoint(PointLikeObject point, SimpleVelodyneViewerColorMap colorMap) : this(point, 0, 0, colorMap) { }
public ViewPoint(PointLikeObject point) : this(point, 0, 0) { }
protected PointLikeObject(PointLikeObject obj) { this.X = obj.X; this.Y = obj.Y; this.Z = obj.Z; }