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>
/// 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)
{
}
/// <summary>
/// Add new points to the view
/// </summary>
/// <param name="points">Collection of VelodynePoint</param>
/// <param name="renderingMode">Rendering mode, if responsive, viewport will be rendered</param>
public void AddNewPoints(IEnumerable <VelodynePoint> points, SimpleVelodyneViewerRenderingMode renderingMode, SimpleVelodyneViewerColorMap colorMap = SimpleVelodyneViewerColorMap.Default)
{
foreach (VelodynePoint pt in points)
{
this.viewPoints.Add(new ViewPoint(pt, colorMap));
}
if (renderingMode == SimpleVelodyneViewerRenderingMode.Responsive)
{
Render();
}
}
/// <summary>
/// Add new points to the view
/// Viewport will be rendered if it is specified by RenderingMode
/// </summary>
/// <param name="points">Collection of VelodynePoint</param>
public void AddNewPoints(IEnumerable <VelodynePoint> points, SimpleVelodyneViewerColorMap colorMap = SimpleVelodyneViewerColorMap.Default)
{
AddNewPoints(points, this.RenderingMode, colorMap);
}
/// <summary>
/// Add points to the view with removing the previous points, viewport will be rendered
/// </summary>
/// <param name="points">Collection of VelodynePoint</param>
/// <param name="renderingMode">Rendering mode, if responsive, viewport will be rendered</param>
public void ClearAndAddNewPoints(IEnumerable <VelodynePoint> points, SimpleVelodyneViewerRenderingMode renderingMode, SimpleVelodyneViewerColorMap colorMap = SimpleVelodyneViewerColorMap.Default)
{
this.Clear();
AddNewPoints(points, renderingMode, colorMap);
}
