Пример #1
0
        /// <summary>
        /// Search for best robot position in monto-carlo method
        /// </summary>
        /// <param name="cloud">Scan points cloud</param>
        /// <param name="searchPose">Search start position</param>
        /// <param name="distance">Best pose distance value (the lower the better)</param>
        /// <returns>Best found pose</returns>
        private Vector3 ParallelMonteCarloSearch(ScanCloud cloud, Vector3 searchPose, out int distance)
        {
            int[]     distances = new int[NumSearchThreads];
            Vector3[] poses     = new Vector3[NumSearchThreads];

            // Let all worker threads find best position
            worker.Work(index =>
            {
                poses[index] = MonteCarloSearch(
                    cloud,
                    searchPose,
                    () => randomQueuesXY[index].Dequeue(),
                    () => randomQueuesTheta[index].Dequeue(),
                    SearchIterationsPerThread,
                    out distances[index]);
            }, true);

            // Queue a task to fill random number queues for next search loop
            worker.Work(FillRandomQueues, false);

            // Find best pose of out all worker results
            int     bestDistance = int.MaxValue;
            Vector3 bestPose     = searchPose;

            for (int i = 0; i < NumSearchThreads; i++)
            {
                if (distances[i] < bestDistance)
                {
                    bestDistance = distances[i];
                    bestPose     = poses[i];
                }
            }

            // Return best pose and "distance"
            distance = bestDistance;
            return(bestPose);
        }
Пример #2
0
        /// <summary>
        /// Get complete hessian matrix derivatives
        /// </summary>
        /// <param name="gridMap">Map</param>
        /// <param name="scan">Scanned points</param>
        /// <param name="pose">Pose at which to calculate</param>
        /// <param name="H"></param>
        /// <param name="dTr"></param>
        protected void GetCompleteHessianDerivs(OccGridMap gridMap, ScanCloud scan, Vector3 pose, out Matrix4x4 H, out Vector3 dTr)
        {
            // Transformation of lidar measurements.
            // Translation is in pixels, need to convert it meters first.
            Matrix3x2 transform =
                Matrix3x2.CreateRotation(pose.Z) *
                Matrix3x2.CreateTranslation(pose.X * gridMap.Properties.CellLength, pose.Y * gridMap.Properties.CellLength) *
                Matrix3x2.CreateScale(gridMap.Properties.ScaleToMap);

            // Do the measurements scaling here, rather than wasting time in the rotDeriv calculation
            float sinRot = MathF.Sin(pose.Z) * gridMap.Properties.ScaleToMap;
            float cosRot = MathF.Cos(pose.Z) * gridMap.Properties.ScaleToMap;

            // Divide work to chunks
            int chunkSize = (scan.Points.Count + worker.NumThreads - 1) / worker.NumThreads;

            Vector3[]   vectors  = new Vector3[worker.NumThreads];
            Matrix4x4[] matrixes = new Matrix4x4[worker.NumThreads];

            // Do work in parallel threads
            worker.Work(index =>
            {
                var localH  = new Matrix4x4();
                var localTr = Vector3.Zero;

                foreach (Vector2 scanPoint in scan.Points.Skip(index * chunkSize).Take(chunkSize))
                {
                    Vector2 scanPointMap         = Vector2.Transform(scanPoint, transform);
                    Vector3 transformedPointData = InterpMapValueWithDerivatives(gridMap, scanPointMap);

                    float funVal = 1.0f - transformedPointData.X;

                    localTr.X += transformedPointData.Y * funVal;
                    localTr.Y += transformedPointData.Z * funVal;

                    float rotDeriv = ((-sinRot * scanPoint.X - cosRot * scanPoint.Y) * transformedPointData.Y +
                                      (cosRot * scanPoint.X - sinRot * scanPoint.Y) * transformedPointData.Z);

                    localTr.Z += rotDeriv * funVal;

                    localH.M11 += transformedPointData.Y.Sqr();
                    localH.M22 += transformedPointData.Z.Sqr();
                    localH.M33 += rotDeriv.Sqr();

                    localH.M12 += transformedPointData.Y * transformedPointData.Z;
                    localH.M13 += transformedPointData.Y * rotDeriv;
                    localH.M23 += transformedPointData.Z * rotDeriv;
                }

                matrixes[index] = localH;
                vectors[index]  = localTr;
            });

            // Aggragate threaded results
            H   = new Matrix4x4();
            dTr = Vector3.Zero;

            for (int i = 0; i < worker.NumThreads; i++)
            {
                H   += matrixes[i];
                dTr += vectors[i];
            }

            // Symmetry for inversion
            H.M21 = H.M12;
            H.M31 = H.M13;
            H.M32 = H.M23;

            // Make 4x4 matrix inversible
            H.M44 = 1.0f;
        }