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