コード例 #1
0
    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);
    }
コード例 #2
0
/// <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;
                    }
                }
            }
        }
    }