public LidarRenderer(string name, int sensorID, int robotID, SensorPose laserToBody)
{
this.name = name;
this.sensorID = sensorID;
this.robotID = robotID;
this.laserToBody = laserToBody;
}
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);
}
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;
}
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;
}
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);
}
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();
}
public SensorPose(SensorPose p) : base(p)
{
}
/// <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;
}
/// <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;
}
public SeptentrioUDP(NetworkAddress na, SensorPose toBodyTransform, int id)
{
this.ip = na.Address; this.port = na.Port;
this.toBodyTransform = toBodyTransform;
this.ID = id;
}
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>();
}
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
}
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];
}
}
}