public static PointGeometry3D ConvertToPointGeometry3D(PointCloud2 pointcloud)
{
PointGeometry3D pointGeometry3D = new PointGeometry3D
{
Indices = new IntCollection(),
Positions = new Vector3Collection(),
Colors = new Color4Collection()
};
int xOffset = -1;
int yOffset = -1;
int zOffset = -1;
int rgbOffset = -1;
foreach (PointField field in pointcloud.Fields)
{
if (field.Name == "x" && field.Datatype == FloatDataType)
{
xOffset = field.Offset;
}
else
if (field.Name == "y" && field.Datatype == FloatDataType)
{
yOffset = field.Offset;
}
else
if (field.Name == "z" && field.Datatype == FloatDataType)
{
zOffset = field.Offset;
}
else
if (field.Name == "rgb" && field.Datatype == FloatDataType)
{
rgbOffset = field.Offset;
}
}
if (xOffset >= 0 && yOffset >= 0 && zOffset >= 0 && rgbOffset >= 0)
{
byte[] pointcloudData = pointcloud.Data.ToByteArray();
if (pointcloud.IsBigendian)
{
Array.Reverse(pointcloudData);
}
for (int i = 0; i < pointcloudData.Length; i += pointcloud.PointStep)
{
float xPosition = BitConverter.ToSingle(pointcloudData, i + xOffset);
if (!float.IsNaN(xPosition))
{
float yPosition = BitConverter.ToSingle(pointcloudData, i + yOffset);
if (!float.IsNaN(xPosition))
{
float zPosition = BitConverter.ToSingle(pointcloudData, i + zOffset);
if (!float.IsNaN(xPosition))
{
SharpDX.Vector3 pointPosition = new SharpDX.Vector3(xPosition, yPosition, zPosition);
Color4 pointColor = new Color4(
(float)pointcloudData[i + rgbOffset + 2] / (float)byte.MaxValue,
(float)pointcloudData[i + rgbOffset + 1] / (float)byte.MaxValue,
(float)pointcloudData[i + rgbOffset] / (float)byte.MaxValue,
1);
pointGeometry3D.Indices.Add(pointGeometry3D.Positions.Count);
pointGeometry3D.Positions.Add(pointPosition);
pointGeometry3D.Colors.Add(pointColor);
}
}
}
}
}
return(pointGeometry3D);
}
protected override void ReceiveMessage(PointCloud2 message)
{
//throw new NotImplementedException();
size = message.data.GetLength(0);
int i = 0;
//Debug.Log("first"+ size);
byteArray = new byte[size];
foreach (byte temp in message.data)
{
byteArray[i] = temp;
i++;
}
//Debug.Log("second"+size);
//byte width = (byte)message.width;
//byte height = (byte)message.height;
//byte row_step = (byte)message.row_step;
//byte point_step = (byte)message.point_step;
width = message.width;
height = message.height;
row_step = message.row_step;
point_step = message.point_step;
Debug.Log("width" + message.width);
Debug.Log("height" + message.height);
Debug.Log("row_step" + message.row_step);
Debug.Log("point_step" + message.point_step);
size = size / point_step;
//Debug.Log("3"+size);
//BitConverter.ToSingle(test, 0);
/*
* pcl = new Vector3[size];
* float min_x = 0.0f;
* float min_y = 0.0f;
* float min_z = 0.0f;
* float max_x = 0.0f;
* float max_y = 0.0f;
* float max_z = 0.0f;
*
* for (int n = 0; n < size; n++)
* {
* int x_posi = n * message.point_step + message.fields[0].offset;
* int y_posi = n * message.point_step + message.fields[1].offset;
* int z_posi = n * message.point_step + message.fields[2].offset;
*
* float x = BitConverter.ToSingle(message.data, x_posi);
* float y = BitConverter.ToSingle(message.data, y_posi);
* float z = BitConverter.ToSingle(message.data, z_posi);
*
* //Debug.Log(x);
* //Debug.Log(y);
* //Debug.Log(z);
*
* min_x = ComFloat(x, min_x, "min");
* min_y = ComFloat(y, min_y, "min");
* min_z = ComFloat(z, min_z, "min");
* max_x = ComFloat(x, max_x, "max");
* max_y = ComFloat(y, max_y, "max");
* max_z = ComFloat(z, max_z, "max");
*
* pcl[n] = new Vector3(x, y, z);
* }
*
* Debug.Log("pcl_Finished"+pcl[0]);
* //床生成
* floor_posi = new Vector3(max_x - min_x, min_y - 10.0f, max_z - min_z);
* Instantiate(floorObject, new Vector3(0.0f,min_y-10.0f,0.0f), Quaternion.identity);
* //obj.transform.scale = new Vector3(max_x-min_x, 10.0f, max_z-min_z);
*/
//0x0001*[0] 0x0010*
//for (byte column = 0; column < width; column++)
//{
// for(byte row = 0; row < height; row++)
// {
// int arrayPosition = column * point_step + row * row_step;
// //Debug.Log("x" + message.fields[0].offset);
// //Debug.Log("y" + message.fields[1].offset);
// //Debug.Log("z" + message.fields[2].offset);
// int x = arrayPosition + message.fields[0].offset;
// int y = arrayPosition + message.fields[1].offset;
// int z = arrayPosition + message.fields[2].offset;
// pcl[column * message.width + row] = new Vector3(message.data[column * message.point_step + row * message.row_step + message.fields[0].offset], message.data[y], message.data[z]);
// //Debug.Log(column * message.width + row);
// //Debug.Log("x" + pcl[column * width + row].x);
// //Debug.Log("y" + pcl[column * width + row].y);
// //Debug.Log("z" + pcl[column * width + row].z);
// }
//}
//int max = message.fields.GetLength(0);
//for (int n = 0; n < max; n++)
//{
// Debug.Log("[" + n+"]"+ message.fields[n]);
//}
//CreateMesh();
if (num == 0)
{
isMessageReceived = true;
}
//Debug.Log("isMessageReceived"+ isMessageReceived);
}
public UpdatePointcloudOctomapRequest(PointCloud2 cloud)
{
this.cloud = cloud;
}
public UpdatePointcloudOctomapRequest()
{
this.cloud = new PointCloud2();
}
protected override void Handle(PointCloud2 message)
{
Container?.Clear();
Container?.AddRange(message.ToSlamPoints());
}
static void ClowWater()
{
List <Vector3> p3 = new List <Vector3>();
p3.Add(new Vector3(1.1, 0.9, 1.0));
p3.Add(new Vector3(6.9, 7.1, 7.0));
var line3fit = AutomationLibrary.Mathematics.Fitting.GeometricFits.FitLine(p3);
var line3 = Line3.FromPointAndDirection(new Vector3(0.5, 0.5, 0.5), new Vector3(1, 1, 1));
var nearest = line3.GetClosestPoint(new Vector3(27, -1.4, 19));
List <Vector2> points = new List <Vector2>();
using (var reader = System.IO.File.OpenText(@"C:\Users\douglas\desktop\pipe.csv"))
{
reader.ReadLine(); // skip header
while (true)
{
var line = reader.ReadLine();
if (line == null)
{
break;
}
var nums = line.Split(',');
var values = nums.Select(n => double.Parse(n)).ToArray();
points.Add(new Vector2(values[0], values[1]));
}
}
var ellipse = AutomationLibrary.Mathematics.Fitting.GeometricFits.FitEllipse(points);
var ellipseFunc = AutomationLibrary.Mathematics.Curves.CircularFunction.FromCartesianPoints(ellipse.Center, points);
var smoothEllipseFunc = ellipseFunc.SavitzkyGolaySmooth(3, 21);
points.Clear();
points.AddRange(GeneratePointsOnEllipticalArc(new Vector2(0.37, -2.4), 21, 24.26, 96.3 * Math.PI / 180.0, .020, -95.0 * Math.PI / 180.0, 97.0 * Math.PI / 180.0).Take(1000));
var pointSet = new PointCloud2(points);
var voronoi = AutomationLibrary.Mathematics.Geometry.Voronoi.VoronoiDiagram.ComputeForPoints(points);
voronoi = voronoi.Filter(0); // build map of nearest points
var centersOfInfiniteCells = new HashSet <Vector2>();
foreach (var edge in voronoi.Edges)
{
if (edge.IsPartlyInfinite)
{
centersOfInfiniteCells.Add(edge.LeftData);
centersOfInfiniteCells.Add(edge.RightData);
}
}
var pointSet2 = new PointCloud2(centersOfInfiniteCells);
var mcc = pointSet2.ComputeMinimumCircumscribingCircle();
var mic = ComputeMaximumInscribedCircle(pointSet, voronoi, mcc);
var lsc = GeometricFits.FitCircle(points);
using (var writer = System.IO.File.CreateText(@"C:\users\douglas\desktop\circlepoints.csv"))
{
writer.WriteLine("X,Y");
foreach (var point in pointSet)
{
writer.WriteLine("{0},{1}", point.X, point.Y);
}
}
Console.WriteLine("n = {0}", points.Count);
Console.WriteLine("MIC @ ({0}), r = {1}", mic.Center, mic.Radius);
Console.WriteLine("LSC @ ({0}), r = {1}", lsc.Center, lsc.Radius);
Console.WriteLine("MCC @ ({0}), r = {1}", mcc.Center, mcc.Radius);
Console.WriteLine();
Console.WriteLine("draw.circle({0}, {1}, {2}, border='{3}')", mic.Center.X, mic.Center.Y, mic.Radius, "red");
Console.WriteLine("draw.circle({0}, {1}, {2}, border='{3}')", lsc.Center.X, lsc.Center.Y, lsc.Radius, "blue");
Console.WriteLine("draw.circle({0}, {1}, {2}, border='{3}')", mcc.Center.X, mcc.Center.Y, mcc.Radius, "green");
Console.ReadLine();
}
