示例#1
0
 public LidarRenderer(string name, int sensorID, int robotID, SensorPose laserToBody)
 {
     this.name = name;
     this.sensorID = sensorID;
     this.robotID = robotID;
     this.laserToBody = laserToBody;
 }
示例#2
0
 public LidarProxy()
 {
     lidarSensorPose = new SensorPose(0, 0, 0.5, 0, 0, 0, 0);
     addressProvider = new HardcodedAddressProvider();
     lidar = new SickLMS(addressProvider.GetAddressByName("Sick"), lidarSensorPose);
     lidar.ScanReceived += new EventHandler<ILidarScanEventArgs<ILidarScan<ILidar2DPoint>>>(lidar_ScanReceived);
 }
示例#3
0
        public VFHFollower(SensorPose sickPose, SensorPose hokuyoPose, double angularResolution, bool useHokuyo)
        {
            pose = null;

            this.useHokuyo = useHokuyo;

            this.sickPose = sickPose;
            sickDCM = Matrix4.FromPose(this.sickPose);

            if (this.useHokuyo)
            {
                this.hokuyoPose = hokuyoPose;
                hokuyoDCM = Matrix4.FromPose(this.hokuyoPose);
            }

            sickHistory = new List<Vector2[]>();
            hokuyoHistory = new List<Vector2[]>();

            this.angularResolution = angularResolution;
            numBuckets = (int) Math.Round(360 / this.angularResolution);
            trapped = false;
            kThreshold = (int) Math.Round(160 / this.angularResolution);

            rHistogram = new Double[numBuckets];
        }
 public SeptentrioSerial(string comPort, int baud, SensorPose sensorPose, ITimingProvider timingProvider)
 {
     rxSerial = new SerialPort(comPort, baud);
     foreach (string p in SerialPort.GetPortNames())
         Console.WriteLine(p);
     rxSerial.DataReceived += new SerialDataReceivedEventHandler(rxSerial_DataReceived);
     this.sensorPose = sensorPose;
     this.timingProvider = timingProvider;
 }
 public void SetSensorPose(SensorPose laserPose, bool forCamera)
 {
     laserToRobot = Matrix4.FromPose(laserPose);
     if (forCamera)
         laserToRobot[2, 3] = 0;
     //laserToRobot[0, 2] = laserPose.x;
     //laserToRobot[1, 2] = laserPose.y;
     //laserToRobot[2, 2] = 0;
     //laserToRobot[3, 3] = 1.0;
 }
示例#6
0
 public SensorCoverage(double fovDegrees, double range, SensorPose location, Color color, string id)
 {
     this.fov = fovDegrees * (Math.PI / 180.0);
     this.range = range;
     this.location = location;
     this.renderColor = color;
     this.id = id;
     //get the option by name
     //Type coverageOptionsType = typeof(SensorCoverageRenderOptions);
     //optionProperties = coverageOptionsType.GetProperties();
 }
        double thresholdHeight; // above 5 cm

        #endregion Fields

        #region Constructors

        public LidarTargetDetector(double thresholdHeight, SensorPose laserPose, bool forCamera)
        {
            this.thresholdHeight = thresholdHeight;
            laserToRobot = Matrix4.FromPose(laserPose);
            if (forCamera)
                laserToRobot[2, 3] = 0;
            //laserToRobot[0, 2] = laserPose.x;
            //laserToRobot[1, 2] = laserPose.y;
            //laserToRobot[2, 2] = laserPose.z;
            //laserToRobot[3, 3] = 1.0;
        }
示例#8
0
 public HokuyoURG(string port, int scannerId, bool useHokuyoTimestamp)
 {
     this.port = port;
     this.scannerId = scannerId;
     this.sensorPose = new SensorPose(0, 0, 0, 0, 0, 0, 0);
     this.useHokuyoTimestamp = useHokuyoTimestamp;
     if (!useHokuyoTimestamp)
     {
         INetworkAddressProvider addressProvider = new HardcodedAddressProvider();
         segway = new SegwayRMP50(addressProvider.GetAddressByName("SegwayFeedback"), addressProvider.GetAddressByName("SegwayControl"), false, 1);
         segway.Start();
         segway.WheelPositionUpdate += new EventHandler<TimestampedEventArgs<Magic.Common.Robots.IRobotTwoWheelStatus>>(segway_WheelPositionUpdate);
     }
 }
 private static Vector2 FindNeighborToCompare(List<ILidar2DPoint> filteredScan, SensorPose lidarPose, int initialIdx, double threshold)
 {
     double currentX = -filteredScan[initialIdx].RThetaPoint.R * Math.Sin(filteredScan[initialIdx].RThetaPoint.theta) - lidarPose.y;
     double currentY = filteredScan[initialIdx].RThetaPoint.R * Math.Cos(filteredScan[initialIdx].RThetaPoint.theta) + lidarPose.x;
     double neighborX = -filteredScan[filteredScan.Count - 1].RThetaPoint.R * Math.Sin(filteredScan[filteredScan.Count - 1].RThetaPoint.theta) - lidarPose.y;
     double neighborY = filteredScan[filteredScan.Count - 1].RThetaPoint.R * Math.Cos(filteredScan[filteredScan.Count - 1].RThetaPoint.theta) + lidarPose.x;
     Vector2 currentPt = new Vector2(currentX, currentY);
     for (int k = initialIdx; k < filteredScan.Count; k++)
     {
         neighborX = -filteredScan[k].RThetaPoint.R * Math.Sin(filteredScan[k].RThetaPoint.theta) - lidarPose.y;
         neighborY = filteredScan[k].RThetaPoint.R * Math.Cos(filteredScan[k].RThetaPoint.theta) + lidarPose.x;
         Vector2 neighborPt = new Vector2(neighborX, neighborY);
         if (currentPt.DistanceTo(neighborPt) > threshold)
             return neighborPt;
     }
     return new Vector2(neighborX, neighborY);
 }
示例#10
0
 public HokuyoURG(string port, int scannerId, SensorPose sensorPose, bool useHokuyoTimestamp)
 {
     this.port = port;
     this.scannerId = scannerId;
     this.sensorPose = sensorPose;
     this.useHokuyoTimestamp = useHokuyoTimestamp;
     if (!useHokuyoTimestamp)
     {
         INetworkAddressProvider addressProvider = new HardcodedAddressProvider();
         //segway = new SegwayRMP50(addressProvider.GetAddressByName("SegwayFeedback"), addressProvider.GetAddressByName("SegwayControl"), false, 1);
         //segway.Start();
         //segway.WheelPositionUpdate += new EventHandler<TimestampedEventArgs<Magic.Common.Robots.IRobotTwoWheelStatus>>(segway_WheelPositionUpdate);
         ILidar2D lidar = new SickLMS(addressProvider.GetAddressByName("Sick"), new SensorPose(), true);
         lidar.ScanReceived += new EventHandler<ILidarScanEventArgs<ILidarScan<ILidar2DPoint>>>(lidar_ScanReceived);
         lidar.Start();
     }
 }
        public GlobalGaussianMixMapQ(OccupancyGrid2D globalOcGrid, GenericMulticastClient<LidarFilterPackageMessage> lidarFilterClient)
        {
            addressProvider = new HardcodedAddressProvider();
            //lidarFilterPackageClient = new GenericMulticastClient<LidarFilterPackageMessage>(addressProvider.GetAddressByName("LidarFilterPackage"), new CSharpMulticastSerializer<LidarFilterPackageMessage>(true));

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

            robotIDToPose = new Dictionary<int, PoseFilterState>();
            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>>();
            otherRobotPolygon = new Dictionary<int, Polygon>();

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

            //lidarFilterPackageClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
            lidarFilterClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<LidarFilterPackageMessage>>(lidarFilterPackageClient_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
            localMapResponseServer = new GenericMulticastServer<UpdateMapDataMessage>(addressProvider.GetAddressByName("LocalMapResponse"), new CSharpMulticastSerializer<UpdateMapDataMessage>(true));
            localMapResponseServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));

            Thread t = new Thread(new ParameterizedThreadStart(UpdateGlobalMap));
            t.Start();
            //Thread t2 = new Thread(new ParameterizedThreadStart(SendGlobalUpdate));
            //t2.Start();
        }
示例#12
0
 public SensorPose(SensorPose p) : base(p)
 {
 }
示例#13
0
        /// <summary>
        /// Returns lidar scan projection into a image plane
        /// </summary>
        /// <param name="lidarScan"></param>
        /// <param name="cameraSize">"320x240" or "640x480"</param>
        /// <param name="camType">"Fire-i" or "FireFly"</param>
        /// <param name="camPose">Your camera pose relative to the lidar</param>
        /// <returns>List of pixel values for each lidar point</returns>
        public static List<Vector2> FindLidarProjection(ILidarScan<ILidar2DPoint> lidarScan, string cameraSize, string camType, SensorPose camPose)
        {
            Matrix DCM4D = new Matrix(4, 4, 1.0);
            double[] fc = new double[2];
            double[] cc = new double[2];
            if (cameraSize.Equals("320x240"))
            {
                //for 320 x 240 image with Unibrain Fire-i camera
                if (camType.Equals("Fire-i"))
                {
                    fc[0] = 384.4507; fc[1] = 384.1266;
                    cc[0] = 155.1999; cc[1] = 101.5641;
                }

                // Fire-Fly MV
                else if (camType.Equals("FireFly"))
                {
                    fc[0] = 345.26498; fc[1] = 344.99438;
                    cc[0] = 159.36854; cc[1] = 118.26944;
                }
            }
            else if (cameraSize.Equals("640x480"))
            {
                // for 640 x 480 image with Unibrain Fire-i camera
                if (camType.Equals("Fire-i"))
                {
                    fc[0] = 763.5805; fc[1] = 763.8337;
                    cc[0] = 303.0963; cc[1] = 215.9287;
                }
                // for Fire-Fly MV (Point Gray)
                else if (camType.Equals("FireFly"))
                {
                    fc[0] = 691.09778; fc[1] = 690.70187;
                    cc[0] = 324.07388; cc[1] = 234.22204;
                }
            }
            double alpha_c = 0;

            // camera matrix
            Matrix KK = new Matrix(fc[0], alpha_c * fc[0], cc[0], 0, fc[1], cc[1], 0, 0, 1);

            // update DCM for point transformation
            DCM4D[0, 3] = camPose.x; DCM4D[1, 3] = camPose.y; DCM4D[2, 3] = camPose.z;
            DCM4D[0, 0] = Math.Cos(camPose.yaw); DCM4D[1, 1] = Math.Cos(camPose.yaw);
            DCM4D[0, 1] = Math.Sin(camPose.yaw); DCM4D[1, 0] = -Math.Sin(camPose.yaw);
            List<Vector2> pixelList = new List<Vector2>(lidarScan.Points.Count);
            foreach (ILidar2DPoint pt in lidarScan.Points)
            {
                Matrix point = new Matrix(4, 1);
                point[0, 0] = -pt.RThetaPoint.ToVector4().Y;
                point[1, 0] = pt.RThetaPoint.ToVector4().X;
                point[2, 0] = pt.RThetaPoint.ToVector4().Z;
                point[3, 0] = 1;
                Matrix transPt = DCM4D * point;
                Matrix ptImgPlane = new Matrix(3, 1);
                ptImgPlane[0, 0] = transPt[0, 0] / transPt[1, 0];
                ptImgPlane[1, 0] = -transPt[2, 0] / transPt[1, 0];
                ptImgPlane[2, 0] = transPt[1, 0] / transPt[1, 0];
                ptImgPlane = KK * ptImgPlane;
                pixelList.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0]));
            }
            return pixelList;
        }
示例#14
0
        /// <summary>
        /// Update tracker with given input. It will do simple data association with given data (comparing distance between existing targets).
        /// </summary>
        /// <param name="uPixel">x-pixel in a camera coordinate</param>
        /// <param name="vPixel">y-pixel in a camera coordinate</param>
        /// <param name="range">laser range reading</param>
        /// <param name="currentPose"></param>
        /// <returns>target index = targetID</returns>
        public int Update(double uPixel, double vPixel, RobotPose currentPose, SensorPose lidarPose, TargetTypes type, ILidar2DPoint lidarPt, string cameraType, int numCamera)
        {
            Matrix zSCR, Xx2;
            double distance = 0;
            int targetIdx = 0;
            lock (locker)
            {
                // Compute X and Y position of the target based on the passed lidar point
                //Vector2 xyPos = TargetTrackingImpl.FindPosCoord(screenSize, range, uPixel, vPixel, currentPose, null);
                // this X Y coordinate = E N
                double yaw = currentPose.yaw - Math.PI / 2;
                double pitch = currentPose.pitch; double roll = currentPose.roll;
                Matrix R_ENU2R = new Matrix(Math.Cos(yaw), Math.Sin(yaw), 0, -Math.Sin(yaw), Math.Cos(yaw), 0, 0, 0, 1) *
                                            new Matrix(1, 0, 0, 0, Math.Cos(pitch), -Math.Sin(pitch), 0, Math.Sin(pitch), Math.Cos(pitch)) *
                                            new Matrix(Math.Cos(roll), 0, Math.Sin(roll), 0, 1, 0, -Math.Sin(roll), 0, Math.Cos(roll));
                Matrix localPt = new Matrix(3, 1); localPt[0, 0] = -lidarPt.RThetaPoint.ToVector2().Y - lidarPose.y; localPt[1, 0] = lidarPt.RThetaPoint.ToVector2().X + lidarPose.x;
                Matrix globalPt = R_ENU2R.Inverse * localPt;
                Vector2 xyPos = new Vector2(globalPt[0, 0] + currentPose.x, globalPt[1, 0] + currentPose.y);
                this.xyPosList.Add(xyPos);
                #region debugging - go away
                // find the closest target
                //if (currentTargetPose.Count == 0)
                //{
                //    distance = double.MaxValue;
                //    targetIdx = 0;
                //}
                //else
                //{
                //    targetIdx = FindClosestTarget(xyPos, ref distance, type);
                //}

                //if (type == TargetTypes.PotentialSOOI || type == TargetTypes.ConfirmedSOOI)
                //{
                //    if (distance > staticDistanceThreshold) // if the distance is larger than the threshold, add as a new target
                //    {
                //        if (currentTargetPose.Count == currentTargetPose.Capacity)
                //            currentTargetPose.RemoveAt(0);
                //        currentTargetPose.Add(xyPos);

                //        implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
                //        targetIdx = currentTargetPose.Count - 1;
                //    }
                //}
                //else if (type == TargetTypes.PotentialMOOI || type == TargetTypes.ConfirmedMOOI)
                //{
                //    if (distance > dynamicDistanceThreshold) // if the distance is larger than the threshold, add as a new target
                //    {
                //        if (currentTargetPose.Count == currentTargetPose.Capacity)
                //            currentTargetPose.RemoveAt(0);
                //        currentTargetPose.Add(xyPos);

                //        implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
                //        targetIdx = currentTargetPose.Count - 1;
                //    }
                //}
                #endregion

                zSCR = new Matrix(3, 1);
                zSCR[0, 0] = uPixel; zSCR[1, 0] = vPixel; zSCR[2, 0] = lidarPt.RThetaPoint.R;
                Xx2 = new Matrix(9, 1);
                Xx2[0, 0] = currentPose.x; Xx2[1, 0] = currentPose.y; /* Xx2[2, 0] = currentPose.z; */ Xx2[2, 0] = 0;
                Xx2[3, 0] = currentPose.roll; Xx2[4, 0] = currentPose.pitch; Xx2[5, 0] = currentPose.yaw;
                //Xx2[3, 0] = 0; Xx2[4, 0] = 0; Xx2[5, 0] = currentPose.yaw;
                Xx2[6, 0] = 0; Xx2[7, 0] = 0; Xx2[8, 0] = 0;

                // modification for 3 cameras
                if (numCamera == 3)
                {
                    if (uPixel > 0 && uPixel <= 320)
                    {
                        Xx2[6, 0] = 50 * Math.PI / 180;
                    }
                    else if (uPixel > 320 && uPixel <= 320 * 2)
                    {
                        zSCR[0, 0] = uPixel - 320; zSCR[1, 0] = vPixel;
                        Xx2[6, 0] = 1.5 * Math.PI / 180;
                    }
                    else if (uPixel > 320 * 2)
                    {
                        zSCR[0, 0] = uPixel - 320 * 2; zSCR[1, 0] = vPixel;
                        Xx2[6, 0] = -47 * Math.PI / 180;
                    }
                }

                // find the most associated target
                Matrix Sxx2 = new Matrix(9, 9);
                Sxx2[0, 0] = Math.Sqrt(Math.Abs(currentPose.covariance[0, 0]));
                Sxx2[1, 1] = Math.Sqrt(Math.Abs(currentPose.covariance[1, 1]));
                Sxx2[2, 2] = Math.Sqrt(Math.Abs(currentPose.covariance[2, 2]));
                Sxx2[3, 3] = Math.Sqrt(0.01); // roll
                Sxx2[4, 4] = Math.Sqrt(0.01); // pitch
                Sxx2[5, 5] = Math.Sqrt(0.03); // yaw
                Sxx2[6, 6] = Math.Sqrt(0.01); // pan
                Sxx2[7, 7] = Math.Sqrt(0.01); // tilt
                Sxx2[8, 8] = Math.Sqrt(0.01); // scan

                //targetIdx = FindMostAssociatedTarget(xyPos, type, Xx2, Sxx2, zSCR);
                Matrix zSCRForTest = new Matrix(2, 1); zSCRForTest[0, 0] = lidarPt.RThetaPoint.R; zSCRForTest[1, 0] = lidarPt.RThetaPoint.theta;
                targetIdx = FindMostAssociatedTarget(xyPos, type, Xx2, Sxx2, zSCRForTest);
                if (type == TargetTypes.PotentialSOOI || type == TargetTypes.ConfirmedSOOI)
                {
                    if (targetIdx == -1) // if no good associated target found, then add a new one
                    {
                        implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
                        targetIdx = implList.Count - 1;
                    }
                }
                else if (type == TargetTypes.PotentialMOOI || type == TargetTypes.ConfirmedMOOI)
                {
                    if (targetIdx == -1)
                    {
                        implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
                        targetIdx = implList.Count - 1;
                    }
                }

                Matrix xhatPOI = new Matrix(7, 1); xhatPOI.Zero();
                xhatPOI[0, 0] = xyPos.X; xhatPOI[1, 0] = xyPos.Y; xhatPOI[2, 0] = 0;
                if (!implList[targetIdx].IsInitialized)
                    implList[targetIdx].SetInitialPOIInfo(xhatPOI);

            }

            Matrix Sx2 = new Matrix(9, 9);
            Sx2[0, 0] = Math.Sqrt(Math.Abs(currentPose.covariance[0, 0]));
            Sx2[1, 1] = Math.Sqrt(Math.Abs(currentPose.covariance[1, 1]));
            Sx2[2, 2] = Math.Sqrt(Math.Abs(currentPose.covariance[2, 2]));
            //Sx2[0, 0] = Math.Sqrt(0.1);
            //Sx2[1, 1] = Math.Sqrt(0.1);
            //Sx2[2, 2] = Math.Sqrt(0.01);
            Sx2[3, 3] = Math.Sqrt(0.01); // roll
            Sx2[4, 4] = Math.Sqrt(0.01); // pitch
            Sx2[5, 5] = Math.Sqrt(0.01); // yaw
            Sx2[6, 6] = Math.Sqrt(0.01); // pan
            Sx2[7, 7] = Math.Sqrt(0.01); // tilt
            Sx2[8, 8] = Math.Sqrt(0.01); // scan

            // update with correct target
            implList[targetIdx].UpdateZSCR(zSCR);
            implList[targetIdx].UpdateVehicleState(Xx2, currentPose.timestamp);
            implList[targetIdx].UpdateSx2(Sx2);
            if (implList[targetIdx].Type != TargetTypes.ConfirmedSOOI && implList[targetIdx].Type != TargetTypes.ConfirmedMOOI)
                implList[targetIdx].SetTargetType(type);
            implList[targetIdx].Update();
            return targetIdx;
        }
示例#15
0
 public SeptentrioUDP(NetworkAddress na, SensorPose toBodyTransform, int id)
 {
     this.ip = na.Address; this.port = na.Port;
     this.toBodyTransform = toBodyTransform;
     this.ID = id;
 }
示例#16
0
 public SickLMS(NetworkAddress na, SensorPose toBodyTransform, bool upsideDown)
 {
     this.ip = na.Address; this.port = na.Port; this.upsideDown = upsideDown;
     this.toBodyTransform = toBodyTransform;
 }
 /// <summary>
 /// Return X, Y coordinate of a pixel given Z and focal length information
 /// </summary>
 /// <param name="focalLength"></param>
 /// <param name="range"></param>
 /// <param name="pixel"></param>
 /// <returns></returns>
 public static Vector2 FindPosCoord(string screenSize, double range, double pixelX, double pixelY, RobotPose robotPose, SensorPose lidarPose)
 {
     double focalLength = 0;
     double centerPixX = 0;
     double centerPixY = 0;
     if (screenSize.Equals("320x240"))
     {
         focalLength = (384.4507 + 384.1266) / 2;
         centerPixX = 160; centerPixY = 120;
     }
     else if (screenSize.Equals("640x480"))
     {
         focalLength = (763.5805 + 763.8337) / 2;
         centerPixX = 320; centerPixY = 240;
     }
     else if (screenSize.Equals("960x240"))
     {
         focalLength = (345.26498 + 344.99438) / 2;
         centerPixX = 960 / 2; centerPixY = 480 / 2;
     }
     double angle = Math.PI / 2 - Math.Atan2(pixelX - centerPixX, focalLength);
     Matrix localPt = new Matrix(3, 1);
     localPt[0, 0] = range * Math.Cos(angle); localPt[1, 0] = range * Math.Sin(angle);
     double yaw = robotPose.yaw - Math.PI / 2; double pitch = robotPose.pitch; double roll = robotPose.roll;
     Matrix R_ENU2R = new Matrix(Math.Cos(yaw), Math.Sin(yaw), 0, -Math.Sin(yaw), Math.Cos(yaw), 0, 0, 0, 1) *
                                      new Matrix(1, 0, 0, 0, Math.Cos(pitch), -Math.Sin(pitch), 0, Math.Sin(pitch), Math.Cos(pitch)) *
                                      new Matrix(Math.Cos(roll), 0, Math.Sin(roll), 0, 1, 0, -Math.Sin(roll), 0, Math.Cos(roll));
     Matrix globalPt = R_ENU2R.Inverse * localPt;
     return new Vector2(globalPt[0, 0] + robotPose.x, globalPt[1, 0] + robotPose.y);
 }
        /// <summary>
        /// Update OccupancyGrid based on lidarScan and robotPose received
        /// </summary>
        /// <param name="lidarScan"></param>
        /// <param name="currentRobotPose"></param>
        public void UpdateOccupancyGrid(ILidarScan<ILidar2DPoint> lidarScan, int robotID, int scannerID, PoseFilterState currentRobotPose, SensorPose lidarPose, List<Polygon> dynamicObstacles)
        {
            if (robotID == 1)
            {
                MAXRANGESick = 7.0;
            }
            else if (robotID == 3)
            {
                MAXRANGESick = 30.0;
            }

            if (lidarPose == null)
            {
                lidarPose = new SensorPose(0, 0, 0.5, 0, 0 * Math.PI / 180.0, 0, 0);
            }
            if (laser2RobotTransMatrixDictionary.ContainsKey(robotID))
            {
                if (laser2RobotTransMatrixDictionary[robotID].ContainsKey(scannerID))
                {
                    JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
                    laserToRobotDCM = laser2RobotTransMatrixDictionary[robotID][scannerID];
                }
                else
                {
                    Matrix4 laser2RobotDCM = Matrix4.FromPose(lidarPose);
                    for (int i = 0; i < 4; i++)
                    {
                        for (int j = 0; j < 4; j++)
                        {
                            laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
                        }
                    }
                    laser2RobotTransMatrixDictionary[robotID].Add(scannerID, laserToRobotDCM);
                    jacobianLaserPoseDictionary[robotID].Add(scannerID, ComputeJacobian(lidarPose.yaw, lidarPose.pitch, lidarPose.roll));
                    JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
                }
            }
            else
            {
                laser2RobotTransMatrixDictionary.Add(robotID, new Dictionary<int, UMatrix>());
                jacobianLaserPoseDictionary.Add(robotID, new Dictionary<int, UMatrix>());
                Matrix4 laser2RobotDCM = Matrix4.FromPose(lidarPose);
                for (int i = 0; i < 4; i++)
                {
                    for (int j = 0; j < 4; j++)
                    {
                        laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
                    }
                }
                laser2RobotTransMatrixDictionary[robotID].Add(scannerID, new UMatrix(laserToRobotDCM));
                jacobianLaserPoseDictionary[robotID].Add(scannerID, ComputeJacobian(lidarPose.yaw, lidarPose.pitch, lidarPose.roll));
                JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
            }

            // calculate robot2global transformation matrix
            if (currentRobotPose == null) return;
            Matrix4 robot2GlocalDCM = Matrix4.FromPose(currentRobotPose);
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    robotToGlocalDCM[i, j] = robot2GlocalDCM[i, j];
                }
            }

            if (lidarScan == null) return;

            UMatrix JTpr = ComputeJacobianQ(currentRobotPose.q1, currentRobotPose.q2, currentRobotPose.q3, currentRobotPose.q4);
            List<UMatrix> JfPrCubixLaserToRobotDCM = new List<UMatrix>(6);
            List<UMatrix> RobotToGlocalDCMJfPlCubix = new List<UMatrix>(7);
            for (int i = 0; i < 7; i++)
            {
                //derivative of the robot transform matrtix w.r.t. some element of the robot psoe
                UMatrix j = new UMatrix(4, 4);
                j[0, 0] = JTpr[0, i]; j[1, 0] = JTpr[1, i]; j[2, 0] = JTpr[2, i]; j[3, 0] = JTpr[3, i];
                j[0, 1] = JTpr[4, i]; j[1, 1] = JTpr[5, i]; j[2, 1] = JTpr[6, i]; j[3, 1] = JTpr[7, i];
                j[0, 2] = JTpr[8, i]; j[1, 2] = JTpr[9, i]; j[2, 2] = JTpr[10, i]; j[3, 2] = JTpr[11, i];
                j[0, 3] = JTpr[12, i]; j[1, 3] = JTpr[13, i]; j[2, 3] = JTpr[14, i]; j[3, 3] = JTpr[15, i];
                JfPrCubixLaserToRobotDCM.Add(j * laserToRobotDCM);

                if (i == 7) continue; // same as break
                UMatrix tempJacobianPl = new UMatrix(4, 4);
                tempJacobianPl[0, 0] = JTpl[0, i]; tempJacobianPl[1, 0] = JTpl[1, i]; tempJacobianPl[2, 0] = JTpl[2, i]; tempJacobianPl[3, 0] = JTpl[3, i];
                tempJacobianPl[0, 1] = JTpl[4, i]; tempJacobianPl[1, 1] = JTpl[5, i]; tempJacobianPl[2, 1] = JTpl[6, i]; tempJacobianPl[3, 1] = JTpl[7, i];
                tempJacobianPl[0, 2] = JTpl[8, i]; tempJacobianPl[1, 2] = JTpl[9, i]; tempJacobianPl[2, 2] = JTpl[10, i]; tempJacobianPl[3, 2] = JTpl[11, i];
                tempJacobianPl[0, 3] = JTpl[12, i]; tempJacobianPl[1, 3] = JTpl[13, i]; tempJacobianPl[2, 3] = JTpl[14, i]; tempJacobianPl[3, 3] = JTpl[15, i];
                RobotToGlocalDCMJfPlCubix.Add(robotToGlocalDCM * tempJacobianPl);
            }
            UMatrix laserToENU = robotToGlocalDCM * laserToRobotDCM;
            //UMatrix pijCell = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);
            // update covariance
            UpdateCovarianceQ(currentRobotPose.Covariance);

            //SickPoint p = new SickPoint(new RThetaCoordinate(1.0f, 0.0f));
            for (int laserIndex = 0; laserIndex < lidarScan.Points.Count; laserIndex++)
            {
                lock (locker)
                {
                    ILidar2DPoint p = lidarScan.Points[laserIndex];
                    if (scannerID == 1)
                    {
                        if (p.RThetaPoint.R >= MAXRANGESick || p.RThetaPoint.R <= MINRANGESick)
                            continue;
                    }
                    else if (scannerID == 2)
                    {
                        if (p.RThetaPoint.R >= MAXRANGEHokuyo || p.RThetaPoint.R <= MINRANGEHokuyo || laserIndex < hokuyoStartIdx || laserIndex > hokuyoEndIdx)
                            continue;
                    }
                    bool hitDynamicObstacles = false;
                    // figure out if this lidar point is hitting other robot

                    // find laser points in 3D space
                    // first define 2D point on the laser plane
                    UMatrix laserPoint = new UMatrix(4, 1);

                    double deg = (p.RThetaPoint.theta * 180.0 / Math.PI);
                    int thetaDegIndex = 0;
                    if (scannerID == 2) // hokuyo
                        thetaDegIndex = (int)Math.Round((deg + laserHalfAngleHokuyo) * 2.84);// % 360;
                    else if (scannerID == 1) // sick
                        thetaDegIndex = (int)Math.Round((deg + laserHalfAngleSick) * 2) % 360;

                    double cosTheta = 0, sinTheta = 0;

                    if (scannerID == 1)
                    {
                        cosTheta = cosLookupSick[thetaDegIndex];
                        sinTheta = sinLookupSick[thetaDegIndex];
                    }
                    else if (scannerID == 2)
                    {
                        cosTheta = cosLookupHokuyo[thetaDegIndex];
                        sinTheta = sinLookupHokuyo[thetaDegIndex];
                    }

                    // initial laser points
                    laserPoint[0, 0] = p.RThetaPoint.R * cosTheta;
                    laserPoint[1, 0] = p.RThetaPoint.R * sinTheta;
                    laserPoint[2, 0] = 0;
                    laserPoint[3, 0] = 1;

                    //calculate r_hat_ENU
                    UMatrix r_hat_ENU = laserToENU * laserPoint;

                    foreach (Polygon dp in dynamicObstacles)
                    {
                        if (dp.IsInside(new Vector2(r_hat_ENU[0, 0], r_hat_ENU[1, 0])))
                        {
                            hitDynamicObstacles = true;
                            break;
                        }
                    }
                    if (hitDynamicObstacles) continue;

                    //-------------------------------//
                    // COVARIANCE UMatrix CALCULATION //
                    //-------------------------------//
                    UMatrix JRr = new UMatrix(4, 2);
                    JRr.Zero();
                    JRr[0, 0] = cosTheta;
                    JRr[0, 1] = -p.RThetaPoint.R * sinTheta;
                    JRr[1, 0] = sinTheta;
                    JRr[1, 1] = p.RThetaPoint.R * cosTheta;

                    UMatrix Jfr = new UMatrix(3, 2); // 3x2
                    Jfr = (laserToENU * JRr).Submatrix(0, 2, 0, 1);	 // 4x4 * 4x4 * 4x2

                    UMatrix Jfpr = new UMatrix(3, 7);
                    UMatrix Jfpl = new UMatrix(3, 6);

                    for (int i = 0; i < 7; i++) //for each state var (i.e. x,y,z,y,p,r)
                    {
                        UMatrix JfprTemp = (JfPrCubixLaserToRobotDCM[i]) * laserPoint; // 4 by 1 UMatrix
                        Jfpr[0, i] = JfprTemp[0, 0];
                        Jfpr[1, i] = JfprTemp[1, 0];
                        Jfpr[2, i] = JfprTemp[2, 0];

                        //UMatrix JfplTemp = (RobotToGlocalDCMJfPlCubix[i]) * laserPoint; // 4 by 1 UMatrix
                        //Jfpl[0, i] = JfplTemp[0, 0];
                        //Jfpl[1, i] = JfplTemp[1, 0];
                        //Jfpl[2, i] = JfplTemp[2, 0];
                    }
                    UMatrix JfprQprJfprT = new UMatrix(3, 3);
                    UMatrix JfplQplJfplT = new UMatrix(3, 3);
                    UMatrix JfrQrJfrT = new UMatrix(3, 3);
                    JfprQprJfprT = (Jfpr * covRobotPoseQ) * Jfpr.Transpose();
                    //JfplQplJfplT = (Jfpl * covLaserPose) * Jfpl.Transpose(); // not doing because covariance of laser point is so small
                    JfrQrJfrT = (Jfr * covLaserScan) * Jfr.Transpose();

                    // add above variables together and get the covariance
                    UMatrix covRHatENU = JfprQprJfprT + /*JfplQplJfplT +*/ JfrQrJfrT; // 3x3 UMatrix
                    //-----------------------------//
                    // FIND WHICH CELLS TO COMPUTE //
                    //-----------------------------//

                    // find out cells around this laser point
                    int laserPointIndexX = 0;
                    int laserPointIndexY = 0;
                    //this is used just to do the transformation from reaal to grid and visa versa
                    psig_u_hat_square.GetIndicies(r_hat_ENU[0, 0], r_hat_ENU[1, 0], out laserPointIndexX, out laserPointIndexY); // get cell (r_hat_ENU_X, r_hat_ENU_y)
                    if ((laserPointIndexX < 0 || laserPointIndexX >= numCellX) || (laserPointIndexY < 0 || laserPointIndexY >= numCellY))
                        continue;

                    int rangeToApplyX = (int)Math.Round(Math.Sqrt(covRHatENU[0, 0]) / (pij_sum.ResolutionX * 2)) * 2;
                    int rangeToApplyY = (int)Math.Round(Math.Sqrt(covRHatENU[1, 1]) / (pij_sum.ResolutionY * 2)) * 2;
                    //-----------------------------------------//
                    // COMPUTE THE DISTRIBUTION OF UNCERTAINTY //
                    //-----------------------------------------//
                    UMatrix pijCell = new UMatrix(rangeToApplyY * 2 + 1, rangeToApplyX * 2 + 1);

                    double sigX = Math.Sqrt(covRHatENU[0, 0]);
                    double sigY = Math.Sqrt(covRHatENU[1, 1]);
                    double rho = covRHatENU[1, 0] / (sigX * sigY);
                    double logTerm = Math.Log(2 * Math.PI * sigX * sigY * Math.Sqrt(1 - (rho * rho)));
                    double xTermDenom = (1 - (rho * rho));
                    //for (int i = -rangeToApply; i <= rangeToApply; i++) // row
                    for (int i = -rangeToApplyX; i <= rangeToApplyX; i++) // row
                    {
                        //for (int j = -rangeToApplyX; j <= rangeToApplyX; j++) // column
                        for (int j = -rangeToApplyY; j <= rangeToApplyY; j++) // column
                        {
                            if (laserPointIndexX + i < 0 || laserPointIndexX + i >= numCellX || laserPointIndexY + j < 0 || laserPointIndexY + j >= numCellY) continue;
                            // estimate using Bivariate Normal Distribution
                            double posX = 0; double posY = 0;
                            psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
                            posX += psig_u_hat_square.ResolutionX / 2;
                            posY += psig_u_hat_square.ResolutionY / 2;
                            double x = posX - r_hat_ENU[0, 0];
                            double y = posY - r_hat_ENU[1, 0];
                            double z = (x * x) / (sigX * sigX) -
                                                (2 * rho * x * y / (sigX * sigY)) +
                                                 (y * y) / (sigY * sigY);
                            double xTerm = -0.5 * z / xTermDenom;
                            // chi2 test
                            //if ((2 * (1 - (rho * rho))) * ((x * x) / (sigX * sigX) + (y * y) / (sigY * sigY) - (2 * rho * x * y) / (sigX * sigY)) > 15.2)
                            //  continue;
                            pijCell[j + rangeToApplyY, i + rangeToApplyX] = Math.Exp(xTerm - logTerm) * psig_u_hat_square.ResolutionX * psig_u_hat_square.ResolutionY;
                            laserHit.SetCellByIdx(laserPointIndexX + i, laserPointIndexY + j, laserHit.GetCellByIdxUnsafe(laserPointIndexX + i, laserPointIndexY + j) + 1);
                        }
                    }

                    //---------------------------//
                    // COMPUTE HEIGHT ESTIMATION //
                    //---------------------------//
                    UMatrix PEN = covRHatENU.Submatrix(0, 1, 0, 1);

                    UMatrix PENInv = PEN.Inverse2x2;

                    UMatrix PuEN = new UMatrix(1, 2);
                    UMatrix PENu = new UMatrix(2, 1);
                    UMatrix PuENPENInv = PuEN * PENInv;
                    UMatrix uHatMatrix = new UMatrix(rangeToApplyY * 2 + 1, rangeToApplyX * 2 + 1);
                    UMatrix sigUHatMatrix = new UMatrix(rangeToApplyY * 2 + 1, rangeToApplyX * 2 + 1);

                    PuEN[0, 0] = covRHatENU[2, 0];
                    PuEN[0, 1] = covRHatENU[2, 1];

                    PENu[0, 0] = covRHatENU[0, 2];
                    PENu[1, 0] = covRHatENU[1, 2];

                    double sig_u_hat_product = (PuENPENInv * PENu)[0, 0]; // output = 1x1 UMatrix

                    for (int i = -rangeToApplyX; i <= rangeToApplyX; i++) // row
                    {
                        for (int j = -rangeToApplyY; j <= rangeToApplyY; j++) // column
                        {
                            UMatrix ENmr_EN = new UMatrix(2, 1);
                            double posX = 0; double posY = 0;
                            psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
                            ENmr_EN[0, 0] = posX - r_hat_ENU[0, 0];
                            ENmr_EN[1, 0] = posY - r_hat_ENU[1, 0];
                            double u_hat_product = (PuENPENInv * (ENmr_EN))[0, 0]; // output = 1x1 UMatrix
                            uHatMatrix[j + rangeToApplyY, i + rangeToApplyX] = r_hat_ENU[2, 0] + u_hat_product;
                            sigUHatMatrix[j + rangeToApplyY, i + rangeToApplyX] = covRHatENU[2, 2] - sig_u_hat_product;
                        }
                    }

                    //-------------------------------------------//
                    // ASSIGN FINAL VALUES TO THE OCCUPANCY GRID //
                    //-------------------------------------------//
                    for (int i = -rangeToApplyX; i <= rangeToApplyX; i++)
                    {
                        for (int j = -rangeToApplyY; j <= rangeToApplyY; j++)
                        {
                            int indexXToUpdate = laserPointIndexX + i;
                            int indexYToUpdate = laserPointIndexY + j;
                            // if the cell to update is out of range, continue
                            if (!psig_u_hat_square.CheckValidIdx(indexXToUpdate, indexYToUpdate))
                                continue;

                            pij_sum.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApplyY, i + rangeToApplyX] + pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
                            pu_hat.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApplyY, i + rangeToApplyX] + pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
                            pu_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApply, i + rangeToApply] + pu_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
                            psig_u_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApplyY, i + rangeToApplyX] * sigUHatMatrix[j + rangeToApplyY, i + rangeToApplyX] + psig_u_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
                            uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                                            (pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate)));

                            double largeU = (pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
                            double largeSig = (psig_u_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) + pu_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate)) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) - largeU * largeU;
                            if (pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) > 1)
                                thresholdedHeightMap.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU);//pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / laserHit.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));

                            uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU);
                            //sigSqrGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU + 2 * Math.Sqrt(largeSig));

                            if (indexMap.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) != 1.0)
                            {
                                Index index = new Index(indexXToUpdate, indexYToUpdate);
                                indicesDictionary.Add(index, indicesDictionary.Count);
                                indexMap.SetCellByIdx(indexXToUpdate, indexYToUpdate, 1.0);
                            }
                        }
                    }

                } // end foreach

                //Console.WriteLine("1: " + sw1.ElapsedMilliseconds +
                //                                    " 2: " + sw2.ElapsedMilliseconds +
                //                                    " 3: " + sw3.ElapsedMilliseconds +
                //                                    " 4: " + sw4.ElapsedMilliseconds +
                //                                    " 5: " + sw5.ElapsedMilliseconds +
                //                                    " 6: " + sw6.ElapsedMilliseconds +
                //                                    " TOTAL: " + (sw1.ElapsedMilliseconds + sw2.ElapsedMilliseconds + sw3.ElapsedMilliseconds + sw4.ElapsedMilliseconds + sw5.ElapsedMilliseconds + sw6.ElapsedMilliseconds).ToString());
            } // end function
        }
 public void UpdateSensorPose(SensorPose p)
 {
     curLidarToBody = new SensorPose(p);
 }
 public GaussianMixMappingQ(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover)
     : this(ocToUpdate, laserRelativePositionToRover, 10)
 {
 }
        public static List<Vector2> ExtractGlobalFeature(ILidarScan<ILidar2DPoint> scan, SensorPose sensorPose, RobotPose currentPose, double wheelRadius)
        {
            Stopwatch sw = new Stopwatch();
            sw.Start();
            List<Vector2> features = new List<Vector2>();
            List<ILidar2DPoint> filteredScan = new List<ILidar2DPoint>();
            //foreach (ILidar2DPoint pt in scan.Points)
            for (int i = 0; i < scan.Points.Count; i++)
            {
                if (scan.Points[i].RThetaPoint.R > 0.20)
                    filteredScan.Add(scan.Points[i]);
            }

            Matrix4 laser2robotDCM = new Matrix4(1, 0, 0, 0,
                                                 0, Math.Cos(sensorPose.pitch), Math.Sin(sensorPose.pitch), 0,
                                                 0, -Math.Sin(sensorPose.pitch), Math.Cos(sensorPose.pitch), sensorPose.z,
                                                 0, 0, 0, 1);

            int neighbor = 3;
            for (int i = neighbor; i < filteredScan.Count - neighbor; i++)
            {
                // AGAIN, these codes are written respect to ENU coordinate frame
                double currentX = -filteredScan[i].RThetaPoint.R * Math.Sin(filteredScan[i].RThetaPoint.theta) - sensorPose.y;
                double currentY = filteredScan[i].RThetaPoint.R * Math.Cos(filteredScan[i].RThetaPoint.theta) + sensorPose.x;
                double lastX = -filteredScan[i - neighbor].RThetaPoint.R * Math.Sin(filteredScan[i - neighbor].RThetaPoint.theta) - sensorPose.y;
                double lastY = filteredScan[i - neighbor].RThetaPoint.R * Math.Cos(filteredScan[i - neighbor].RThetaPoint.theta) + sensorPose.x;
                double nextX = -filteredScan[i + neighbor].RThetaPoint.R * Math.Sin(filteredScan[i + neighbor].RThetaPoint.theta) - sensorPose.y;
                double nextY = filteredScan[i + neighbor].RThetaPoint.R * Math.Cos(filteredScan[i + neighbor].RThetaPoint.theta) + sensorPose.x;

                Matrix localPt = new Matrix(4, 1), lastLocalPt = new Matrix(4, 1), nextLocalPt = new Matrix(4, 1);
                localPt[0, 0] = currentX; localPt[1, 0] = currentY; localPt[3, 0] = 1;
                lastLocalPt[0, 0] = lastX; lastLocalPt[1, 0] = lastY; lastLocalPt[3, 0] = 1;
                nextLocalPt[0, 0] = nextX; nextLocalPt[1, 0] = nextY; nextLocalPt[3, 0] = 1;
                Vector4 currentPt = laser2robotDCM * localPt;
                Vector4 lastPt = laser2robotDCM * lastLocalPt;
                Vector4 nextPt = laser2robotDCM * nextLocalPt;

                // check if the z position of the laser return is higher than 10 cm of current z-state
                //if (currentPt.Z > currentPose.z + 0.1)
                //    features.Add(new Vector2(currentPt.X, currentPt.Y));

                // height comparison
                Vector2 neighborPt = FindNeighborToCompare(filteredScan, sensorPose, i, 0.1);
                Matrix neighbotPtM = new Matrix(4, 1);
                neighbotPtM[0, 0] = neighborPt.X; neighbotPtM[1, 0] = neighborPt.Y; neighbotPtM[3, 0] = 1;
                Vector4 neighborPtV = laser2robotDCM * neighbotPtM;

                double yaw = currentPose.yaw - Math.PI / 2;
                double heightDiff = neighborPtV.Z - currentPt.Z;
                if (heightDiff > 0.1)
                {
                    features.Add(new Vector2(neighborPtV.X * Math.Cos(yaw) - neighborPtV.Y * Math.Sin(yaw) + currentPose.x,
                                            neighborPtV.X * Math.Sin(yaw) + neighborPtV.Y * Math.Cos(yaw) + currentPose.y));
                }
                else if (heightDiff < -0.10)
                {
                    features.Add(new Vector2(currentPt.X * Math.Cos(yaw) - currentPt.Y * Math.Sin(yaw) + currentPose.x,
                                            currentPt.X * Math.Sin(yaw) + currentPt.Y * Math.Cos(yaw) + currentPose.y));
                }

            }
            Console.WriteLine("Jump(?) Extraction Time:" + sw.ElapsedMilliseconds);
            return features;
        }
 public void SetLidarPose(SensorPose p)
 {
     lock (dataLocker) { curLidarToBody = p; }
 }
        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>();
        }
 public LidarPolyExtractor(double threshold, RobotPose curRobotPose, SensorPose curLidarToBody)
 {
     this.curLidarToBody = curLidarToBody;
     this.curRobotPose = curRobotPose;
     this.threshold = threshold;
 }
        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>();
        }
示例#26
0
 public TSIPGPS(NetworkAddress na, SensorPose toBodyTransform)
 {
     this.ip = na.Address; this.port = na.Port;
     this.toBodyTransform = toBodyTransform;
 }
        /// <summary>
        /// Update OccupancyGrid based on lidarScan and robotPose received
        /// </summary>
        /// <param name="lidarScan"></param>
        /// <param name="currentRobotPose"></param>
        public void UpdateOccupancyGrid(ILidarScan<ILidar2DPoint> lidarScan, int robotID, RobotPose currentRobotPose, SensorPose lidarPose, List<Polygon> dynamicObstacles)
        {
            if (lidarPose == null)
            {
                lidarPose = new SensorPose(0, 0, 0.5, 0, 0 * Math.PI / 180.0, 0, 0);
            }
            if (laser2RobotTransMatrixDictionary.ContainsKey(robotID))
            {
                JTpl = jacobianLaserPoseDictionary[robotID];
                laserToRobotDCM = laser2RobotTransMatrixDictionary[robotID];
            }
            else
            {
                Matrix4 laser2RobotDCM = Matrix4.FromPose(lidarPose);
                for (int i = 0; i < 4; i++)
                {
                    for (int j = 0; j < 4; j++)
                    {
                        laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
                    }
                }
                laser2RobotTransMatrixDictionary.Add(robotID, laserToRobotDCM);
                jacobianLaserPoseDictionary.Add(robotID, ComputeJacobian(lidarPose.yaw, lidarPose.pitch, lidarPose.roll));
                JTpl = jacobianLaserPoseDictionary[robotID];
            }

            // calculate robot2global transformation matrix
            Matrix4 robot2GlocalDCM = Matrix4.FromPose(currentRobotPose);
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    robotToGlocalDCM[i, j] = robot2GlocalDCM[i, j];
                }
            }

            if (lidarScan == null) return;
            Stopwatch sw1 = new Stopwatch();
            Stopwatch sw2 = new Stopwatch();
            Stopwatch sw3 = new Stopwatch();
            Stopwatch sw4 = new Stopwatch();
            Stopwatch sw5 = new Stopwatch();
            Stopwatch sw6 = new Stopwatch();

            UMatrix JTpr = ComputeJacobian(currentRobotPose.yaw, currentRobotPose.pitch, currentRobotPose.roll);
            List<UMatrix> JfPrCubixLaserToRobotDCM = new List<UMatrix>(6);
            List<UMatrix> RobotToGlocalDCMJfPlCubix = new List<UMatrix>(6);
            for (int i = 0; i < 6; i++)
            {
                //derivative of the robot transform matrtix w.r.t. some element of the robot psoe
                UMatrix j = new UMatrix(4, 4);
                j[0, 0] = JTpr[0, i]; j[1, 0] = JTpr[1, i]; j[2, 0] = JTpr[2, i]; j[3, 0] = JTpr[3, i];
                j[0, 1] = JTpr[4, i]; j[1, 1] = JTpr[5, i]; j[2, 1] = JTpr[6, i]; j[3, 1] = JTpr[7, i];
                j[0, 2] = JTpr[8, i]; j[1, 2] = JTpr[9, i]; j[2, 2] = JTpr[10, i]; j[3, 2] = JTpr[11, i];
                j[0, 3] = JTpr[12, i]; j[1, 3] = JTpr[13, i]; j[2, 3] = JTpr[14, i]; j[3, 3] = JTpr[15, i];
                JfPrCubixLaserToRobotDCM.Add(j * laserToRobotDCM);

                UMatrix tempJacobianPl = new UMatrix(4, 4);
                tempJacobianPl[0, 0] = JTpl[0, i]; tempJacobianPl[1, 0] = JTpl[1, i]; tempJacobianPl[2, 0] = JTpl[2, i]; tempJacobianPl[3, 0] = JTpl[3, i];
                tempJacobianPl[0, 1] = JTpl[4, i]; tempJacobianPl[1, 1] = JTpl[5, i]; tempJacobianPl[2, 1] = JTpl[6, i]; tempJacobianPl[3, 1] = JTpl[7, i];
                tempJacobianPl[0, 2] = JTpl[8, i]; tempJacobianPl[1, 2] = JTpl[9, i]; tempJacobianPl[2, 2] = JTpl[10, i]; tempJacobianPl[3, 2] = JTpl[11, i];
                tempJacobianPl[0, 3] = JTpl[12, i]; tempJacobianPl[1, 3] = JTpl[13, i]; tempJacobianPl[2, 3] = JTpl[14, i]; tempJacobianPl[3, 3] = JTpl[15, i];
                RobotToGlocalDCMJfPlCubix.Add(robotToGlocalDCM * tempJacobianPl);
            }
            UMatrix laserToENU = robotToGlocalDCM * laserToRobotDCM;
            UMatrix pijCell = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);
            // update covariance
            UpdateCovariance(currentRobotPose.covariance);

            //SickPoint p = new SickPoint(new RThetaCoordinate(1.0f, 0.0f));
            for (int laserIndex = 0; laserIndex < lidarScan.Points.Count; laserIndex++)
            {
                lock (locker)
                {

                    ILidar2DPoint p = lidarScan.Points[laserIndex];
                    if (p.RThetaPoint.R >= MAXRANGE) continue;

                    bool hitDynamicObstacles = false;
                    // figure out if this lidar point is hitting other robot

                    // find laser points in 3D space
                    // first define 2D point on the laser plane
                    UMatrix laserPoint = new UMatrix(4, 1);

                    double deg = (p.RThetaPoint.theta * 180.0 / Math.PI);
                    int thetaDegIndex = (int)Math.Round((deg + 90.0) * 2.0) % 360;
                    double cosTheta = cosLookup[thetaDegIndex];
                    double sinTheta = sinLookup[thetaDegIndex];

                    laserPoint[0, 0] = p.RThetaPoint.R * cosTheta;
                    laserPoint[1, 0] = p.RThetaPoint.R * sinTheta;
                    laserPoint[2, 0] = 0;
                    laserPoint[3, 0] = 1;

                    //calculate r_hat_ENU
                    UMatrix r_hat_ENU = laserToENU * laserPoint;

                    foreach (Polygon dp in dynamicObstacles)
                    {
                        if (dp.IsInside(new Vector2(r_hat_ENU[0, 0], r_hat_ENU[1, 0])))
                        {
                            hitDynamicObstacles = true;
                            break;
                        }
                    }
                    if (hitDynamicObstacles) continue;

                    //-------------------------------//
                    // COVARIANCE UMatrix CALCULATION //
                    //-------------------------------//
                    UMatrix JRr = new UMatrix(4, 2);
                    JRr.Zero();
                    JRr[0, 0] = cosTheta;
                    JRr[0, 1] = -p.RThetaPoint.R * sinTheta;
                    JRr[1, 0] = sinTheta;
                    JRr[1, 1] = p.RThetaPoint.R * cosTheta;

                    UMatrix Jfr = new UMatrix(3, 2); // 3x2
                    Jfr = (laserToENU * JRr).Submatrix(0, 2, 0, 1);	 // 4x4 * 4x4 * 4x2

                    UMatrix Jfpr = new UMatrix(3, 6);
                    UMatrix Jfpl = new UMatrix(3, 6);
                    sw1.Reset();
                    sw1.Start();

                    for (int i = 0; i < 6; i++) //for each state var (i.e. x,y,z,y,p,r)
                    {
                        UMatrix JfprTemp = (JfPrCubixLaserToRobotDCM[i]) * laserPoint; // 4 by 1 UMatrix
                        Jfpr[0, i] = JfprTemp[0, 0];
                        Jfpr[1, i] = JfprTemp[1, 0];
                        Jfpr[2, i] = JfprTemp[2, 0];

                        //UMatrix JfplTemp = (RobotToGlocalDCMJfPlCubix[i]) * laserPoint; // 4 by 1 UMatrix
                        //Jfpl[0, i] = JfplTemp[0, 0];
                        //Jfpl[1, i] = JfplTemp[1, 0];
                        //Jfpl[2, i] = JfplTemp[2, 0];
                    }
                    sw1.Stop();
                    sw2.Reset();
                    sw2.Start();
                    UMatrix JfprQprJfprT = new UMatrix(3, 3);
                    UMatrix JfplQplJfplT = new UMatrix(3, 3);
                    UMatrix JfrQrJfrT = new UMatrix(3, 3);
                    JfprQprJfprT = (Jfpr * covRobotPose) * Jfpr.Transpose();
                    //JfplQplJfplT = (Jfpl * covLaserPose) * Jfpl.Transpose();
                    JfrQrJfrT = (Jfr * covLaserScan) * Jfr.Transpose();

                    // add above variables together and get the covariance
                    UMatrix covRHatENU = JfprQprJfprT + /*JfplQplJfplT +*/ JfrQrJfrT; // 3x3 UMatrix
                    sw2.Stop();
                    sw3.Reset();
                    sw3.Start();
                    //-----------------------------//
                    // FIND WHICH CELLS TO COMPUTE //
                    //-----------------------------//

                    // find out cells around this laser point
                    int laserPointIndexX = 0;
                    int laserPointIndexY = 0;
                    //this is used just to do the transformation from reaal to grid and visa versa
                    psig_u_hat_square.GetIndicies(r_hat_ENU[0, 0], r_hat_ENU[1, 0], out laserPointIndexX, out laserPointIndexY); // get cell (r_hat_ENU_X, r_hat_ENU_y)
                    sw3.Stop();
                    sw4.Reset();
                    sw4.Start();
                    //-----------------------------------------//
                    // COMPUTE THE DISTRIBUTION OF UNCERTAINTY //
                    //-----------------------------------------//

                    double sigX = Math.Sqrt(covRHatENU[0, 0]);
                    double sigY = Math.Sqrt(covRHatENU[1, 1]);
                    double rho = covRHatENU[1, 0] / (sigX * sigY);
                    double logTerm = Math.Log(2 * Math.PI * sigX * sigY * Math.Sqrt(1 - (rho * rho)));
                    double xTermDenom = (1 - (rho * rho));
                    for (int i = -rangeToApply; i <= rangeToApply; i++) // column
                    {
                        for (int j = -rangeToApply; j <= rangeToApply; j++) // row
                        {
                            // estimate using Bivariate Normal Distribution
                            double posX = 0; double posY = 0;
                            psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
                            posX += psig_u_hat_square.ResolutionX / 2;
                            posY += psig_u_hat_square.ResolutionY / 2;
                            double x = posX - r_hat_ENU[0, 0];
                            double y = posY - r_hat_ENU[1, 0];
                            double z = (x * x) / (sigX * sigX) -
                                                (2 * rho * x * y / (sigX * sigY)) +
                                                 (y * y) / (sigY * sigY);
                            double xTerm = -0.5 * z / xTermDenom;
                            // chi2 test
                            //if ((2 * (1 - (rho * rho))) * ((x * x) / (sigX * sigX) + (y * y) / (sigY * sigY) - (2 * rho * x * y) / (sigX * sigY)) > 15.2)
                            //  continue;
                            pijCell[j + rangeToApply, i + rangeToApply] = Math.Exp(xTerm - logTerm) * psig_u_hat_square.ResolutionX * psig_u_hat_square.ResolutionY;
                            laserHit.SetCellByIdx(laserPointIndexX + i, laserPointIndexY + j, laserHit.GetCellByIdx(laserPointIndexX + i, laserPointIndexY + j) + 1);
                        }
                    }
                    sw4.Stop();
                    sw5.Reset();
                    sw5.Start();

                    //---------------------------//
                    // COMPUTE HEIGHT ESTIMATION //
                    //---------------------------//
                    //				Matrix2 PEN = new Matrix2(covRHatENU[0, 0], covRHatENU[0, 1], covRHatENU[1, 0], covRHatENU[1, 1]);

                    UMatrix PEN = covRHatENU.Submatrix(0, 1, 0, 1);

                    UMatrix PENInv = PEN.Inverse2x2;

                    UMatrix PuEN = new UMatrix(1, 2);
                    UMatrix PENu = new UMatrix(2, 1);
                    UMatrix PuENPENInv = PuEN * PENInv;
                    UMatrix uHatMatrix = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);
                    UMatrix sigUHatMatrix = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);

                    PuEN[0, 0] = covRHatENU[2, 0];
                    PuEN[0, 1] = covRHatENU[2, 1];

                    PENu[0, 0] = covRHatENU[0, 2];
                    PENu[1, 0] = covRHatENU[1, 2];

                    double sig_u_hat_product = (PuENPENInv * PENu)[0, 0]; // output = 1x1 UMatrix

                    for (int i = -rangeToApply; i <= rangeToApply; i++) // column
                    {
                        for (int j = -rangeToApply; j <= rangeToApply; j++) // row
                        {
                            UMatrix ENmr_EN = new UMatrix(2, 1);
                            double posX = 0; double posY = 0;
                            psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
                            ENmr_EN[0, 0] = posX - r_hat_ENU[0, 0];
                            ENmr_EN[1, 0] = posY - r_hat_ENU[1, 0];
                            double u_hat_product = (PuENPENInv * (ENmr_EN))[0, 0]; // output = 1x1 UMatrix
                            uHatMatrix[j + rangeToApply, i + rangeToApply] = r_hat_ENU[2, 0] + u_hat_product;
                            sigUHatMatrix[j + rangeToApply, i + rangeToApply] = covRHatENU[2, 2] - sig_u_hat_product;
                        }
                    }
                    sw5.Stop();
                    sw6.Reset();
                    sw6.Start();

                    //-------------------------------------------//
                    // ASSIGN FINAL VALUES TO THE OCCUPANCY GRID //
                    //-------------------------------------------//
                    for (int i = -rangeToApply; i <= rangeToApply; i++)
                    {
                        for (int j = -rangeToApply; j <= rangeToApply; j++)
                        {
                            int indexXToUpdate = laserPointIndexX + i;
                            int indexYToUpdate = laserPointIndexY + j;
                            // if the cell to update is out of range, continue
                            if (!psig_u_hat_square.CheckValidIdx(indexXToUpdate, indexYToUpdate))
                            {
                                //Console.WriteLine("Laser points out of the occupancy grid map");
                                continue;
                            }

                            pij_sum.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApply, i + rangeToApply] + pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            pu_hat.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApply, i + rangeToApply] * uHatMatrix[j + rangeToApply, i + rangeToApply] + pu_hat.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            pu_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApply, i + rangeToApply] * uHatMatrix[j + rangeToApply, i + rangeToApply] * uHatMatrix[j + rangeToApply, i + rangeToApply] + pu_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            psig_u_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApply, i + rangeToApply] * sigUHatMatrix[j + rangeToApply, i + rangeToApply] + psig_u_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                                            (pu_hat.GetCellByIdx(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate)));
                            normalisedPijSum.SetCellByIdx(indexXToUpdate, indexYToUpdate, pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate) / laserHit.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            double largeU = (pu_hat.GetCellByIdx(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            double largeSig = (psig_u_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate) + pu_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate)) / pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate) - largeU * largeU;

                            uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU);
                            sigSqrGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeSig);

                            Index index = new Index(indexXToUpdate, indexYToUpdate);
                            if (!indicesDictionary.ContainsKey(index))
                                indicesDictionary.Add(index, indicesDictionary.Count);
                            /*
                            if (indicesDictionary.ContainsKey(index))
                            {
                                int indexOfIndices = indicesDictionary[index];
                                heightList[indexOfIndices] = (float)largeU;
                                covList[indexOfIndices] = (float)largeSig;
                                pijSumList[indexOfIndices] = (float)pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate);
                            }
                            else
                            {
                                indicesDictionary.Add(index, indicesDictionary.Count);
                                indicesList.Add(new Index(indexXToUpdate, indexYToUpdate));
                                heightList.Add((float)largeU);
                                covList.Add((float)largeSig);
                                pijSumList.Add((float)pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            }
                             */
                        }
                    }
                    sw6.Stop();

                } // end foreach

                //Console.WriteLine("1: " + sw1.ElapsedMilliseconds +
                //                                    " 2: " + sw2.ElapsedMilliseconds +
                //                                    " 3: " + sw3.ElapsedMilliseconds +
                //                                    " 4: " + sw4.ElapsedMilliseconds +
                //                                    " 5: " + sw5.ElapsedMilliseconds +
                //                                    " 6: " + sw6.ElapsedMilliseconds +
                //                                    " TOTAL: " + (sw1.ElapsedMilliseconds + sw2.ElapsedMilliseconds + sw3.ElapsedMilliseconds + sw4.ElapsedMilliseconds + sw5.ElapsedMilliseconds + sw6.ElapsedMilliseconds).ToString());
            } // end function
        }
示例#28
0
 public SensorPose(SensorPose p)
     : base(p)
 {
 }
 public void UpdateSensorPose(SensorPose sensorPose)
 {
     Matrix4 laser2RobotDCM = Matrix4.FromPose(sensorPose);
     for (int i = 0; i < 4; i++)
     {
         for (int j = 0; j < 4; j++)
         {
             laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
         }
     }
 }