private static List <InternalDataFormat> Processhdf5Container_DaimlerLidar(int fileIndex, Hdf5Container_LidarDaimler hdf5Container_inp)
{
List <InternalDataFormat> listOfData_out = new List <InternalDataFormat>();
MetaData.Hdf5_DaimlerLidar._importedContainers.Add(hdf5Container_inp);
MetaData.Hdf5_DaimlerLidar._tableIndexToID.Add(new Dictionary <Tuple <int, int>, int>());
int containerRows = hdf5Container_inp._distances.GetLength(0);
int containerCols = hdf5Container_inp._distances.GetLength(1);
for (int j = 0; j < containerRows; j++)
{
for (int k = 0; k < containerCols; k++)
{
InternalDataFormat data = CreateInternalDataFormat(hdf5Container_inp, j, k);
MetaData.Hdf5_DaimlerLidar._tableIndexToID[fileIndex].Add(new Tuple <int, int>(j, k), data._ID);
if (hdf5Container_inp._pointValid[j, k] == 1)
{
listOfData_out.Add(data);
}
}
}
return(listOfData_out);
}
/// <summary>
/// this method sets the ground point attribute of the given internal data list to the right value
/// </summary>
/// <param name="listOfDataLists">List of internal data</param>
public static void SetGroundLabels(List <InternalDataFormat> listOfDataLists)
{
int segmentCount = 3;
float initialSeedDistanceThreshold = 0.3f;
float seedDistanceThreshold = 0.2f;
float covarIterations = 1;
InternalDataFormat seed = new InternalDataFormat(0, Vector3.zero, 0, 2);
List <InternalDataFormat> seedPoints = new List <InternalDataFormat>();
//List<List<InternalDataFormat>> cloudSegments = GetSimpleCloudSegments(listOfDataLists);
//List<List<InternalDataFormat>> cloudSegments = GetCloudSegmentsSegmentedByYAxis(listOfDataLists, segmentCount);
List <List <InternalDataFormat> > cloudSegments = GetCloudSegmentsSegmentedByYAndXAxis(listOfDataLists, segmentCount);
for (int i = 0; i < cloudSegments.Count; i++)
{
List <InternalDataFormat> segmentData = cloudSegments.ElementAt(i);
seedPoints = GetInitialSeedPoints(segmentData, initialSeedDistanceThreshold);
for (int j = 0; j < covarIterations; j++)
{
List <Vector3d> points = new List <Vector3d>();
for (int m = 0; m < seedPoints.Count; m++)
{
var pos = seedPoints[m]._position;
points.Add(new Vector3d(pos.x, pos.y, pos.z));
}
OrthogonalPlaneFit3 estimatedGroundPlane = new OrthogonalPlaneFit3(points);
Vector3 planeOriginPoint = new Vector3((float)estimatedGroundPlane.Origin.x, (float)estimatedGroundPlane.Origin.y, (float)estimatedGroundPlane.Origin.z);
Vector3 planeNormal = new Vector3((float)estimatedGroundPlane.Normal.x, (float)estimatedGroundPlane.Normal.y, (float)estimatedGroundPlane.Normal.z);
Plane estimatedGroundPlaneUnity = new Plane(planeNormal, planeOriginPoint);
if (j == covarIterations - 1)
{
Util.DrawPlane(planeOriginPoint, planeNormal, planeNormal.magnitude * 2, i);
}
for (int k = 0; k < segmentData.Count; k++)
{
seed = cloudSegments.ElementAt(i).ElementAt(k);
float distancePointToPlane = estimatedGroundPlaneUnity.GetDistanceToPoint(seed._position);
if (distancePointToPlane <= seedDistanceThreshold)
{
seed._groundPointLabel = 1;
seedPoints.Add(seed);
}
else
{
seed._groundPointLabel = 0;
}
}
}
}
}
