public static void Draw <C>(MultiEchoLaserScanMsg message, PointCloudDrawing pointCloud, MultiEchoLaserScanVisualizerSettings settings) where C : ICoordinateSpace, new()
{
pointCloud.SetCapacity(message.ranges.Length * message.ranges[0].echoes.Length);
// negate the angle because ROS coordinates are right-handed, unity coordinates are left-handed
float angle = -message.angle_min;
// foreach(MLaserEcho echo in message.ranges)
for (int i = 0; i < message.ranges.Length; i++)
{
var echoes = message.ranges[i].echoes;
// foreach (float range in echo.echoes)
for (int j = 0; j < echoes.Length; j++)
{
Vector3 point = Quaternion.Euler(0, Mathf.Rad2Deg * angle, 0) * Vector3.forward * echoes[j];
Color c = Color.HSVToRGB(Mathf.InverseLerp(message.range_min, message.range_max, echoes[j]), 1, 1);
var radius = settings.PointRadius;
if (message.intensities.Length > 0 && settings.UseIntensitySize)
{
radius = Mathf.InverseLerp(settings.SizeRange[0], settings.SizeRange[1], message.intensities[i].echoes[j]);
}
pointCloud.AddPoint(point, c, radius);
}
angle -= message.angle_increment;
}
}
/// <summary>
/// user switches the plane model in the listview
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void SelectPlanesListView_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
try
{
SelectPlanesListViewItem selectedItem = ((SelectPlanesListViewItem)e.AddedItems[0]);
//transform
List <List <Point> > points = new List <List <Point> >();
points.Add(selectedItem.PlaneModel.getPointList());
Post_knv_Server.DataIntegration.PointCloudDrawing.PointCloudPicturePackage pcpp = PointCloudDrawing.addTriangleToPicturePackage(pointCloud.pictures, points);
this._PointImagePlane_Bottom.Source = pcpp.bottomview;
this._PointImagePlane_Front.Source = pcpp.frontview;
this._PointImagePlane_Side.Source = pcpp.sideview;
}
catch (Exception) { }
}
/// <summary>
/// if the selection is changed in the listview the pictures have to be updated
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void EuclidianListView_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
try
{
EuclidianListViewItem ecl = ((EuclidianListViewItem)e.AddedItems[0]);
//transform
List <List <Point> > points = Algorithm.DelaunayTriangulation.calculateDelaunayTrianglePoints(ecl.pointcloud);
Post_knv_Server.DataIntegration.PointCloudDrawing.PointCloudPicturePackage pcpp = PointCloudDrawing.addTriangleToPicturePackage(refPointCloud.pictures, points);
this._PointImageEuclid_Bottom.Source = pcpp.bottomview;
this._PointImageEuclid_Front.Source = pcpp.frontview;
this._PointImageEuclid_Side.Source = pcpp.sideview;
}
catch (Exception) { }
}
