void gaussMixMapClient_MsgReceived(object sender, MsgReceivedEventArgs<GlobalMapCell> e)
        {
            isWorking = true;

            if (globalHeightMap == null)
            {
                double globalMapResolution = e.message.DataList[0].largeU; // first index has grid information
                double globalMapExtent = e.message.DataList[0].largeSig;
                globalHeightMap = new OccupancyGrid2D(globalMapResolution, globalMapResolution, globalMapExtent, globalMapExtent);
                globalSigMap = new OccupancyGrid2D(globalHeightMap);
                globalPijMap = new OccupancyGrid2D(globalHeightMap);
                laserHitMap = new OccupancyGrid2D(globalHeightMap);
                thresholdGlobalHeightMap = new OccupancyGrid2D(globalHeightMap);
            }
            // if message is received, update the 3 gridmap
            GlobalGaussianMixMap.ConvertToOcGrid(ref globalHeightMap, ref globalSigMap, ref globalPijMap, ref laserHitMap,
                e.message.DataList);
            for (int i = 0; i < globalHeightMap.NumCellX; i++)
            {
                for (int j = 0; j < globalHeightMap.NumCellY; j++)
                {
                    if (globalPijMap.GetCellByIdx(i, j) > 1)
                    {
                        thresholdGlobalHeightMap.SetCellByIdx(i, j, globalHeightMap.GetCellByIdx(i, j));
                    }
                    else
                    {
                        thresholdGlobalHeightMap.SetCellByIdx(i, j, 0);
                    }
                }
            }
        }
Beispiel #2
0
        public object DeepCopy()
        {
            OccupancyGrid2D copy = new OccupancyGrid2D(this.resolutionX, this.resolutionY, this.extentX, this.extentY);

            //Copy the occupancyMap
            Array.Copy(this.occupancyMap, copy.occupancyMap, this.occupancyMap.Length);

            return(copy);
        }
Beispiel #3
0
        public string ToLog()
        {
            string          toReturn = "";
            OccupancyGrid2D copy     = new OccupancyGrid2D(this);

            for (int i = 0; i < NumCellY; i++)
            {
                for (int j = 0; j < NumCellX; j++)
                {
                    toReturn += this.GetCellByIdx(j, i).ToString() + " ";
                }
                toReturn += "\n";
            }
            return(toReturn);
        }
        public GlobalGaussianMixMap(OccupancyGrid2D globalOcGrid)
        {
            addressProvider = new HardcodedAddressProvider();
            //robotPoseClient = new GenericMulticastClient<RobotPoseMessage>(addressProvider.GetAddressByName("RobotPose"), new CSharpMulticastSerializer<RobotPoseMessage>(true));
            //lidarScanClient = new GenericMulticastClient<LidarScanMessage>(addressProvider.GetAddressByName("LidarScan"), new CSharpMulticastSerializer<LidarScanMessage>(true));
            //sensorPoseClient = new GenericMulticastClient<SensorPoseMessage>(addressProvider.GetAddressByName("SensorPose"), new CSharpMulticastSerializer<SensorPoseMessage>(true));
            lidarPosePackageClient = new GenericMulticastClient<LidarPosePackageMessage>(addressProvider.GetAddressByName("LidarPosePackage"), new CSharpMulticastSerializer<LidarPosePackageMessage>(true));

            simLidarPosePackageClient = new GenericMulticastClient<SimMessage<LidarPosePackageMessage>>(addressProvider.GetAddressByName("SimLidarPosePackage"), new CSharpMulticastSerializer<SimMessage<LidarPosePackageMessage>>(true));

            globalGaussianMixMapServer = new GenericMulticastServer<GlobalMapCell>(addressProvider.GetAddressByName("GlobalGaussianMixMap"), new CSharpMulticastSerializer<GlobalMapCell>(true));
            globalGaussianMixMapServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));

            robotIDToPose = new Dictionary<int, RobotPose>();
            robotIDToScan = new Dictionary<int, ILidarScan<ILidar2DPoint>>();
            robotIDToSensorPose = new Dictionary<int, SensorPose>();
            robotIDToTimestamp = new Dictionary<int, double>();
            robotIDToPastTimestamp = new Dictionary<int, double>();
            globalOcGridByEachRobot = new Dictionary<int, OccupancyGrid2D>();
            robotIDToDynamicObstacles = new Dictionary<int, List<Polygon>>();

            this.globalOcGrid = new OccupancyGrid2D(globalOcGrid);
            laserToRobot = new SensorPose(0, 0, 0.5, 0, 0 / 180.0, 0, 0);
            gaussianMixMapAlgorithm = new GaussianMixMapping(globalOcGrid, laserToRobot);
            globalOcGridByEachRobotAlgorithm = new Dictionary<int, GaussianMixMapping>();

            lidarPosePackageClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
            simLidarPosePackageClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));

            lidarPosePackageClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<LidarPosePackageMessage>>(lidarPosePackageClient_MsgReceived);
            simLidarPosePackageClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<SimMessage<LidarPosePackageMessage>>>(simLidarPosePackageClient_MsgReceived);

            // local map request from robots
            localMapRequestClient = new GenericMulticastClient<LocalMapRequestMessage>(addressProvider.GetAddressByName("LocalMapRequest"), new CSharpMulticastSerializer<LocalMapRequestMessage>(true));
            localMapRequestClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
            localMapRequestClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<LocalMapRequestMessage>>(localMapRequestClient_MsgReceived);

            // local map update sender
            localMapReponseServer = new GenericMulticastServer<UpdateMapDataMessage>(addressProvider.GetAddressByName("LocalMapResponse"), new CSharpMulticastSerializer<UpdateMapDataMessage>(true));
            localMapReponseServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));

            Thread t = new Thread(new ParameterizedThreadStart(UpdateGlobalMap));
            t.Start();
            //Thread t2 = new Thread(new ParameterizedThreadStart(SendGlobalUpdate));
            //t2.Start();
        }
Beispiel #5
0
        public static OccupancyGrid2D operator +(OccupancyGrid2D a, OccupancyGrid2D b)
        {
            OccupancyGrid2D toReturn = new OccupancyGrid2D(a.ResolutionX, a.resolutionY, a.extentX, a.ExtentY);

            if (!(a.ExtentX == b.ExtentX && a.ExtentY == b.ExtentY && a.ResolutionX == b.ResolutionX && a.ResolutionY == b.ResolutionY))
            {
                throw new Exception("OccupancyGrid size does not match");
            }
            else
            {
                for (int i = 0; i < a.Width; i++)
                {
                    for (int j = 0; j < a.Height; j++)
                    {
                        toReturn.SetCellByIdx(i, j, a.GetCellByIdx(i, j) + b.GetCellByIdx(i, j));
                    }
                }
            }
            return(toReturn);
        }
        //void localMapUpdateClient_MsgReceived(object sender, MsgReceivedEventArgs<UpdateMapDataMessage> e)
        //{
        //    lock (locker)
        //    {
        //        //UpdateCurrentLocalMap(ref heightMap, ref covMap, ref pijMap, e.message);
        //        UpdateCurrentLocalMap(ref heightMap, e.message);
        //    }
        //}
        void UpdateCurrentLocalMap(ref OccupancyGrid2D currentLocalHeightMap, ref OccupancyGrid2D currentLocalCovMap,
															 ref OccupancyGrid2D currentLocalPijMap, UpdateMapDataMessage updateMessage)
        {
            try
            {
                //check if the size matches
                foreach (UpdateMapDataMessage.UpdateMapDataCell cell in updateMessage.CellData)
                {
                    if (cell.X == cell.Y && cell.Y == 0) continue; // the first packet
                    double x = cell.X; double y = cell.Y;
                    //currentLocalHeightMap.SetCell(x, y, cell.Height);
                    //currentLocalCovMap.SetCell(x, y, cell.Cov);
                    //currentLocalPijMap.SetCell(x, y, cell.Pij);
                }
            }
            catch
            {
                throw new IndexOutOfRangeException("The update message and current map are not in the same size");
            }
        }
        public List<Waypoint> FindPath(Waypoint start, Waypoint goal, OccupancyGrid2D og, out bool success)
        {
            List<Waypoint> path = new List<Waypoint>();

            //added by aaron (sort of a hack)
            if (og == null || goal.Coordinate.DistanceTo(start.Coordinate) == 0)
            {
                path.Add(new Waypoint(start.Coordinate, true, 0));
                success = true;
                return path;
            }

            int xIdx, yIdx;
            success = true;
            Vector2[] NESWVector = new Vector2[4];
            Vector2[] diagVector = new Vector2[4];
            bool[] NESW = new bool[4];
            Vector2 startV = start.Coordinate; // Start Vector2
            Vector2 goalV = goal.Coordinate; // Goal Vector2

            PriorityQueue open = new PriorityQueue();
            closed = new OccupancyGrid2D(resX, resY, extentX, extentY);
            opened = new OccupancyGrid2D(resX, resY, extentX, extentY);

            GetIndicies(startV.X, startV.Y, out xIdx, out yIdx);
            startV = new Vector2(xIdx, yIdx);
            GetIndicies(goalV.X, goalV.Y, out xIdx, out yIdx);
            goalV = new Vector2(xIdx, yIdx);

            Node root = new Node(goalV, goalV.DistanceTo(startV), 0, null);

            Node current = root;
            open.Push(current);

            // Do the spreading/discovering stuff until we discover a path.
            while (current.xy != startV)
            {
                if (open.q.Count == 0 || open.q.Count > MAX_OPEN)
                {
                    Console.WriteLine("Failure in DSstar. Open count is: " + open.q.Count);
                    success = false;
                    break;
                }
                current = open.Pop();

                NESWVector[0] = new Vector2(current.xy.X, current.xy.Y - 1);
                NESWVector[1] = new Vector2(current.xy.X + 1, current.xy.Y);
                NESWVector[2] = new Vector2(current.xy.X, current.xy.Y + 1);
                NESWVector[3] = new Vector2(current.xy.X - 1, current.xy.Y);

                diagVector[0] = new Vector2(current.xy.X + 1, current.xy.Y - 1);
                diagVector[1] = new Vector2(current.xy.X + 1, current.xy.Y + 1);
                diagVector[2] = new Vector2(current.xy.X - 1, current.xy.Y + 1);
                diagVector[3] = new Vector2(current.xy.X - 1, current.xy.Y - 1);

                for (int i = 0; i < 4; i++)
                {
                    if ((int)og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) < 255)
                    {
                        if (closed.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
                        {
                            NESW[i] = true;
                            if (opened.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
                            {
                                open.Push(new Node(NESWVector[i], NESWVector[i].DistanceTo(startV), current.h + 1
                                    + og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) / blurWeight, current));
                                opened.SetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y, 1);
                            }
                        }
                    }
                }

                for (int i = 0; i < 4; i++)
                {
                    if (NESW[i % 4] && NESW[(i + 1) % 4])
                    {
                        if (og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) < 255)
                        {
                            if (closed.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
                            {
                                if (opened.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
                                {
                                    open.Push(new Node(diagVector[i], diagVector[i].DistanceTo(startV), current.h + 1.4
                                        + og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) / blurWeight, current));
                                    opened.SetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y, 1);
                                }
                            }
                        }
                    }
                }

                for (int i = 0; i < 4; i++) NESW[i] = false;

                closed.SetCellByIdx((int) current.xy.X, (int) current.xy.Y, 1);
            }

            // Build a path using the discovered path.
            double x, y;
            Waypoint waypoint;

            // First waypoint is a user waypoint
            GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y);
            waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
            path.Add(waypoint);
            current = current.dad;

            // Middle waypoints are path waypoints
            while (current != root && current != null)
            {
                GetReals((int) current.xy.X, (int) current.xy.Y, out x, out y);
                waypoint = new Waypoint(x + resX / 2, y + resY / 2, false, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
                path.Add(waypoint);
                current = current.dad;
            }

            // Last waypoint is a user waypoint
            if (current != null)
            {
                GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y);
                waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
                path.Add(waypoint);
            }

            return path;
        }
 public static OccupancyGrid2D operator +(OccupancyGrid2D a, OccupancyGrid2D b)
 {
     OccupancyGrid2D toReturn = new OccupancyGrid2D(a.ResolutionX, a.resolutionY, a.extentX, a.ExtentY);
     if (!(a.ExtentX == b.ExtentX && a.ExtentY == b.ExtentY && a.ResolutionX == b.ResolutionX && a.ResolutionY == b.ResolutionY))
     {
         throw new Exception("OccupancyGrid size does not match");
     }
     else
     {
         for (int i = 0; i < a.Width; i++)
         {
             for (int j = 0; j < a.Height; j++)
             {
                 toReturn.SetCellByIdx(i, j, a.GetCellByIdx(i, j) + b.GetCellByIdx(i, j));
             }
         }
     }
     return toReturn;
 }
        public object DeepCopy()
        {
            OccupancyGrid2D copy = new OccupancyGrid2D(this.resolutionX, this.resolutionY, this.extentX, this.extentY);
            //Copy the occupancyMap
            Array.Copy(this.occupancyMap, copy.occupancyMap, this.occupancyMap.Length);

            return copy;
        }
Beispiel #10
0
 /// <summary>
 /// this is a copy constructor. You can use deepcopy, but this is better.
 /// </summary>
 /// <param name="gridToCopy"></param>
 public OccupancyGrid2D(OccupancyGrid2D gridToCopy)
     : this(gridToCopy.resolutionX, gridToCopy.resolutionY, gridToCopy.extentX, gridToCopy.extentY)
 {
     //Copy the occupancyMap
     Array.Copy(gridToCopy.occupancyMap, this.occupancyMap, gridToCopy.occupancyMap.Length);
 }
        /// <summary>
        /// Get difference of two occupancy map
        /// </summary>
        /// <param name="globalMulti">ocGrid1</param>
        /// <param name="globalSingle">ocGrid2</param>
        /// <param name="currentPose">current position</param>
        /// <param name="extentX">x-length of the comparison</param>
        /// <param name="extentY">y-length of the comparison</param>
        /// <returns>list of index classes - has ocGrid1 value</returns>
        public UpdateMapDataMessage Diff(int robotID, OccupancyGrid2D globalMulti, RobotPose currentPose, double extentX, double extentY)
        {
            if (!globalOcGridByEachRobotAlgorithm.ContainsKey(robotID)) return null;
            OccupancyGrid2D globalSingle = globalOcGridByEachRobotAlgorithm[robotID].UhatGM;
            //OccupancyGrid2D globalSingle = globalOcGridByEachRobot[robotID];
            //List<Index> diffIndexToSend = new List<Index>();
            List<Position> diffPositionToSend = new List<Position>();
            //List<float> heightList = new List<float>();
            //List<float> covList = new List<float>();
            //List<float> pijList = new List<float>();

            List<float> pijSum = new List<float>();
            List<float> puHat = new List<float>();
            List<float> puHatSquare = new List<float>();
            List<float> pSigUhateSquare = new List<float>();

            int numCellXHalf = (int)(extentX / globalMulti.ResolutionX);
            int numCellYHalf = (int)(extentY / globalMulti.ResolutionY);
            int currentCellX, currentCellY;
            globalMulti.GetIndicies(currentPose.x, currentPose.y, out currentCellX, out currentCellY);
            int comparisonCellX, comparisonCellY;
            for (int i = 0; i < numCellYHalf * 2; i++) // [i, j] = [column, row]
            {
                for (int j = 0; j < numCellXHalf * 2; j++)
                {
                    comparisonCellX = currentCellX - numCellXHalf + j;
                    comparisonCellY = currentCellY - numCellYHalf + i;
                    if (globalMulti.GetCellByIdx(comparisonCellX, comparisonCellY) != globalSingle.GetCellByIdx(comparisonCellX, comparisonCellY))
                    {
                        double x, y; globalMulti.GetReals(comparisonCellX, comparisonCellY, out x, out y);
                        diffPositionToSend.Add(new Position((float)x, (float)y));
                        //heightList.Add((float)gaussianMixMapAlgorithm.UhatGM.GetCellByIdx(j, i));
                        //covList.Add((float)gaussianMixMapAlgorithm.Psig_u_hat_square.GetCellByIdx(j, i));
                        //pijList.Add((float)gaussianMixMapAlgorithm.Pij_sum.GetCellByIdx(j, i));
                        pijSum.Add((float)gaussianMixMapAlgorithm.Pij_sum.GetCell(x, y));
                        puHat.Add((float)gaussianMixMapAlgorithm.Pu_hat.GetCell(x, y));
                        puHatSquare.Add((float)gaussianMixMapAlgorithm.Pu_hat_square.GetCell(x, y));
                        pSigUhateSquare.Add((float)gaussianMixMapAlgorithm.Psig_u_hat_square.GetCell(x, y));
                    }
                }
            }
            return new UpdateMapDataMessage(robotID, diffPositionToSend, pijSum, puHat, puHatSquare, pSigUhateSquare);
        }
 /// <summary>
 /// Return only height map difference
 /// </summary>
 /// <param name="robotID">Robot ID</param>
 /// <param name="globalMulti">global Map to compare</param>
 /// <param name="currentPose">robot's current position</param>
 /// <param name="extentX">extent x</param>
 /// <param name="extentY">extent y</param>
 /// <returns>UpdateMapDataMessage with only height information</returns>
 //public UpdateMapDataMessage HeightDiff(int robotID, OccupancyGrid2D globalMulti, RobotPose currentPose, double extentX, double extentY)
 //{
 //    if (!globalOcGridByEachRobotAlgorithm.ContainsKey(robotID)) return null;
 //    OccupancyGrid2D globalSingle = globalOcGridByEachRobotAlgorithm[robotID].UhatGM;
 //    //List<Index> diffIndexToSend = new List<Index>();
 //    List<Position> diffPositionToSend = new List<Position>();
 //    List<float> heightList = new List<float>();
 //    int numCellXHalf = (int)(extentX / globalMulti.ResolutionX);
 //    int numCellYHalf = (int)(extentY / globalMulti.ResolutionY);
 //    int currentCellX, currentCellY;
 //    globalMulti.GetIndicies(currentPose.x, currentPose.y, out currentCellX, out currentCellY);
 //    int comparisonCellX, comparisonCellY;
 //    for (int i = 0; i < numCellYHalf * 2; i++) // [i, j] = [column, row]
 //    {
 //        for (int j = 0; j < numCellXHalf * 2; j++)
 //        {
 //            comparisonCellX = currentCellX - numCellXHalf + j;
 //            comparisonCellY = currentCellY - numCellYHalf + i;
 //            if (comparisonCellX < 0 || comparisonCellX > globalSingle.NumCellX || comparisonCellY < 0 || comparisonCellY > globalSingle.NumCellY)
 //                continue;
 //            if (globalMulti.GetCellByIdx(comparisonCellX, comparisonCellY) != globalSingle.GetCellByIdx(comparisonCellX, comparisonCellY))
 //            {
 //                double x, y; globalMulti.GetReals(comparisonCellX, comparisonCellY, out x, out y);
 //                diffPositionToSend.Add(new Position((float)x, (float)y));
 //                heightList.Add((float)gaussianMixMapAlgorithm.UhatGM.GetCellByIdx(comparisonCellX, comparisonCellY));
 //            }
 //        }
 //    }
 //    return new UpdateMapDataMessage(robotID, diffPositionToSend, heightList);
 //}
 /// <summary>
 /// Converts GlobalMapCell data into a occupancy grid for global map
 /// </summary>
 /// <param name="globalHeightMap">returned global height map</param>
 /// <param name="globalSigMap">returned global covariance map</param>
 /// <param name="globalPijMap">returned laser hitting (kinda) map</param>
 /// <param name="globalMapDataList">incoming message from network</param>
 public static void ConvertToOcGrid(ref OccupancyGrid2D globalHeightMap, ref OccupancyGrid2D globalSigMap, ref OccupancyGrid2D globalPijMap, ref OccupancyGrid2D laserHitMap, List<GlobalMapCell.GlobalMapCellData> globalMapDataList)
 {
     foreach (GlobalMapCell.GlobalMapCellData cell in globalMapDataList)
     {
         if (cell.Equals(globalMapDataList[0]))
             continue;
         int col = cell.colIdx; int row = cell.rowIdx;
         globalHeightMap.SetCellByIdx(col, row, cell.largeU);
         globalSigMap.SetCellByIdx(col, row, cell.largeSig);
         globalPijMap.SetCellByIdx(col, row, cell.pij);
         laserHitMap.SetCellByIdx(col, row, cell.laserHit);
     }
 }
Beispiel #13
0
 public void UpdateOG(OccupancyGrid2D og)
 {
     lock (ogLock)
     {
         this.og = og;
     }
 }
Beispiel #14
0
        public void UpdateObstacles(List<Polygon> obstacles, double r, out List<Polygon> bloated)
        {
            if (pose == null)
            {
                bloated = default(List<Polygon>);
                return;
            }

            lock (bloatedObstacleLock)
            {
                bloatedObstacles = new List<Polygon>();

                foreach (Polygon p in obstacles)
                    bloatedObstacles.Add(Polygon.ConvexMinkowskiConvolution(Polygon.VehiclePolygonWithRadius(r), Polygon.GrahamScan(p.points, 1e-3)));
            }

            bloated = bloatedObstacles;

            // Following is deprecated for new height map planning

            Bitmap bmp = new Bitmap((int)Math.Round(2 * extentX / resX), (int)Math.Round(2 * extentY / resY));
            Graphics canvas = Graphics.FromImage(bmp);
            Pen pen = new Pen(Color.Blue, 1);
            PointF[] points;

            lock (bloatedObstacleLock)
            {
                foreach (Polygon p in bloatedObstacles)
                {
                    points = new PointF[p.points.Count];

                    for (int i = 0; i < p.points.Count; i++)
                    {
                        points[i] = new PointF((float)(p.points.ElementAt(i).X / resX + extentX / resX),
                            (float)(p.points.ElementAt(i).Y / resY + extentY / resY));
                    }

                    canvas.DrawPolygon(pen, points);
                }
            }

            bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
            bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
            //bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
            //bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
            //bmp = Blur(bmp);
            //bmp = Blur(bmp);

            lock (ogLock)
            {
                og = new OccupancyGrid2D(resX, resY, extentX, extentY);
                og.SetCellsFast(bmp);//, pose, og, 10);
            }
        }
        public GaussianMixMapping(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover)
        {
            //currentLaserPoseToRobot = laserRelativePositionToRover;
            Matrix4 laser2RobotDCM = Matrix4.FromPose(laserRelativePositionToRover);
            laserToRobotDCM = new UMatrix(4, 4);
            robotToGlocalDCM = new UMatrix(4, 4);
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
                }
            }
            covRobotPose = new UMatrix(6, 6);
            covLaserPose = new UMatrix(6, 6);
            covLaserScan = new UMatrix(2, 2);

            pij_sum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            pu_hat = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            pu_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            psig_u_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            uhatGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            sigSqrGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            laserHit = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            normalisedPijSum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            #region Initial setup
            // initial setup
            // these matrices were obtained from Jon using Vicon with Pioneer and HOKUYO laser
            // these matrices need to be redefined
            // these matrices are in row-wise

            //double[] Qp_robot = new double[36]{0.0026,    0.0011,   -0.0008,   -0.0019,    0.0125,   -0.0047,
            //                      0.0011,    0.0082,   -0.0054,    0.0098,    0.0116,   -0.0330,
            //                     -0.0008,   -0.0054,    0.0132,   -0.0173,   -0.0154,    0.0115,
            //                     -0.0019,    0.0098,   -0.0173,    0.0542,    0.0076,   -0.0319,
            //                      0.0125,    0.0116,   -0.0154,    0.0076,    0.0812,   -0.0397,
            //                     -0.0047,   -0.0330,    0.0115,   -0.0319,   -0.0397,    0.1875};

            //double[] Qp_laser = new double[36]{0.0077, -0.0009, -0.0016, -0.0127, -0.0415, -0.0011,
            //                       -0.0009, 0.0051, -0.0013, -0.0035, 0.0045, 0.0197,
            //                       -0.0016, -0.0013, 0.0101, -0.0167, 0.0189, 0.0322,
            //                       -0.0127, -0.0035, -0.0167, 0.2669, -0.0548, -0.2940,
            //                       -0.0415, 0.0045, 0.0189, -0.0548, 0.8129, 0.3627,
            //                       -0.0011, 0.0197, 0.0322, -0.2940, 0.3627, 0.7474};

            double[] Qp_robot = new double[36]{0.1, 0, 0, 0, 0, 0,
                                                                                 0, 0.1, 0, 0, 0, 0,
                                                                                 0, 0, 0.1, 0, 0, 0,
                                                                                 0, 0, 0, 0.01, 0, 0,
                                                                                 0, 0, 0, 0, 0.01, 0,
                                                                                 0, 0, 0, 0, 0, 0.01};

            double[] Qp_laser = new double[36]{0.001, 0, 0, 0, 0, 0,
                                                                                 0, 0.001, 0, 0, 0, 0,
                                                                                 0, 0, 0.001, 0, 0, 0,
                                                                                 0, 0, 0, 0.0001, 0, 0,
                                                                                 0, 0, 0, 0, 0.0001, 0,
                                                                                 0, 0, 0, 0, 0, 0.0001};

            double[] Qr = new double[4] { .01 * .01, 0, 0, (0.1 * Math.PI / 180.0) * (0.1 * Math.PI / 180.0) };

            // assign to our UMatrix
            for (int i = 0; i < 6; i++)
            {
                for (int j = 0; j < 6; j++)
                {
                    covRobotPose[j, i] = Qp_robot[j * 6 + i];// *1e-6;
                    covLaserPose[j, i] = Qp_laser[j * 6 + i];// *1e-6;
                }
            }
            covLaserScan[0, 0] = Qr[0];
            covLaserScan[0, 1] = 0;
            covLaserScan[1, 0] = 0;
            covLaserScan[1, 1] = Qr[3];
            #endregion

            //JTpl = ComputeJacobian(currentLaserPoseToRobot.yaw, currentLaserPoseToRobot.pitch, currentLaserPoseToRobot.roll);

            //	int thetaDegIndex = (int)((p.RThetaPoint.theta * 180.0/Math.PI) * 2) + 180;
            int k = 0;
            for (double i = -90; i <= 90; i += .5)
            {
                sinLookup[k] = (Math.Sin(i * Math.PI / 180.0));
                cosLookup[k] = (Math.Cos(i * Math.PI / 180.0));
                k++;
            }

            // initialize arrays
            //pijToSend = new float[361 * rangeLong * rangeLong];
            //largeUToSend = new float[361 * rangeLong * rangeLong];
            //largeSigToSend = new float[361 * rangeLong * rangeLong];
            //colIdxChange = new int[361 * rangeLong * rangeLong];
            //rowIdxChange = new int[361 * rangeLong * rangeLong];

            indicesList = new List<Index>();
            heightList = new List<float>();
            covList = new List<float>();
            pijSumList = new List<float>();

            laser2RobotTransMatrixDictionary = new Dictionary<int, UMatrix>();
            jacobianLaserPoseDictionary = new Dictionary<int, UMatrix>();

            indicesDictionary = new Dictionary<Index, int>();
        }
        public GaussianMixMappingQ(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover, int cellToUpdate)
        {
            //currentLaserPoseToRobot = laserRelativePositionToRover;
            this.rangeToApply = cellToUpdate;
            Matrix4 laser2RobotDCM = Matrix4.FromPose(laserRelativePositionToRover);
            laserToRobotDCM = new UMatrix(4, 4);
            robotToGlocalDCM = new UMatrix(4, 4);
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
                }
            }
            covRobotPose = new UMatrix(6, 6);
            covLaserPose = new UMatrix(6, 6);
            covLaserScan = new UMatrix(2, 2);

            pij_sum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            pu_hat = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            pu_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            psig_u_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            uhatGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            sigSqrGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            laserHit = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            thresholdedHeightMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            resetCountMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            indexMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            this.numCellX = ocToUpdate.NumCellX;
            this.numCellY = ocToUpdate.NumCellY;
            #region Initial setup

            double[] Qp_robot = new double[36]{0.1, 0, 0, 0, 0, 0,
                                               0, 0.1, 0, 0, 0, 0,
                                               0, 0, 0.1, 0, 0, 0,
                                               0, 0, 0, 0.01, 0, 0,
                                               0, 0, 0, 0, 0.01, 0,
                                               0, 0, 0, 0, 0, 0.01};

            double[] Qp_laser = new double[36]{0.001, 0, 0, 0, 0, 0,
                                               0, 0.001, 0, 0, 0, 0,
                                               0, 0, 0.001, 0, 0, 0,
                                               0, 0, 0, 0.0001, 0, 0,
                                               0, 0, 0, 0, 0.0001, 0,
                                               0, 0, 0, 0, 0, 0.0001};

            double[] Qr = new double[4] { .01 * .01, 0, 0, (0.1 * Math.PI / 180.0) * (0.1 * Math.PI / 180.0) };

            // assign to our UMatrix
            for (int i = 0; i < 6; i++)
            {
                for (int j = 0; j < 6; j++)
                {
                    covRobotPose[j, i] = Qp_robot[j * 6 + i];// *1e-6;
                    covLaserPose[j, i] = Qp_laser[j * 6 + i];// *1e-6;
                }
            }
            covLaserScan[0, 0] = Qr[0];
            covLaserScan[0, 1] = 0;
            covLaserScan[1, 0] = 0;
            covLaserScan[1, 1] = Qr[3];
            #endregion

            int k = 0;

            sinLookupHokuyo = new double[682];
            cosLookupHokuyo = new double[682];
            for (double i = -120; i <= 120; i += 0.35190615835777126099706744868035)
            {
                sinLookupHokuyo[k] = (Math.Sin(i * Math.PI / 180.0));
                cosLookupHokuyo[k] = (Math.Cos(i * Math.PI / 180.0));
                k++;
            }
            laserHalfAngleHokuyo = 120;
            MAXRANGEHokuyo = 5.0;
            MINRANGEHokuyo = 0.5;

            k = 0;
            sinLookupSick = new double[361];
            cosLookupSick = new double[361];
            for (double i = -90; i <= 90; i += .5)
            {
                sinLookupSick[k] = (Math.Sin(i * Math.PI / 180.0));
                cosLookupSick[k] = (Math.Cos(i * Math.PI / 180.0));
                k++;
            }
            laserHalfAngleSick = 90;
            MAXRANGESick = 30.0;
            MINRANGESick = 0.5;

            hokuyoStartIdx = 100;
            hokuyoEndIdx = 500;

            indicesList = new List<Index>();
            heightList = new List<float>();
            covList = new List<float>();
            pijSumList = new List<float>();

            laser2RobotTransMatrixDictionary = new Dictionary<int, Dictionary<int, UMatrix>>();
            jacobianLaserPoseDictionary = new Dictionary<int, Dictionary<int, UMatrix>>();

            indicesDictionary = new Dictionary<Index, int>();
        }
        /// <summary>
        /// Returns arrays to send out through messaging
        /// </summary>
        /// <param name="largeUDiff"></param>
        /// <param name="largeSigDiff"></param>
        /// <param name="pijDiff"></param>
        /// <param name="colIdx"></param>
        /// <param name="rowIdx"></param>
        public void GetArraysToSend(out List<Index> indexList, out List<float> heightList, out List<float> covList, out List<float> pijSumList, out List<float> laserHit)
        {
            int index = 0;
            if (indicesDictionary == null)
            {
                indexList = new List<Index>();
                heightList = new List<float>();
                covList = new List<float>();
                pijSumList = new List<float>();
                laserHit = new List<float>();
                return;
            }
            else
            {
                indexList = new List<Index>(indicesDictionary.Count);
                heightList = new List<float>(indicesDictionary.Count);
                covList = new List<float>(indicesDictionary.Count);
                pijSumList = new List<float>(indicesDictionary.Count);
                laserHit = new List<float>(indicesDictionary.Count);
            }

            lock (locker)
            {
                foreach (KeyValuePair<Index, int> pair in indicesDictionary)
                {
                    indexList.Add(pair.Key);
                    heightList.Add((float)uhatGM.GetCellByIdxUnsafe(pair.Key.Col, pair.Key.Row));
                    //heightList.Add((float)thresholdedHeightMap.GetCellByIdxUnsafe(pair.Key.Col, pair.Key.Row));
                    covList.Add((float)sigSqrGM.GetCellByIdxUnsafe(pair.Key.Col, pair.Key.Row));
                    pijSumList.Add((float)pij_sum.GetCellByIdxUnsafe(pair.Key.Col, pair.Key.Row));
                    laserHit.Add((float)LaserHit.GetCellByIdxUnsafe(pair.Key.Col, pair.Key.Row));
                    indexMap.SetCellByIdx(pair.Key.Col, pair.Key.Row, 0.0); // reset the index map
                }
                indicesDictionary.Clear();

            }
        }
 /// <summary>
 /// this is a copy constructor. You can use deepcopy, but this is better.
 /// </summary>
 /// <param name="gridToCopy"></param>
 public OccupancyGrid2D(OccupancyGrid2D gridToCopy)
     : this(gridToCopy.resolutionX, gridToCopy.resolutionY, gridToCopy.extentX, gridToCopy.extentY)
 {
     //Copy the occupancyMap
     Array.Copy(gridToCopy.occupancyMap, this.occupancyMap, gridToCopy.occupancyMap.Length);
 }
 public string ToLog()
 {
     string toReturn = "";
     OccupancyGrid2D copy = new OccupancyGrid2D(this);
     for (int i = 0; i < NumCellY; i++)
     {
         for (int j = 0; j < NumCellX; j++)
         {
             toReturn += this.GetCellByIdx(j, i).ToString() + " ";
         }
         toReturn += "\n";
     }
     return toReturn;
 }