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