public LidarPoseTargetMessage(int robotID, int targetID, RobotPose detectionPose, ILidarScan<ILidar2DPoint> scan)
{
this.robotID = robotID;
this.targetID = targetID;
this.detectionPose = detectionPose;
this.scan = scan;
}
public LidarFilterPackageMessage(int robotID, PoseFilterState state, SensorPose sensorPose, ILidarScan<ILidar2DPoint> scan)
{
this.robotID = robotID;
this.state = state;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
/// <summary>
/// Find targets above grounds and return in global and robot coordinate
/// </summary>
/// <param name="robotPose"></param>
/// <param name="laserScan"></param>
/// <param name="localPoints"></param>
/// <param name="globalPoints"></param>
public void DetectTarget(RobotPose robotPose, ILidarScan <ILidar2DPoint> laserScan, out List <Vector3> laserPoints, out List <Vector3> localPoints, bool lidarUpsideDown)
{
// initialization
localPoints = new List <Vector3>();
laserPoints = new List <Vector3>();
for (int i = 0; i < laserScan.Points.Count; i++)
{
Vector4 localPt = laserToRobot * laserScan.Points[i].RThetaPoint.ToVector4();
Vector4 newLocalPt;
if (lidarUpsideDown)
{
newLocalPt = new Vector4(-localPt.Y, localPt.X, localPt.Z, localPt.W);
}
else
{
newLocalPt = new Vector4(localPt.Y, localPt.X, localPt.Z, localPt.W);
}
localPoints.Add(new Vector3(newLocalPt.X, newLocalPt.Y, newLocalPt.Z));
if (lidarUpsideDown)
{
laserPoints.Add(new Vector3(laserScan.Points[i].RThetaPoint.R, laserScan.Points[i].RThetaPoint.theta, laserScan.Points[i].RThetaPoint.thetaDeg));
}
else
{
laserPoints.Add(new Vector3(laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.R,
laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.theta,
laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.thetaDeg));
}
}
}
public LidarPoseTargetMessage(int robotID, int targetID, RobotPose detectionPose, ILidarScan <ILidar2DPoint> scan)
{
this.robotID = robotID;
this.targetID = targetID;
this.detectionPose = detectionPose;
this.scan = scan;
}
public LidarPosePackageMessage(int robotID, RobotPose pose, SensorPose sensorPose, ILidarScan<ILidar2DPoint> scan)
{
this.robotID = robotID;
this.pose = pose;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
public void SetScan(ILidarScan <ILidar2DPoint> scan)
{
lock (this.drawLock)
{
this.scan = scan;
this.time = scan.Timestamp;
this.numPoints = scan.Points.Count;
}
}
public void SetScan(ILidarScan <ILidar2DPoint> scan, RobotPose robotPose, int startIdx, int endIdx)
{
lock (this.drawLock)
{
this.scan = scan;
this.time = scan.Timestamp;
this.numPoints = endIdx - startIdx + 1;
this.robotPose = robotPose;
this.startIdx = startIdx;
this.endIdx = endIdx;
}
}
public void SetScan(ILidarScan <ILidar2DPoint> scan, RobotPose robotPose)
{
lock (this.drawLock)
{
this.scan = scan;
this.time = scan.Timestamp;
this.numPoints = scan.Points.Count;
this.robotPose = robotPose;
this.startIdx = 0;
this.endIdx = scan.Points.Count - 1;
}
}
public TargetListNoImageMessage(int robotID, List<Vector2> pixelLTCorner, List<Vector2> pixelRBCorner, List<int> targetIDs, ILidarScan<ILidar2DPoint> lidarScan, double timeStamp)
{
int numTarget = targetIDs.Count;
//this.pixelLTCorner = new List<Vector2>(); this.pixelRBCorner = new List<Vector2>();
//for (int i = 0; i < numTarget; i++)
//{
// this.pixelLTCorner.Add(pixelLTCorner[i]);
// this.pixelRBCorner.Add(pixelRBCorner[i]);
//}
this.robotID = robotID;
this.pixelLTCorner = pixelLTCorner;
this.pixelRBCorner = pixelRBCorner;
this.timeStamp = timeStamp;
this.targetID = targetIDs;
this.lidarScan = lidarScan;
}
public void Handle2DLidarData(int robotID, ILidarScan <ILidar2DPoint> scan)
{
if (robots.ContainsKey(robotID) == false)
{
Console.WriteLine("warning: receiving lidar data for unknown robot : " + robotID); return;
}
if (scan.ScannerID == 1)
{
robots[robotID].LidarRendererFront.SetScan(scan);
}
if (scan.ScannerID == 0)
{
robots[robotID].LidarRendererFront.SetScan(scan);
}
}
public TargetListNoImageMessage(int robotID, List <Vector2> pixelLTCorner, List <Vector2> pixelRBCorner, List <int> targetIDs, ILidarScan <ILidar2DPoint> lidarScan, double timeStamp)
{
int numTarget = targetIDs.Count;
//this.pixelLTCorner = new List<Vector2>(); this.pixelRBCorner = new List<Vector2>();
//for (int i = 0; i < numTarget; i++)
//{
// this.pixelLTCorner.Add(pixelLTCorner[i]);
// this.pixelRBCorner.Add(pixelRBCorner[i]);
//}
this.robotID = robotID;
this.pixelLTCorner = pixelLTCorner;
this.pixelRBCorner = pixelRBCorner;
this.timeStamp = timeStamp;
this.targetID = targetIDs;
this.lidarScan = lidarScan;
}
private void ScanToTextFile(ILidarScan <ILidar2DPoint> LidarScan)
{
TextWriter Raw = new StreamWriter("C:\\Users\\labuser\\Desktop\\RawData.txt");
TextWriter Pts = new StreamWriter("C:\\Users\\labuser\\Desktop\\PointData.txt");
Raw.WriteLine();
Pts.WriteLine();
for (int i = 0; i < LidarScan.Points.Count; i++)
{
Raw.Write(LidarScan.Points[i].RThetaPoint.R);
Raw.Write(" ");
Raw.WriteLine(LidarScan.Points[i].RThetaPoint.theta);
Pts.WriteLine(LidarScan.Points[i].RThetaPoint.ToVector2().ToString());
}
Raw.Close();
Pts.Close();
}
public Vector2[] TransformToGlobal(ILidarScan <ILidar2DPoint> scan, Matrix4 sensorDcm)
{
Vector2[] globalScan = new Vector2[scan.Points.Count];
Vector4 point;
Matrix4 transformDcm = Matrix4.FromPose(pose) * sensorDcm;
int n = scan.Points.Count;
for (int i = 0; i < n; i++)
{
point = transformDcm * scan.Points[i].RThetaPoint.ToVector4();
if (point.Z >= 0.05)
{
globalScan[i] = new Vector2(point.X, point.Y);
}
}
return(globalScan);
}
/// <summary>
/// Method to fill object variables
/// </summary>
/// <param name="lidarscandata">lidar scan data in the form of a List of Vector2s</param>
public void RunDetector(ILidarScan <ILidar2DPoint> LidarScan, RobotPose pose)
{
//ScanToTextFile(LidarScan); //export scan data to analyze in matlab
List <Vector2> lidarscandata = new List <Vector2>(LidarScan.Points.Count);
for (int ii = 0; ii < LidarScan.Points.Count; ii++)
{
if (LidarScan.Points[ii].RThetaPoint.R < rangetoconsider)
{
lidarscandata.Add(LidarScan.Points[ii].RThetaPoint.ToVector2(isSickUpsideDown));
}
}
if (isSickUpsideDown)
{
lidarscandata.Reverse();
}
polygons = ClusterScanData(lidarscandata);
//PolygonsToTextFile(); //export polygon data to analyze in matlab
AllGaps = FindAllGaps(polygons, pose);
CheckGaps(AllGaps);
}
/// <summary>
/// Find targets above grounds and return in global and robot coordinate
/// </summary>
/// <param name="robotPose"></param>
/// <param name="laserScan"></param>
/// <param name="localPoints"></param>
/// <param name="globalPoints"></param>
public void DetectTarget(RobotPose robotPose, ILidarScan<ILidar2DPoint> laserScan, out List<Vector3> laserPoints, out List<Vector3> localPoints, bool lidarUpsideDown)
{
// initialization
localPoints = new List<Vector3>();
laserPoints = new List<Vector3>();
for (int i = 0; i < laserScan.Points.Count; i++)
{
Vector4 localPt = laserToRobot * laserScan.Points[i].RThetaPoint.ToVector4();
Vector4 newLocalPt;
if (lidarUpsideDown)
newLocalPt = new Vector4(-localPt.Y, localPt.X, localPt.Z, localPt.W);
else
newLocalPt = new Vector4(localPt.Y, localPt.X, localPt.Z, localPt.W);
localPoints.Add(new Vector3(newLocalPt.X, newLocalPt.Y, newLocalPt.Z));
if (lidarUpsideDown)
laserPoints.Add(new Vector3(laserScan.Points[i].RThetaPoint.R, laserScan.Points[i].RThetaPoint.theta, laserScan.Points[i].RThetaPoint.thetaDeg));
else
laserPoints.Add(new Vector3(laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.R,
laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.theta,
laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.thetaDeg));
}
}
public void Handle2DLidarData(int robotID, ILidarScan<ILidar2DPoint> scan)
{
if (robots.ContainsKey(robotID) == false) { Console.WriteLine("warning: receiving lidar data for unknown robot : " + robotID); return; }
if (scan.ScannerID == 1) robots[robotID].LidarRendererFront.SetScan(scan);
if (scan.ScannerID == 0) robots[robotID].LidarRendererFront.SetScan(scan);
}
public Vector2[] TransformToGlobal(ILidarScan<ILidar2DPoint> scan, Matrix4 sensorDcm)
{
Vector2[] globalScan = new Vector2[scan.Points.Count];
Vector4 point;
Matrix4 transformDcm = Matrix4.FromPose(pose) * sensorDcm;
int n = scan.Points.Count;
for (int i = 0; i < n; i++)
{
point = transformDcm * scan.Points[i].RThetaPoint.ToVector4();
if (point.Z >= 0.05)
globalScan[i] = new Vector2(point.X, point.Y);
}
return globalScan;
}
public RobotTwoWheelCommand GetCommand(ILidarScan<ILidar2DPoint> LidarScan, int angleTolerance, double threshold, double vmax, double wmax, out List<Polygon> polys)
{
// convert laser data into polar direction and magnitude
//ScanToTextFile(LidarScan); //export scan data to analyze in matlab
hysteresismin = threshold * .75;
double[] magnitudes, betas, gammas;
int n = LidarScan.Points.Count;
magnitudes = new double[n];
betas = new double[n];
gammas = new double[n];
for (int ii = 0; ii < n; ii++)
{
if (LidarScan.Points[ii].RThetaPoint.R < dmax)
{
magnitudes[ii] = (a - Math.Pow(LidarScan.Points[ii].RThetaPoint.R, 2) * b);
if (LidarScan.Points[ii].RThetaPoint.R < Rbloat)
gammas[ii] = Math.PI / 2;
else
gammas[ii] = Math.Asin(Rbloat / LidarScan.Points[ii].RThetaPoint.R);
}
else
{
magnitudes[ii] = 0;
gammas[ii] = Math.Asin(Rbloat / dmax);
}
double betaii = LidarScan.Points[ii].RThetaPoint.thetaDeg;
while (betaii < 0) { betaii = betaii + 360; }
while (betaii > 360) { betaii = betaii - 360; }
betas[ii] = betaii;
}
// create polar histogram
double[] hk = new double[numk];
double[] angles = new double[numk]; //corresponding angle
for (int z = 0; z < numk; z++)
{
hk[z] = 0;
angles[z] = z * alpha;
}
for (int j = 0; j < magnitudes.Count(); j++)
{
//int kmin = (int)(((betas[j] - gammas[j] + 360) % 360) / alpha);
//int kmax = (int)(((betas[j] + gammas[j] + 360) % 360) / alpha);
int k = (int)(betas[j] / alpha);
//for (int k = kmin; k <= kmax; k++)
hk[k] = hk[k] + magnitudes[j];
}
for (int k = 0; k < numk; k++)
{
if (hkold[k] >= threshold && (hk[k] > hysteresismin && hk[k] < threshold))
hk[k] = hkold[k];
}
hkold = hk;
// smooth the histogram
double[] hkprime = Smoother(hk, 5);
/*for (int i = 0; i < numk; i++)
Console.WriteLine(angles[i] + "," + hk[i] + "," + hkprime[i]);
Console.WriteLine("END +++++++++++++++++++++++");*/
// Temporary building of polygons
//List<Vector2> points = new List<Vector2>();
polys = new List<Polygon>();
/*for (int i = 0; i < numk; i++)
{
points.Add(new Vector2(hkprime[i] * Math.Cos(angles[i] * Math.PI / 180), hkprime[i] * Math.Sin(angles[i] * Math.PI / 180)));
}
polys.Add(new Polygon(points));*/
// find candidate valleys
double goalangle = 180 * (goalpoint.ArcTan) / Math.PI;
ktarget = (int)(Math.Round(goalangle / alpha));
List<double[]> valleys = FindValleys(hkprime);
if (!trapped)
{
// pick out best candidate valley
double[] valley = PickBestValley(valleys);
// calculate steering angle
double steerangle = FindSteerAngle(valley); //degrees
// calculate speed commands
double vmin = 0; // (m/s)
//double vmax = .2; // (m/s)
//double wmax = 25; // (deg/s)
double w = 0;
double v = 0;
double steerwindow = angleTolerance;
if (Math.Abs(steerangle) > steerwindow)
{
w = wmax * Math.Sign(steerangle);
}
else
{
w = wmax * (steerangle / steerwindow);
}
/*points = new List<Vector2>();
points.Add(new Vector2(0, 0));
points.Add(new Vector2(threshold * 1.5 * Math.Cos(steerangle * Math.PI / 180), threshold * 1.5 * Math.Sin(steerangle * Math.PI / 180)));
polys.Add(new Polygon(points));
points = new List<Vector2>();
points.Add(new Vector2(0, 0));
points.Add(new Vector2(threshold * 1 * Math.Cos(goalangle * Math.PI / 180), threshold * 1 * Math.Sin(goalangle * Math.PI / 180)));
polys.Add(new Polygon(points));
points = new List<Vector2>();
for (int i = 0; i < 180; i++)
{
points.Add(new Vector2(threshold * Math.Cos(i * 2 * Math.PI / 180), threshold * Math.Sin(i * 2 * Math.PI / 180)));
}
polys.Add(new Polygon(points));*/
double hc = Math.Min(hk[0], threshold);
double vp = vmax * (1 - hc / threshold);
v = vp * (1 - Math.Abs(w) / wmax) + vmin;
double vcommand = v * 3.6509; // multiply by 3.6509 to get "segway units"
double wcommand = w * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec)
//RobotTwoWheelCommand command = new RobotTwoWheelCommand(0, 0);
RobotTwoWheelCommand command = new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand);
return command;
}
else // trapped (no valleys were found), so stop, or lift threshold
{
RobotTwoWheelCommand command = new RobotTwoWheelCommand(0, 0);
return command;
}
}
void lidar_ScanReceived(object sender, ILidarScanEventArgs <ILidarScan <ILidar2DPoint> > e)
{
currentData = e.Scan;
Messaging.lidarScanServer.SendUnreliably(new LidarScanMessage(Messaging.robotID, e.Scan));
}
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 UpdateLidarScan(ILidarScan<ILidar2DPoint> s)
{
lock (dataLocker)
{
Stopwatch sw = new Stopwatch();
sw.Start();
curScan2D = s;
if (curRobotPose != null && curScan2D != null /*&& curRobotPose.timestamp != priorTimeStamp && curRobotPose.timestamp != 0*/)
{
//Get the laser to body coordinate system (this is I for now)
Matrix4 Tlaser2body = Matrix4.FromPose(curLidarToBody);
//Get the body to global transformation
Matrix4 Tbody2global = Matrix4.FromPose(curRobotPose);
//Get a vector from the current lidar pose
Vector3 lidarInBody = new Vector3((double)curLidarToBody.x, (double)curLidarToBody.y, (double)curLidarToBody.z);
//Transform the sensor position in body coordinates to the sensor position in global coordinates
Vector3 lidarInGlobal = Tbody2global.TransformPoint(lidarInBody);
//Get the current grid indicies
int xLidarPoseIndex, yLidarPoseIndex;
currentOccupancyGrid.GetIndicies(lidarInGlobal.X, lidarInGlobal.Y, out xLidarPoseIndex, out yLidarPoseIndex);
//Find the cells corresponding to each LIDAR return and make a list of the cells that are clear from the sensor to that point
Dictionary<Vector2, Boolean> occupiedCellsThisScan = new Dictionary<Vector2, Boolean>(curScan2D.Points.Count());
Dictionary<Vector2, Boolean> clearCellsThisScan = new Dictionary<Vector2, Boolean>();
//Process each lidar return
foreach (ILidar2DPoint pt in curScan2D.Points)
{
if (pt.RThetaPoint.R < lidarMaxPracticalRange)
{
//Extract the lidar point in XYZ (laser coordinates)
Vector3 v3 = pt.RThetaPoint.ToVector3();
//Convert laser to body coordinate system
Vector3 vBody = Tlaser2body.TransformPoint(v3);
//Convert body to global cooridnate system
Vector3 vGlobal = Tbody2global.TransformPoint(vBody);
//Find the index of the laser return
int xLaserIndex, yLaserIndex;
currentOccupancyGrid.GetIndicies(vGlobal.X, vGlobal.Y, out xLaserIndex, out yLaserIndex);
//Add to the list of occupied cells
if (currentOccupancyGrid.CheckValidIdx(xLaserIndex, yLaserIndex))
{
occupiedCellsThisScan[new Vector2(xLaserIndex, yLaserIndex)] = true;
//occupiedCellsThisScan[new Vector2(xLaserIndex + 1, yLaserIndex)] = true;
//occupiedCellsThisScan[new Vector2(xLaserIndex, yLaserIndex - 1)] = true;
//occupiedCellsThisScan[new Vector2(xLaserIndex + 1, yLaserIndex - 1)] = true;
}
}
}
//Process each lidar return
foreach (ILidar2DPoint pt in curScan2D.Points)
{
if (pt.RThetaPoint.R < lidarMaxPracticalRange)
{
//Extract the lidar point in XYZ (laser coordinates)
Vector3 v3 = pt.RThetaPoint.ToVector3();
//Convert laser to body coordinate system
Vector3 vBody = Tlaser2body.TransformPoint(v3);
//Convert body to global cooridnate system
Vector3 vGlobal = Tbody2global.TransformPoint(vBody);
//Find the index of the laser return
int xLaserIndex, yLaserIndex;
currentOccupancyGrid.GetIndicies(vGlobal.X, vGlobal.Y, out xLaserIndex, out yLaserIndex);
//Ray trace between the two points performing the update
Raytrace(xLidarPoseIndex, yLidarPoseIndex, xLaserIndex, yLaserIndex, occupiedCellsThisScan, clearCellsThisScan);
}
}
//decay the whole grid
for (int i = 0; i < currentOccupancyGrid.NumCellX; i++)
{
for (int j = 0; j < currentOccupancyGrid.NumCellY; j++)
{
double value = currentOccupancyGrid.GetCellByIdx(i, j);
currentOccupancyGrid.SetCellByIdx(i, j, value *= gridDecay);
}
}
foreach (Vector2 cellIdx in occupiedCellsThisScan.Keys)
{
UpdateCellOccupied((int)cellIdx.X, (int)cellIdx.Y);
}
foreach (Vector2 cellIdx in clearCellsThisScan.Keys)
{
UpdateCellClear((int)cellIdx.X, (int)cellIdx.Y);
}
//Copy for the timestamp for the next iteration
priorTimeStamp = (double)curRobotPose.timestamp;
}
// Console.WriteLine("OG Took: " + sw.ElapsedMilliseconds);
}//lock
if (outputOccupancyGrid != null)
{
outputOccupancyGrid = (IOccupancyGrid2D)currentOccupancyGrid.DeepCopy();
if (curRobotPose != null)
if (NewGridAvailable != null) NewGridAvailable(this, new NewOccupancyGrid2DAvailableEventArgs(GetOccupancyGrid(), curRobotPose.timestamp));
}
}
static public double FindTargetDistance(ILidarScan <ILidar2DPoint> lidarScan, double u, double v, Dictionary <int, int> colorPix, Vector2 TLCorner, Vector2 RBCorner,
string cameraSize, string camType, RobotPose camPose, TargetTypes type, out ILidar2DPoint lidarPt, ref List <Vector2> ptInBox)
{
#region camera stuff
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);
#endregion
// 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);
List <ILidar2DPoint> lidarScanInBox = new List <ILidar2DPoint>();
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]));
if (ptImgPlane[0, 0] >= TLCorner.X && ptImgPlane[0, 0] <= RBCorner.X && ptImgPlane[1, 0] >= TLCorner.Y && ptImgPlane[1, 0] <= RBCorner.Y)
{
if (colorPix.Count > 0)
{
if (colorPix.ContainsKey((int)ptImgPlane[0, 0]) && colorPix[(int)ptImgPlane[0, 0]] == 255)
{
lidarScanInBox.Add(pt);
ptInBox.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0]));
}
}
else
{
lidarScanInBox.Add(pt);
ptInBox.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0]));
}
}
}
if (lidarScanInBox.Count == 0)
{
lidarPt = null;
return(-1);
}
lidarPt = FineTargetDistanceClusterBased(lidarScanInBox);
if (lidarPt == null)
{
return(-1);
}
return(lidarPt.RThetaPoint.R);
}
public void SetScan(ILidarScan<ILidar2DPoint> scan, RobotPose robotPose)
{
lock (this.drawLock)
{
this.scan = scan;
this.time = scan.Timestamp;
this.numPoints = scan.Points.Count;
this.robotPose = robotPose;
this.startIdx = 0;
this.endIdx = scan.Points.Count - 1;
}
}
void lidar_ScanReceived(object sender, ILidarScanEventArgs<ILidarScan<ILidar2DPoint>> e)
{
currentData = e.Scan;
Messaging.lidarScanServer.SendUnreliably(new LidarScanMessage(Messaging.robotID, e.Scan));
}
/// <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>
static public 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 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 LidarPosePackageMessage(int robotID, RobotPose pose, SensorPose sensorPose, ILidarScan <ILidar2DPoint> scan)
{
this.robotID = robotID;
this.pose = pose;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
public List<int> fExtract(ILidarScan<ILidar2DPoint> currentScan, double bAngle, double maxRange)
{
#region cluster basis variables
ibook = new List<int>();
List<Vector2> xCart = new List<Vector2>();
List<Vector2> currentScanXY = new List<Vector2>();
List<double> iNonzero = new List<double>();
List<double> tPulse = new List<double>();
List<double> rCart = new List<double>();
List<double> rCartXY = new List<double>();
List<double> rMap = new List<double>();
List<int> iDotCorners = new List<int>();
#endregion
#region cluster basis
for (int i = 0; i < currentScan.Points.Count; i++)
{
currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2());
rCartXY.Add(currentScan.Points[i].RThetaPoint.R);
if (currentScan.Points[i].RThetaPoint.R < maxRange)
{
xCart.Add(currentScanXY[i]);
ibook.Add(i);
tPulse.Add(currentScan.Points[i].RThetaPoint.theta);
rCart.Add(currentScan.Points[i].RThetaPoint.R);
}
}
iDotCorners = dotProductExtract(xCart, bAngle);
List<int> ibookTemp = new List<int>();
List<double> tPulseTemp = new List<double>();
List<double> rCartTemp = new List<double>();
for (int i = 0; i < iDotCorners.Count; i++)
{
ibookTemp.Add(ibook[iDotCorners[i]]);
tPulseTemp.Add(tPulse[iDotCorners[i]]);
rCartTemp.Add(rCart[iDotCorners[i]]);
}
ibook = new List<int>(ibookTemp);
tPulse = new List<double>(tPulseTemp);
rMap = new List<double>(rCartTemp);
#endregion
#region cluster train variables
List<bool> rPulse = new List<bool>();
List<bool> rState = new List<bool>();
List<int> iCluster = new List<int>();
List<Vector2> iClusterVec = new List<Vector2>();
List<Vector2> iClusterVecTemp = new List<Vector2>();
List<double> cMap = new List<double>();
List<int> stableClusters = new List<int>();
rPulse.Add(true);
rState.Add(true);
iCluster.Add(0);
#endregion
#region cluster train
for (int i = 1; i < rMap.Count - 1; i++)
{
rPulse.Add((Math.Abs(rMap[i] - rMap[i - 1]) < 1.0) && (Math.Abs(tPulse[i] - tPulse[i - 1]) < 5.0 * (Math.PI / 180.0)));
if (rPulse[i] == true)
rState.Add(rState[i - 1]);
else
{
rState.Add(!(rState[i - 1]));
iCluster.Add(i - 1);
iCluster.Add(i);
}
}
#endregion
#region stable (single-element) clusters
if (iCluster.Count > 1)
{
#region instantiate clusters
iCluster.Add(iDotCorners.Count - 1);
for (int i = 0; i < iCluster.Count / 2; i++)
iClusterVec.Add(new Vector2(iCluster[2 * i], iCluster[2 * i + 1]));
for (int i = 0; i < iClusterVec.Count; i++)
{
double nC = 0.0;
Vector2 qTemp = new Vector2();
qTemp.X = 0;
qTemp.Y = 0;
for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++)
{
qTemp = qTemp + currentScanXY[ibook[j]];
nC = nC + 1.0;
}
qTemp = qTemp / nC;
cMap.Add(Math.Sqrt(Math.Pow(qTemp.X, 2) + Math.Pow(qTemp.Y, 2)));
}
#endregion
#region propose stable clusters
iClusterVecTemp.Clear();
for (int i = 1; i < cMap.Count - 1; i++)
{
if ((cMap[i] > cMap[i - 1]) && (cMap[i] > cMap[i + 1]))
{ }
else
{
if (cMap[i] > 1.0)
iClusterVecTemp.Add(iClusterVec[i]);
}
}
iClusterVec = new List<Vector2>(iClusterVecTemp);
#endregion
#region cluster stability assessment
stableClusters.Clear();
for (int i = 0; i < iClusterVec.Count; i++)
{
#region shadow-test-1
double st1 = rCart[iDotCorners[(int)iClusterVec[i].X] - 1];
double nC = 0.0;
double rTemp = 0.0;
for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++)
{
rTemp = rTemp + rCart[iDotCorners[j]];
nC = nC + 1.0;
}
double st2 = rTemp / nC;
double st3 = rCart[iDotCorners[(int)iClusterVec[i].X] + 1];
Vector2 O = new Vector2(2.0, st2);
Vector2 u = new Vector2(1.0, st1);
Vector2 v = new Vector2(3.0, st3);
Vector2 stX = u + v - 2 * O;
#endregion
#region shadow-test-2
st1 = rCart[iDotCorners[(int)iClusterVec[i].Y] - 1];
nC = 0.0;
rTemp = 0.0;
for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++)
{
rTemp = rTemp + rCart[iDotCorners[j]];
nC = nC + 1.0;
}
st2 = rTemp / nC;
st3 = rCart[iDotCorners[(int)iClusterVec[i].Y] + 1];
O = new Vector2(2.0, st2);
u = new Vector2(1.0, st1);
v = new Vector2(3.0, st3);
Vector2 stY = u + v - 2 * O;
#endregion
if (Math.Atan2(stX.Y, stX.X) > 0 && Math.Atan2(stY.Y, stY.X) > 0)
stableClusters.Add(i);
}
iClusterVecTemp.Clear();
for (int i = 0; i < stableClusters.Count; i++)
iClusterVecTemp.Add(iClusterVec[stableClusters[i]]);
iClusterVec = new List<Vector2>(iClusterVecTemp);
#endregion
#region reject large clusters
iClusterVecTemp.Clear();
for (int i = 0; i < iClusterVec.Count; i++)
{
if (Math.Abs((int)iClusterVec[i].X - (int)iClusterVec[i].Y) <= 1)
iClusterVecTemp.Add(iClusterVec[i]);
}
iClusterVec = new List<Vector2>(iClusterVecTemp);
ibookTemp.Clear();
if (ibook != null)
{
for (int i = 0; i < iClusterVec.Count; i++)
{
ibookTemp.Add(ibook[(int)iClusterVec[i].X]);
}
ibook = new List<int>(ibookTemp);
}
#endregion
}
else
{
ibook = null;
}
#endregion
return ibook;
}
/// <summary>
/// Method to fill object variables
/// </summary>
/// <param name="lidarscandata">lidar scan data in the form of a List of Vector2s</param>
public void RunDetector(ILidarScan<ILidar2DPoint> LidarScan, RobotPose pose)
{
//ScanToTextFile(LidarScan); //export scan data to analyze in matlab
List<Vector2> lidarscandata = new List<Vector2>(LidarScan.Points.Count);
for (int ii = 0; ii < LidarScan.Points.Count; ii++)
{
if (LidarScan.Points[ii].RThetaPoint.R < rangetoconsider)
lidarscandata.Add(LidarScan.Points[ii].RThetaPoint.ToVector2(isSickUpsideDown));
}
if (isSickUpsideDown)
lidarscandata.Reverse();
polygons = ClusterScanData(lidarscandata);
//PolygonsToTextFile(); //export polygon data to analyze in matlab
AllGaps = FindAllGaps(polygons, pose);
CheckGaps(AllGaps);
}
/// <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 void SetScan(ILidarScan<ILidar2DPoint> scan)
{
lock (this.drawLock)
{
this.scan = scan;
this.time = scan.Timestamp;
this.numPoints = scan.Points.Count;
}
}
public List <int> fExtract(ILidarScan <ILidar2DPoint> currentScan, double bAngle, double maxRange)
{
#region cluster basis variables
ibook = new List <int>();
List <Vector2> xCart = new List <Vector2>();
List <Vector2> currentScanXY = new List <Vector2>();
List <double> iNonzero = new List <double>();
List <double> tPulse = new List <double>();
List <double> rCart = new List <double>();
List <double> rCartXY = new List <double>();
List <double> rMap = new List <double>();
List <int> iDotCorners = new List <int>();
#endregion
#region cluster basis
for (int i = 0; i < currentScan.Points.Count; i++)
{
currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2());
rCartXY.Add(currentScan.Points[i].RThetaPoint.R);
if (currentScan.Points[i].RThetaPoint.R < maxRange)
{
xCart.Add(currentScanXY[i]);
ibook.Add(i);
tPulse.Add(currentScan.Points[i].RThetaPoint.theta);
rCart.Add(currentScan.Points[i].RThetaPoint.R);
}
}
iDotCorners = dotProductExtract(xCart, bAngle);
List <int> ibookTemp = new List <int>();
List <double> tPulseTemp = new List <double>();
List <double> rCartTemp = new List <double>();
for (int i = 0; i < iDotCorners.Count; i++)
{
ibookTemp.Add(ibook[iDotCorners[i]]);
tPulseTemp.Add(tPulse[iDotCorners[i]]);
rCartTemp.Add(rCart[iDotCorners[i]]);
}
ibook = new List <int>(ibookTemp);
tPulse = new List <double>(tPulseTemp);
rMap = new List <double>(rCartTemp);
#endregion
#region cluster train variables
List <bool> rPulse = new List <bool>();
List <bool> rState = new List <bool>();
List <int> iCluster = new List <int>();
List <Vector2> iClusterVec = new List <Vector2>();
List <Vector2> iClusterVecTemp = new List <Vector2>();
List <double> cMap = new List <double>();
List <int> stableClusters = new List <int>();
rPulse.Add(true);
rState.Add(true);
iCluster.Add(0);
#endregion
#region cluster train
for (int i = 1; i < rMap.Count - 1; i++)
{
rPulse.Add((Math.Abs(rMap[i] - rMap[i - 1]) < 1.0) && (Math.Abs(tPulse[i] - tPulse[i - 1]) < 5.0 * (Math.PI / 180.0)));
if (rPulse[i] == true)
{
rState.Add(rState[i - 1]);
}
else
{
rState.Add(!(rState[i - 1]));
iCluster.Add(i - 1);
iCluster.Add(i);
}
}
#endregion
#region stable (single-element) clusters
if (iCluster.Count > 1)
{
#region instantiate clusters
iCluster.Add(iDotCorners.Count - 1);
for (int i = 0; i < iCluster.Count / 2; i++)
{
iClusterVec.Add(new Vector2(iCluster[2 * i], iCluster[2 * i + 1]));
}
for (int i = 0; i < iClusterVec.Count; i++)
{
double nC = 0.0;
Vector2 qTemp = new Vector2();
qTemp.X = 0;
qTemp.Y = 0;
for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++)
{
qTemp = qTemp + currentScanXY[ibook[j]];
nC = nC + 1.0;
}
qTemp = qTemp / nC;
cMap.Add(Math.Sqrt(Math.Pow(qTemp.X, 2) + Math.Pow(qTemp.Y, 2)));
}
#endregion
#region propose stable clusters
iClusterVecTemp.Clear();
for (int i = 1; i < cMap.Count - 1; i++)
{
if ((cMap[i] > cMap[i - 1]) && (cMap[i] > cMap[i + 1]))
{
}
else
{
if (cMap[i] > 1.0)
{
iClusterVecTemp.Add(iClusterVec[i]);
}
}
}
iClusterVec = new List <Vector2>(iClusterVecTemp);
#endregion
#region cluster stability assessment
stableClusters.Clear();
for (int i = 0; i < iClusterVec.Count; i++)
{
#region shadow-test-1
double st1 = rCart[iDotCorners[(int)iClusterVec[i].X] - 1];
double nC = 0.0;
double rTemp = 0.0;
for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++)
{
rTemp = rTemp + rCart[iDotCorners[j]];
nC = nC + 1.0;
}
double st2 = rTemp / nC;
double st3 = rCart[iDotCorners[(int)iClusterVec[i].X] + 1];
Vector2 O = new Vector2(2.0, st2);
Vector2 u = new Vector2(1.0, st1);
Vector2 v = new Vector2(3.0, st3);
Vector2 stX = u + v - 2 * O;
#endregion
#region shadow-test-2
st1 = rCart[iDotCorners[(int)iClusterVec[i].Y] - 1];
nC = 0.0;
rTemp = 0.0;
for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++)
{
rTemp = rTemp + rCart[iDotCorners[j]];
nC = nC + 1.0;
}
st2 = rTemp / nC;
st3 = rCart[iDotCorners[(int)iClusterVec[i].Y] + 1];
O = new Vector2(2.0, st2);
u = new Vector2(1.0, st1);
v = new Vector2(3.0, st3);
Vector2 stY = u + v - 2 * O;
#endregion
if (Math.Atan2(stX.Y, stX.X) > 0 && Math.Atan2(stY.Y, stY.X) > 0)
{
stableClusters.Add(i);
}
}
iClusterVecTemp.Clear();
for (int i = 0; i < stableClusters.Count; i++)
{
iClusterVecTemp.Add(iClusterVec[stableClusters[i]]);
}
iClusterVec = new List <Vector2>(iClusterVecTemp);
#endregion
#region reject large clusters
iClusterVecTemp.Clear();
for (int i = 0; i < iClusterVec.Count; i++)
{
if (Math.Abs((int)iClusterVec[i].X - (int)iClusterVec[i].Y) <= 1)
{
iClusterVecTemp.Add(iClusterVec[i]);
}
}
iClusterVec = new List <Vector2>(iClusterVecTemp);
ibookTemp.Clear();
if (ibook != null)
{
for (int i = 0; i < iClusterVec.Count; i++)
{
ibookTemp.Add(ibook[(int)iClusterVec[i].X]);
}
ibook = new List <int>(ibookTemp);
}
#endregion
}
else
{
ibook = null;
}
#endregion
return(ibook);
}
public LidarScanMessage(int robotID, ILidarScan <ILidar2DPoint> scan)
{
this.scan = scan;
this.robotID = robotID;
}
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 RobotTwoWheelCommand GetCommand(ILidarScan <ILidar2DPoint> LidarScan, int angleTolerance, double threshold, double vmax, double wmax, out List <Polygon> polys)
{
// convert laser data into polar direction and magnitude
//ScanToTextFile(LidarScan); //export scan data to analyze in matlab
hysteresismin = threshold * .75;
double[] magnitudes, betas, gammas;
int n = LidarScan.Points.Count;
magnitudes = new double[n];
betas = new double[n];
gammas = new double[n];
for (int ii = 0; ii < n; ii++)
{
if (LidarScan.Points[ii].RThetaPoint.R < dmax)
{
magnitudes[ii] = (a - Math.Pow(LidarScan.Points[ii].RThetaPoint.R, 2) * b);
if (LidarScan.Points[ii].RThetaPoint.R < Rbloat)
{
gammas[ii] = Math.PI / 2;
}
else
{
gammas[ii] = Math.Asin(Rbloat / LidarScan.Points[ii].RThetaPoint.R);
}
}
else
{
magnitudes[ii] = 0;
gammas[ii] = Math.Asin(Rbloat / dmax);
}
double betaii = LidarScan.Points[ii].RThetaPoint.thetaDeg;
while (betaii < 0)
{
betaii = betaii + 360;
}
while (betaii > 360)
{
betaii = betaii - 360;
}
betas[ii] = betaii;
}
// create polar histogram
double[] hk = new double[numk];
double[] angles = new double[numk]; //corresponding angle
for (int z = 0; z < numk; z++)
{
hk[z] = 0;
angles[z] = z * alpha;
}
for (int j = 0; j < magnitudes.Count(); j++)
{
//int kmin = (int)(((betas[j] - gammas[j] + 360) % 360) / alpha);
//int kmax = (int)(((betas[j] + gammas[j] + 360) % 360) / alpha);
int k = (int)(betas[j] / alpha);
//for (int k = kmin; k <= kmax; k++)
hk[k] = hk[k] + magnitudes[j];
}
for (int k = 0; k < numk; k++)
{
if (hkold[k] >= threshold && (hk[k] > hysteresismin && hk[k] < threshold))
{
hk[k] = hkold[k];
}
}
hkold = hk;
// smooth the histogram
double[] hkprime = Smoother(hk, 5);
/*for (int i = 0; i < numk; i++)
* Console.WriteLine(angles[i] + "," + hk[i] + "," + hkprime[i]);
*
* Console.WriteLine("END +++++++++++++++++++++++");*/
// Temporary building of polygons
//List<Vector2> points = new List<Vector2>();
polys = new List <Polygon>();
/*for (int i = 0; i < numk; i++)
* {
* points.Add(new Vector2(hkprime[i] * Math.Cos(angles[i] * Math.PI / 180), hkprime[i] * Math.Sin(angles[i] * Math.PI / 180)));
* }
* polys.Add(new Polygon(points));*/
// find candidate valleys
double goalangle = 180 * (goalpoint.ArcTan) / Math.PI;
ktarget = (int)(Math.Round(goalangle / alpha));
List <double[]> valleys = FindValleys(hkprime);
if (!trapped)
{
// pick out best candidate valley
double[] valley = PickBestValley(valleys);
// calculate steering angle
double steerangle = FindSteerAngle(valley); //degrees
// calculate speed commands
double vmin = 0; // (m/s)
//double vmax = .2; // (m/s)
//double wmax = 25; // (deg/s)
double w = 0;
double v = 0;
double steerwindow = angleTolerance;
if (Math.Abs(steerangle) > steerwindow)
{
w = wmax * Math.Sign(steerangle);
}
else
{
w = wmax * (steerangle / steerwindow);
}
/*points = new List<Vector2>();
* points.Add(new Vector2(0, 0));
* points.Add(new Vector2(threshold * 1.5 * Math.Cos(steerangle * Math.PI / 180), threshold * 1.5 * Math.Sin(steerangle * Math.PI / 180)));
* polys.Add(new Polygon(points));
*
* points = new List<Vector2>();
* points.Add(new Vector2(0, 0));
* points.Add(new Vector2(threshold * 1 * Math.Cos(goalangle * Math.PI / 180), threshold * 1 * Math.Sin(goalangle * Math.PI / 180)));
* polys.Add(new Polygon(points));
*
* points = new List<Vector2>();
* for (int i = 0; i < 180; i++)
* {
* points.Add(new Vector2(threshold * Math.Cos(i * 2 * Math.PI / 180), threshold * Math.Sin(i * 2 * Math.PI / 180)));
* }
* polys.Add(new Polygon(points));*/
double hc = Math.Min(hk[0], threshold);
double vp = vmax * (1 - hc / threshold);
v = vp * (1 - Math.Abs(w) / wmax) + vmin;
double vcommand = v * 3.6509; // multiply by 3.6509 to get "segway units"
double wcommand = w * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec)
//RobotTwoWheelCommand command = new RobotTwoWheelCommand(0, 0);
RobotTwoWheelCommand command = new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand);
return(command);
}
else // trapped (no valleys were found), so stop, or lift threshold
{
RobotTwoWheelCommand command = new RobotTwoWheelCommand(0, 0);
return(command);
}
}
private void lidar_ScanReceived(object sender, ILidarScanEventArgs<ILidarScan<ILidar2DPoint>> e)
{
currentData = e.Scan;
}
void sick_ScanReceived(object sender, ILidarScanEventArgs<ILidarScan<ILidar2DPoint>> e)
{
scan = e.Scan;
BuildHistogram();
}
void SLAMEKFMainLoop(object o)
{
k = 0;
while (running)
{
lock (dataLock)
{
if (currentScan == null || viconPose == null || odomPose == null || viconLastPose == viconPose)
{
continue; // no data, no update
}
else
{
#region discrete-time increment
k = k + 1;
#endregion
if (k == 1)
{
#region state initialization
initialOdomPose = odomPose;
transformedOdomPose = new RobotPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odomPose.timestamp);
transformedOdomPose.x = (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x - (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.y = (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x + (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.yaw = odomPose.yaw;
transformedOdomPose = transformedOdomPose - initialOdomPose + initialViconPose;
odomLastPose = transformedOdomPose;
xhatvPose = transformedOdomPose;
#endregion
#region map initialization and management
#region extract features -> fz
List <Vector2> currentScanXY = new List <Vector2>();
fz.Clear();
ibook = fExtract(currentScan, bAngle, maxRange);
if (ibook != null)
{
for (int i = 0; i < currentScan.Points.Count; i++)
{
currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2());
}
for (int i = 0; i < ibook.Count; i++)
{
fz.Add(rotateTranslate(currentScanXY[ibook[i]], xhatvPose));
}
}
#endregion
#region populate the map -> landmarkList
landmarkList.Clear();
for (int i = 0; i < fz.Count; i++)
{
landmarkList.Add(fz[i]);
}
#endregion
#region populate the map -> mhat
mhat = new Matrix(2 * landmarkList.Count, 1);
for (int i = 0; i < landmarkList.Count; i++)
{
mhat[2 * i, 0] = landmarkList[i].X;
mhat[2 * i + 1, 0] = landmarkList[i].Y;
}
#endregion
#region covariance management -> Paug
Qv = new Matrix(3, 3, Math.Pow(sigw, 2));
Pv = Qv;
Paug = Pv;
for (int i = 0; i < fz.Count; i++)
{
Paug = blkdiag(Paug, new Matrix(2, 2, Math.Pow(sigw, 2)));
}
#endregion
lastScan = currentScan;
#endregion
}
else
{
#region odometry processing
transformedOdomPose = new RobotPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odomPose.timestamp);
transformedOdomPose.x = (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x - (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.y = (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x + (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.yaw = odomPose.yaw;
transformedOdomPose = transformedOdomPose - initialOdomPose + initialViconPose;
xStep = transformedOdomPose - odomLastPose;
double omega = odomLastPose.yaw + xStep.yaw;
if (xStep.x == 0)
{
uvk.x = 0;
}
else
{
double vx = xStep.x / Math.Cos(omega);
uvk.x = vx * Math.Cos(omega);
}
if (xStep.y == 0)
{
uvk.y = 0;
}
else
{
double vy = xStep.y / Math.Sin(omega);
uvk.y = vy * Math.Sin(omega);
}
uvk.yaw = xStep.yaw;
uvk.z = 0;
uvk.roll = 0;
uvk.pitch = 0;
uvk.timestamp = odomPose.timestamp;
odomLastPose = transformedOdomPose;
#endregion
#region time-update
xhatvPose = xhatvPose + uvk;
Matrix Uvk = new Matrix(3, 1);
Uvk[0, 0] = uvk.x;
Uvk[1, 0] = uvk.y;
Uvk[2, 0] = uvk.yaw;
Pv = Pv + ScalarMultiply(Math.Pow(0.5, 2), Uvk * Uvk.Transpose()) + Qv;
Paug.SetSubMatrix(0, 2, 0, 2, Pv);
Matrix Qmv = new Matrix(0, 0, 1.0);
for (int i = 0; i < landmarkList.Count; i++)
{
Qmv = blkdiag(Qmv, ScalarMultiply(Math.Pow(sigw / 5, 2), Pv.Submatrix(0, 1, 0, 1)));
}
Matrix Qm = new Matrix(Paug.Rows - 3, Paug.Columns - 3, Math.Pow(sigw / 5, 2));
Paug.SetSubMatrix(3, Paug.Rows - 1, 3, Paug.Columns - 1, Paug.Submatrix(3, Paug.Rows - 1, 3, Paug.Columns - 1) + Qmv + Qm);
#endregion
#region feature exraction
numLandmarks = landmarkList.Count;
List <Vector2> currentScanXY = new List <Vector2>();
fz.Clear();
ibook = fExtract(currentScan, bAngle, maxRange);
if (ibook != null)
{
for (int i = 0; i < currentScan.Points.Count; i++)
{
currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2());
}
for (int i = 0; i < ibook.Count; i++)
{
fz.Add(rotateTranslate(currentScanXY[ibook[i]], xhatvPose));
}
}
#endregion
#region data association
#region variables
fzHat = new List <Vector2>(landmarkList);
List <Vector3> jObsv = new List <Vector3>();
List <Vector2> iObsv = new List <Vector2>();
List <Vector2> obsvFeatureList = new List <Vector2>();
Matrix cLP = new Matrix(fz.Count, fzHat.Count);
Matrix dapGate = new Matrix(fz.Count, fzHat.Count);
Matrix fzM = new Matrix(2, 1);
Matrix fzHatM = new Matrix(2, 1);
#endregion
#region initialization
// "lpsolver55.dll" directory:
// (note: Alt + F7 -> Build -> enable "Allow UnSafe Code")
lpsolve.Init("C:\\users\\labuser\\desktop\\MAGIC\\Framework\\SLAM");
double[] xOpt;
double ignoreThisEntry = 0;
int nDecision = fz.Count * fzHat.Count;
int nConstraints = fz.Count + fzHat.Count;
int lp = lpsolve.make_lp((int)ignoreThisEntry, nDecision);
//lpsolve.set_sense(lp, true);
#endregion
#region c-vector (objective function)
double[] cObj = new double[nDecision + 1];
cObj[0] = ignoreThisEntry;
int ic = 0;
for (int i = 0; i < fz.Count; i++)
{
for (int j = 0; j < fzHat.Count; j++)
{
ic = ic + 1;
cObj[ic] = -1000000.0 + ciGate(fz[i], Paug.Submatrix(0, 1, 0, 1), fzHat[j], Paug.Submatrix(2 * j + 3, 2 * j + 4, 2 * j + 3, 2 * j + 4), 1.0);
cLP[i, j] = cObj[ic];
fzM[0, 0] = fz[i].X;
fzM[1, 0] = fz[i].Y;
fzHatM[0, 0] = fzHat[j].X;
fzHatM[1, 0] = fzHat[j].Y;
dapGate[i, j] = ((fzM - fzHatM).Transpose() * Paug.Submatrix(2 * j + 3, 2 * j + 4, 2 * j + 3, 2 * j + 4).Inverse *(fzM - fzHatM))[0, 0];
}
}
lpsolve.set_obj_fn(lp, cObj);
//lpsolve.set_timeout(lp, 0);
#endregion
#region b-vector (RHS of LE)
double[] bVec = new double[nConstraints];
for (int i = 0; i < bVec.Length; i++)
{
bVec[i] = 1.0;
}
#endregion
#region A-matrix (constraints setup)
double[,] A = new double[nConstraints, nDecision];
int jc = 0;
for (int i = 0; i < fz.Count; i++)
{
int jr = 1;
for (int j = 0; j < fzHat.Count; j++)
{
A[i, j + jc] = 1;
A[fz.Count - 1 + jr, j + jc] = 1;
jr = jr + 1;
}
jc = jc + fzHat.Count;
}
List <double[]> lpConstraints = new List <double[]>();
lpConstraints.Clear();
for (int i = 0; i < nConstraints; i++)
{
double[] Aline = new double[nDecision + 1];
Aline[0] = ignoreThisEntry;
for (int j = 1; j < nDecision + 1; j++)
{
Aline[j] = A[i, j - 1];
}
lpConstraints.Add(Aline);
}
for (int i = 0; i < nConstraints; i++)
{
lpsolve.add_constraint(lp, lpConstraints[i], lpsolve.lpsolve_constr_types.LE, bVec[i]);
}
#endregion
#region optimization and results
lpsolve.solve(lp);
xOpt = new double[lpsolve.get_Ncolumns(lp)];
lpsolve.get_variables(lp, xOpt);
lpsolve.delete_lp(lp);
ic = 0;
double tau = 6.63;
for (int i = 0; i < fz.Count; i++)
{
for (int j = 0; j < fzHat.Count; j++)
{
if ((xOpt[ic] > 0.98) && (xOpt[ic] < 1.02) && (dapGate[i, j] < tau))
{
jObsv.Add(new Vector3(i, j, cLP[i, j]));
}
ic = ic + 1;
}
}
if (jObsv.Count > 0)
{
for (int j = 0; j < jObsv.Count; j++)
{
iObsv.Add(new Vector2(jObsv[j].X, jObsv[j].Y));
obsvFeatureList.Add(rotateTranslate(currentScanXY[ibook[(int)(jObsv[j].X)]], xhatvPose)); // dev-only
}
}
numObsv = iObsv.Count;
#endregion
#endregion
#region measurement-update
if (numObsv > 0)
{
#region spf parameters
double L = 3 + 2 * landmarkList.Count;
double alpha = 1.0;
double kappa = 0.0;
double beta = 2.0;
double lambda = Math.Pow(alpha, 2) * (L + kappa) - L;
double gam = Math.Sqrt(L + lambda);
double wm0 = lambda / (L + lambda);
double wc0 = lambda / (L + lambda) + (1 - Math.Pow(alpha, 2) + beta);
wm = new Matrix((int)(2 * L) + 1, 1);
for (int j = 0; j < (int)(2 * L) + 1; j++)
{
if (j == 0)
{
wm[j, 0] = wm0;
}
else
{
wm[j, 0] = 1 / (2.0 * (L + lambda));
}
}
wc = new Matrix((int)(2 * L) + 1, 1);
for (int j = 0; j < (int)(2.0 * L) + 1; j++)
{
if (j == 0)
{
wc[j, 0] = wc0;
}
else
{
wc[j, 0] = 1 / (2.0 * (L + lambda));
}
}
#endregion
#region spf sampler
CholeskyDecomposition PCholContainer = new CholeskyDecomposition(ScalarMultiply(L + lambda, Paug));
Matrix PChol = PCholContainer.LeftTriangularFactor;
chi0 = new Matrix((int)L, 1);
chi0[0, 0] = xhatvPose.x - uvk.x;
chi0[1, 0] = xhatvPose.y - uvk.y;
chi0[2, 0] = xhatvPose.yaw - uvk.yaw;
for (int i = 0; i < mhat.Rows; i++)
{
chi0[3 + i, 0] = mhat[i, 0];
}
chi1 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
{
chi1.SetSubMatrix(0, chi1.Rows - 1, i, i, chi0 + PChol.Submatrix(0, chi0.Rows - 1, i, i));
}
chi2 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
{
chi2.SetSubMatrix(0, chi2.Rows - 1, i, i, chi0 - PChol.Submatrix(0, chi0.Rows - 1, i, i));
}
chi = chi0;
chi = chi.Concat(2, chi1);
chi = chi.Concat(2, chi2);
#endregion
#region spf time-update
Matrix chiBar = new Matrix((int)L, (int)(2 * L) + 1);
Matrix uvkSPF = new Matrix((int)L, 1);
uvkSPF.Zero();
uvkSPF[0, 0] = uvk.x;
uvkSPF[1, 0] = uvk.y;
uvkSPF[2, 0] = uvk.yaw;
for (int i = 0; i < (int)(2 * L) + 1; i++)
{
chiBar.SetSubMatrix(0, chiBar.Rows - 1, i, i, chi.Submatrix(0, chi.Rows - 1, i, i) + uvkSPF);
}
Matrix xBar = chiBar * wm;
Matrix PBar = new Matrix((int)L, (int)L);
PBar.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
{
PBar = PBar + ScalarMultiply(wc[i, 0], (chiBar.Submatrix(0, chiBar.Rows - 1, i, i) - xBar) * (chiBar.Submatrix(0, chiBar.Rows - 1, i, i) - xBar).Transpose());
}
#endregion
#region sigma-point update
CholeskyDecomposition PBarCholContainer = new CholeskyDecomposition(PBar);
Matrix PBarChol = PBarCholContainer.LeftTriangularFactor;
chi0 = xBar;
chi1 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
{
chi1.SetSubMatrix(0, chi1.Rows - 1, i, i, xBar + ScalarMultiply(gam, PBarChol.Submatrix(0, chi0.Rows - 1, i, i)));
}
chi2 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
{
chi2.SetSubMatrix(0, chi2.Rows - 1, i, i, xBar - ScalarMultiply(gam, PBarChol.Submatrix(0, chi0.Rows - 1, i, i)));
}
chiUpdate = chi0;
chiUpdate = chiUpdate.Concat(2, chi1);
chiUpdate = chiUpdate.Concat(2, chi2);
#endregion
#region spf measurement-update setup
#region predicted measurement
Matrix YBar = new Matrix(2 * numObsv, (int)(2 * L) + 1);
YBar.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
{
chiUpdateObsv = new Matrix((int)(2 * numObsv), 1);
for (int j = 0; j < numObsv; j++)
{
chiUpdateObsv[2 * j, 0] = chiUpdate[(int)(2 * iObsv[j].Y + 3), i];
chiUpdateObsv[2 * j + 1, 0] = chiUpdate[(int)(2 * iObsv[j].Y + 1 + 3), i];
}
YBar.SetSubMatrix(0, YBar.Rows - 1, i, i, hFunction(chiUpdate.Submatrix(0, 2, i, i), chiUpdateObsv));
}
Matrix yBar = YBar * wm;
#endregion
#region actual measurement
Matrix z = new Matrix(2 * numObsv, 1);
for (int i = 0; i < numObsv; i++)
{
z[2 * i, 0] = currentScan.Points[ibook[(int)(iObsv[i].X)]].RThetaPoint.R;
z[2 * i + 1, 0] = currentScan.Points[ibook[(int)(iObsv[i].X)]].RThetaPoint.theta;
}
#endregion
#region innovation covariance
S = new Matrix(2 * numObsv, 2 * numObsv);
S.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
{
S = S + ScalarMultiply(wc[i, 0], (YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar) *
(YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar).Transpose());
}
S = S + new Matrix(2 * numObsv, 2 * numObsv, Math.Pow(sigv, 2));
#endregion
#region Kalman gain matrix
W = new Matrix((int)L, 2 * numObsv);
W.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
{
W = W + ScalarMultiply(wc[i, 0], (chiUpdate.Submatrix(0, chiUpdate.Rows - 1, i, i) - xBar) *
(YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar).Transpose());
}
W = W * S.Inverse;
#endregion
#region measurement wrapper
zCart = new Matrix(2 * numObsv, 1);
for (int i = 0; i < numObsv; i++)
{
zCart[2 * i, 0] = z[2 * i, 0] * Math.Cos(z[2 * i + 1, 0]);
zCart[2 * i + 1, 0] = z[2 * i, 0] * Math.Sin(z[2 * i + 1, 0]);
}
for (int i = 0; i < numObsv; i++)
{
z[2 * i, 0] = Math.Sqrt(Math.Pow(zCart[2 * i, 0], 2) + Math.Pow(zCart[2 * i + 1, 0], 2));
z[2 * i + 1, 0] = Math.Atan2(zCart[2 * i + 1, 0], zCart[2 * i, 0]);
}
yBarCart = new Matrix(2 * numObsv, 1);
for (int i = 0; i < numObsv; i++)
{
yBarCart[2 * i, 0] = yBar[2 * i, 0] * Math.Cos(yBar[2 * i + 1, 0]);
yBarCart[2 * i + 1, 0] = yBar[2 * i, 0] * Math.Sin(yBar[2 * i + 1, 0]);
}
for (int i = 0; i < numObsv; i++)
{
yBar[2 * i, 0] = Math.Sqrt(Math.Pow(yBarCart[2 * i, 0], 2) + Math.Pow(yBarCart[2 * i + 1, 0], 2));
yBar[2 * i + 1, 0] = Math.Atan2(yBarCart[2 * i + 1, 0], yBarCart[2 * i, 0]);
}
#endregion
#endregion
#region spf measurement-update
//(((z - yBar).Transpose() * S * (z - yBar))[0, 0] < 6.63)
xBar = xBar + W * (z - yBar);
Paug = PBar - W * S * W.Transpose();
#region SLAM-context of measurement update
#region localization
xhatvPose.x = xBar[0, 0];
xhatvPose.y = xBar[1, 0];
xhatvPose.yaw = xBar[2, 0];
Pv = Paug.Submatrix(0, 2, 0, 2);
#endregion
#region mapping
mhat = xBar.Submatrix(3, (int)L - 1, 0, 0);
int landmarkListCount = landmarkList.Count;
landmarkList.Clear();
for (int i = 0; i < landmarkListCount; i++)
{
landmarkList.Add(new Vector2(mhat[2 * i, 0], mhat[2 * i + 1, 0]));
}
#endregion
#endregion
#endregion
}
#endregion
#region show features
slamFeatures.Clear();
if (ibook != null)
{
for (int i = 0; i < ibook.Count; i++)
{
slamFeatures.Add(fz[i]);
}
}
#endregion
#region map management
double minDist = 0.5;
double sigMapFreeze = 0.5;
int landmarkMaxCount = 20;
for (int i = 0; i < iObsv.Count; i++)
{
fz.RemoveAt((int)(iObsv[i].X - i));
}
for (int i = 0; i < fz.Count; i++)
{
int nFlag = 0;
for (int j = 0; j < landmarkList.Count; j++)
{
if ((fz[i] - landmarkList[j]).Length > minDist)
{
nFlag = nFlag + 1;
}
}
if ((nFlag == landmarkList.Count) && (Pv.Trace < sigMapFreeze) && (landmarkList.Count - 1 <= landmarkMaxCount))
{
landmarkList.Add(fz[i]);
Paug = blkdiag(Paug, new Matrix(2, 2, (Pv.Submatrix(0, 1, 0, 1)).Trace));
Matrix mHatNew = new Matrix(2, 1);
mHatNew[0, 0] = fz[i].X;
mHatNew[1, 0] = fz[i].Y;
mhat = mhat.Concat(1, mHatNew);
break; // add one feature at a time
}
}
#endregion
#region show landmarks
slamLandmarks.Clear();
slamLandmarksCovariance.Clear();
for (int i = 0; i < landmarkList.Count; i++)
{
slamLandmarks.Add(landmarkList[i]);
slamLandmarksCovariance.Add(Paug.Submatrix(2 * i + 3, 2 * i + 4, 2 * i + 3, 2 * i + 4));
}
#endregion
#region show correspondences
slamCorrespondences.Clear();
for (int i = 0; i < obsvFeatureList.Count; i++)
{
slamCorrespondences.Add(obsvFeatureList[i]);
}
#endregion
#region broadcast the results
viconLastPose = viconPose;
//filteredPose = transformedOdomPose;
filteredPose = xhatvPose;
lastScan = currentScan;
//covMatrix = Pv; // only vehicle xy-submatrix is used in the form
covMatrix = Pv; // only vehicle xy-submatrix is used in the form
//transformedOdomPose = viconPose;
#endregion
}
}
}
Thread.Sleep(100);
}
}
public void ClearBuffer()
{
scan = null;
}
/// <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
}
void sick_ScanReceived(object sender, ILidarScanEventArgs <ILidarScan <ILidar2DPoint> > e)
{
scan = e.Scan;
BuildHistogram();
}
/// <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 RobotTwoWheelCommand GetCommand(ILidarScan<ILidar2DPoint> sickScan, ILidarScan<ILidar2DPoint> hokuyoScan, int angleTolerance, double threshold, double dMax, double vMax, double wMax, out List<Polygon> polys)
{
double hysteresis = threshold * .9;
List<Double[]> histograms = new List<double[]>();
// Transform scan to global coordinate and add to queue
Vector2[] globalSickScan = TransformToGlobal(sickScan, sickDCM);
sickHistory.Add(globalSickScan);
histograms.Add(BuildHistogram(globalSickScan, dMax));
histograms.Add(BuildHistogram(sickHistory[0], dMax));
if (sickHistory.Count >= 30)
sickHistory.RemoveAt(0);
if (useHokuyo)
{
Vector2[] globalHokuyoScan = TransformToGlobal(hokuyoScan, hokuyoDCM);
hokuyoHistory.Add(globalHokuyoScan);
histograms.Add(BuildHistogram(globalHokuyoScan, dMax));
histograms.Add(BuildHistogram(hokuyoHistory[0], dMax));
if (hokuyoHistory.Count >= 30)
hokuyoHistory.RemoveAt(0);
}
Double[] histogram = CombinedHistogram(histograms);
for (int i = 0; i < numBuckets; i++)
{
if (rHistogram[i] >= threshold && histogram[i] > hysteresis && histogram[i] < threshold)
histogram[i] = rHistogram[i];
}
rHistogram = histogram;
histogram = Smooth(histogram, 10);
double goalAngle = 180 * (goalPoint.ArcTan) / Math.PI;
int targetBucket = (int)(Math.Round(goalAngle / angularResolution));
List<double[]> valleys = FindValleys(histogram, threshold);
polys = new List<Polygon>();
if (!trapped)
{
// pick out best candidate valley
double[] valley = PickBestValley(valleys, targetBucket);
// calculate steering angle
double steerAngle = FindSteerAngle(valley, targetBucket); //degrees
// calculate speed commands
double vmin = 0; // (m/s)
//double vmax = .2; // (m/s)
//double wmax = 25; // (deg/s)
double w = 0;
double v = 0;
if (Math.Abs(steerAngle) > angleTolerance)
{
w = wMax * Math.Sign(steerAngle);
}
else
{
w = wMax * (steerAngle / angleTolerance);
}
List<Vector2> points = new List<Vector2>();
for (int i = 0; i < numBuckets; i++)
{
points.Add(new Vector2(histogram[i] * Math.Cos(i * angularResolution * Math.PI / 180), histogram[i] * Math.Sin(i * angularResolution * Math.PI / 180)));
}
polys.Add(new Polygon(points));
points = new List<Vector2>();
points.Add(new Vector2(0, 0));
points.Add(new Vector2(threshold * 1.5 * Math.Cos(steerAngle * Math.PI / 180), threshold * 1.5 * Math.Sin(steerAngle * Math.PI / 180)));
polys.Add(new Polygon(points));
points = new List<Vector2>();
points.Add(new Vector2(0, 0));
points.Add(new Vector2(threshold * 1 * Math.Cos(goalAngle * Math.PI / 180), threshold * 1 * Math.Sin(goalAngle * Math.PI / 180)));
polys.Add(new Polygon(points));
points = new List<Vector2>();
for (int i = 0; i < 180; i++)
{
points.Add(new Vector2(threshold * Math.Cos(i * 2 * Math.PI / 180), threshold * Math.Sin(i * 2 * Math.PI / 180)));
}
polys.Add(new Polygon(points));
double hc = Math.Min(histogram[0], threshold);
double vp = vMax * (1 - hc / threshold);
v = vp * (1 - Math.Abs(w) / wMax) + vmin;
double vcommand = v * 3.6509; // multiply by 3.6509 to get "segway units"
double wcommand = w * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec)
return new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand);
}
else // trapped (no valleys were found), so stop, or lift threshold
return new RobotTwoWheelCommand(0, 0);
}
public RobotTwoWheelCommand GetCommand(ILidarScan <ILidar2DPoint> sickScan, ILidarScan <ILidar2DPoint> hokuyoScan, int angleTolerance, double threshold, double dMax, double vMax, double wMax, out List <Polygon> polys)
{
double hysteresis = threshold * .9;
List <Double[]> histograms = new List <double[]>();
// Transform scan to global coordinate and add to queue
Vector2[] globalSickScan = TransformToGlobal(sickScan, sickDCM);
sickHistory.Add(globalSickScan);
histograms.Add(BuildHistogram(globalSickScan, dMax));
histograms.Add(BuildHistogram(sickHistory[0], dMax));
if (sickHistory.Count >= 30)
{
sickHistory.RemoveAt(0);
}
if (useHokuyo)
{
Vector2[] globalHokuyoScan = TransformToGlobal(hokuyoScan, hokuyoDCM);
hokuyoHistory.Add(globalHokuyoScan);
histograms.Add(BuildHistogram(globalHokuyoScan, dMax));
histograms.Add(BuildHistogram(hokuyoHistory[0], dMax));
if (hokuyoHistory.Count >= 30)
{
hokuyoHistory.RemoveAt(0);
}
}
Double[] histogram = CombinedHistogram(histograms);
for (int i = 0; i < numBuckets; i++)
{
if (rHistogram[i] >= threshold && histogram[i] > hysteresis && histogram[i] < threshold)
{
histogram[i] = rHistogram[i];
}
}
rHistogram = histogram;
histogram = Smooth(histogram, 10);
double goalAngle = 180 * (goalPoint.ArcTan) / Math.PI;
int targetBucket = (int)(Math.Round(goalAngle / angularResolution));
List <double[]> valleys = FindValleys(histogram, threshold);
polys = new List <Polygon>();
if (!trapped)
{
// pick out best candidate valley
double[] valley = PickBestValley(valleys, targetBucket);
// calculate steering angle
double steerAngle = FindSteerAngle(valley, targetBucket); //degrees
// calculate speed commands
double vmin = 0; // (m/s)
//double vmax = .2; // (m/s)
//double wmax = 25; // (deg/s)
double w = 0;
double v = 0;
if (Math.Abs(steerAngle) > angleTolerance)
{
w = wMax * Math.Sign(steerAngle);
}
else
{
w = wMax * (steerAngle / angleTolerance);
}
List <Vector2> points = new List <Vector2>();
for (int i = 0; i < numBuckets; i++)
{
points.Add(new Vector2(histogram[i] * Math.Cos(i * angularResolution * Math.PI / 180), histogram[i] * Math.Sin(i * angularResolution * Math.PI / 180)));
}
polys.Add(new Polygon(points));
points = new List <Vector2>();
points.Add(new Vector2(0, 0));
points.Add(new Vector2(threshold * 1.5 * Math.Cos(steerAngle * Math.PI / 180), threshold * 1.5 * Math.Sin(steerAngle * Math.PI / 180)));
polys.Add(new Polygon(points));
points = new List <Vector2>();
points.Add(new Vector2(0, 0));
points.Add(new Vector2(threshold * 1 * Math.Cos(goalAngle * Math.PI / 180), threshold * 1 * Math.Sin(goalAngle * Math.PI / 180)));
polys.Add(new Polygon(points));
points = new List <Vector2>();
for (int i = 0; i < 180; i++)
{
points.Add(new Vector2(threshold * Math.Cos(i * 2 * Math.PI / 180), threshold * Math.Sin(i * 2 * Math.PI / 180)));
}
polys.Add(new Polygon(points));
double hc = Math.Min(histogram[0], threshold);
double vp = vMax * (1 - hc / threshold);
v = vp * (1 - Math.Abs(w) / wMax) + vmin;
double vcommand = v * 3.6509; // multiply by 3.6509 to get "segway units"
double wcommand = w * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec)
return(new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand));
}
else // trapped (no valleys were found), so stop, or lift threshold
{
return(new RobotTwoWheelCommand(0, 0));
}
}
private void ScanToTextFile(ILidarScan<ILidar2DPoint> LidarScan)
{
TextWriter Raw = new StreamWriter("C:\\Users\\labuser\\Desktop\\RawData.txt");
TextWriter Pts = new StreamWriter("C:\\Users\\labuser\\Desktop\\PointData.txt");
Raw.WriteLine();
Pts.WriteLine();
for (int i = 0; i < LidarScan.Points.Count; i++)
{
Raw.Write(LidarScan.Points[i].RThetaPoint.R);
Raw.Write(" ");
Raw.WriteLine(LidarScan.Points[i].RThetaPoint.theta);
Pts.WriteLine(LidarScan.Points[i].RThetaPoint.ToVector2().ToString());
}
Raw.Close();
Pts.Close();
}
public void UpdateLidarScan(ILidarScan <ILidar2DPoint> s)
{
lock (dataLocker)
{
Stopwatch sw = new Stopwatch();
sw.Start();
curScan2D = s;
if (curRobotPose != null && curScan2D != null /*&& curRobotPose.timestamp != priorTimeStamp && curRobotPose.timestamp != 0*/)
{
//Get the laser to body coordinate system (this is I for now)
Matrix4 Tlaser2body = Matrix4.FromPose(curLidarToBody);
//Get the body to global transformation
Matrix4 Tbody2global = Matrix4.FromPose(curRobotPose);
//Get a vector from the current lidar pose
Vector3 lidarInBody = new Vector3((double)curLidarToBody.x, (double)curLidarToBody.y, (double)curLidarToBody.z);
//Transform the sensor position in body coordinates to the sensor position in global coordinates
Vector3 lidarInGlobal = Tbody2global.TransformPoint(lidarInBody);
//Get the current grid indicies
int xLidarPoseIndex, yLidarPoseIndex;
currentOccupancyGrid.GetIndicies(lidarInGlobal.X, lidarInGlobal.Y, out xLidarPoseIndex, out yLidarPoseIndex);
//Find the cells corresponding to each LIDAR return and make a list of the cells that are clear from the sensor to that point
Dictionary <Vector2, Boolean> occupiedCellsThisScan = new Dictionary <Vector2, Boolean>(curScan2D.Points.Count());
Dictionary <Vector2, Boolean> clearCellsThisScan = new Dictionary <Vector2, Boolean>();
//Process each lidar return
foreach (ILidar2DPoint pt in curScan2D.Points)
{
if (pt.RThetaPoint.R < lidarMaxPracticalRange)
{
//Extract the lidar point in XYZ (laser coordinates)
Vector3 v3 = pt.RThetaPoint.ToVector3();
//Convert laser to body coordinate system
Vector3 vBody = Tlaser2body.TransformPoint(v3);
//Convert body to global cooridnate system
Vector3 vGlobal = Tbody2global.TransformPoint(vBody);
//Find the index of the laser return
int xLaserIndex, yLaserIndex;
currentOccupancyGrid.GetIndicies(vGlobal.X, vGlobal.Y, out xLaserIndex, out yLaserIndex);
//Add to the list of occupied cells
if (currentOccupancyGrid.CheckValidIdx(xLaserIndex, yLaserIndex))
{
occupiedCellsThisScan[new Vector2(xLaserIndex, yLaserIndex)] = true;
//occupiedCellsThisScan[new Vector2(xLaserIndex + 1, yLaserIndex)] = true;
//occupiedCellsThisScan[new Vector2(xLaserIndex, yLaserIndex - 1)] = true;
//occupiedCellsThisScan[new Vector2(xLaserIndex + 1, yLaserIndex - 1)] = true;
}
}
}
//Process each lidar return
foreach (ILidar2DPoint pt in curScan2D.Points)
{
if (pt.RThetaPoint.R < lidarMaxPracticalRange)
{
//Extract the lidar point in XYZ (laser coordinates)
Vector3 v3 = pt.RThetaPoint.ToVector3();
//Convert laser to body coordinate system
Vector3 vBody = Tlaser2body.TransformPoint(v3);
//Convert body to global cooridnate system
Vector3 vGlobal = Tbody2global.TransformPoint(vBody);
//Find the index of the laser return
int xLaserIndex, yLaserIndex;
currentOccupancyGrid.GetIndicies(vGlobal.X, vGlobal.Y, out xLaserIndex, out yLaserIndex);
//Ray trace between the two points performing the update
Raytrace(xLidarPoseIndex, yLidarPoseIndex, xLaserIndex, yLaserIndex, occupiedCellsThisScan, clearCellsThisScan);
}
}
//decay the whole grid
for (int i = 0; i < currentOccupancyGrid.NumCellX; i++)
{
for (int j = 0; j < currentOccupancyGrid.NumCellY; j++)
{
double value = currentOccupancyGrid.GetCellByIdx(i, j);
currentOccupancyGrid.SetCellByIdx(i, j, value *= gridDecay);
}
}
foreach (Vector2 cellIdx in occupiedCellsThisScan.Keys)
{
UpdateCellOccupied((int)cellIdx.X, (int)cellIdx.Y);
}
foreach (Vector2 cellIdx in clearCellsThisScan.Keys)
{
UpdateCellClear((int)cellIdx.X, (int)cellIdx.Y);
}
//Copy for the timestamp for the next iteration
priorTimeStamp = (double)curRobotPose.timestamp;
}
// Console.WriteLine("OG Took: " + sw.ElapsedMilliseconds);
}//lock
if (outputOccupancyGrid != null)
{
outputOccupancyGrid = (IOccupancyGrid2D)currentOccupancyGrid.DeepCopy();
if (curRobotPose != null)
{
if (NewGridAvailable != null)
{
NewGridAvailable(this, new NewOccupancyGrid2DAvailableEventArgs(GetOccupancyGrid(), curRobotPose.timestamp));
}
}
}
}
public List<Polygon> SegmentScan(ILidarScan<ILidar2DPoint> scan)
{
//this is super stupid, but may be functional for now....
//it may be necessary to sort these
List<Polygon> clusters = new List<Polygon> ();
List<Vector2> pts = new List<Vector2>(scan.Points.Count);
//Get the laser to body coordinate system (this is I for now)
Matrix4 Tlaser2body = Matrix4.FromPose(curLidarToBody);
//Get the body to global transformation
Matrix4 Tbody2global = Matrix4.FromPose(curRobotPose);
//Get a vector from the current lidar pose
Vector3 lidarInBody = new Vector3((double)curLidarToBody.x, (double)curLidarToBody.y, (double)curLidarToBody.z);
//Transform the sensor position in body coordinates to the sensor position in global coordinates
Vector3 lidarInGlobal = Tbody2global.TransformPoint(lidarInBody);
foreach (ILidar2DPoint pt in scan.Points)
{
if (pt.RThetaPoint.R < lidarMaxPracticalRange)
{
//Extract the lidar point in XYZ (laser coordinates)
Vector3 v3 = pt.RThetaPoint.ToVector3();
//Convert laser to body coordinate system
Vector3 vBody = Tlaser2body.TransformPoint(v3);
//Convert body to global cooridnate system
Vector3 vGlobal = Tbody2global.TransformPoint(vBody);
pts.Add(new Vector2 (vGlobal.X, vGlobal.Y));
}
}
//int slidingWindow = 5;
Polygon p = new Polygon ();
p.Add (pts[0]);
for (int i = 1; i < pts.Count; i++)
{
if (Math.Abs(pts[i].DistanceTo(pts[i - 1])) < threshold)
p.Add(pts[i]);
else
{
//create a new poly
clusters.Add(p);
p = new Polygon();
p.Add(pts[i]);
}
}
clusters.Add(p); //add the last polygon
Console.WriteLine("Got " + clusters.Count + " polys");
Polygon vehiclePolygon = VehiclePolygonWithRadius(vehicleRadius); // vehicle model of polygon. Note that radius is parameter
// Inside foreach, I cannot reassign eachpolygon. So, make a List<Polygon>, assign into this, and return this
List<Polygon> ret = new List<Polygon>(clusters.Count);
foreach (Polygon eachPolygon in clusters)
{
// Do the MinKowskiConvolution and GrahamScan to remove interior points
//Polygon convolutionPolygon = Polygon.ConvexMinkowskiConvolution(eachPolygon, vehiclePolygon);
Polygon convexHull;
if (eachPolygon.Count > 1)
{
convexHull = Polygon.GrahamScan(eachPolygon, .001);
}
else
{
convexHull = eachPolygon;
}
Polygon bloated = Polygon.ConvexMinkowskiConvolution(convexHull, vehiclePolygon);
//convexHull.Add(convexHull[0]);
ret.Add(bloated);
//convolutionPolygonList.Add(eachPolygon);
}
// convolutionPolygonList = DecomposePolygonList(convolutionPolygonList);
return ret;
}
public List <Polygon> SegmentScan(ILidarScan <ILidar2DPoint> scan)
{
//this is super stupid, but may be functional for now....
//it may be necessary to sort these
List <Polygon> clusters = new List <Polygon> ();
List <Vector2> pts = new List <Vector2>(scan.Points.Count);
//Get the laser to body coordinate system (this is I for now)
Matrix4 Tlaser2body = Matrix4.FromPose(curLidarToBody);
//Get the body to global transformation
Matrix4 Tbody2global = Matrix4.FromPose(curRobotPose);
//Get a vector from the current lidar pose
Vector3 lidarInBody = new Vector3((double)curLidarToBody.x, (double)curLidarToBody.y, (double)curLidarToBody.z);
//Transform the sensor position in body coordinates to the sensor position in global coordinates
Vector3 lidarInGlobal = Tbody2global.TransformPoint(lidarInBody);
foreach (ILidar2DPoint pt in scan.Points)
{
if (pt.RThetaPoint.R < lidarMaxPracticalRange)
{
//Extract the lidar point in XYZ (laser coordinates)
Vector3 v3 = pt.RThetaPoint.ToVector3();
//Convert laser to body coordinate system
Vector3 vBody = Tlaser2body.TransformPoint(v3);
//Convert body to global cooridnate system
Vector3 vGlobal = Tbody2global.TransformPoint(vBody);
pts.Add(new Vector2(vGlobal.X, vGlobal.Y));
}
}
//int slidingWindow = 5;
Polygon p = new Polygon();
p.Add(pts[0]);
for (int i = 1; i < pts.Count; i++)
{
if (Math.Abs(pts[i].DistanceTo(pts[i - 1])) < threshold)
{
p.Add(pts[i]);
}
else
{
//create a new poly
clusters.Add(p);
p = new Polygon();
p.Add(pts[i]);
}
}
clusters.Add(p); //add the last polygon
Console.WriteLine("Got " + clusters.Count + " polys");
Polygon vehiclePolygon = VehiclePolygonWithRadius(vehicleRadius); // vehicle model of polygon. Note that radius is parameter
// Inside foreach, I cannot reassign eachpolygon. So, make a List<Polygon>, assign into this, and return this
List <Polygon> ret = new List <Polygon>(clusters.Count);
foreach (Polygon eachPolygon in clusters)
{
// Do the MinKowskiConvolution and GrahamScan to remove interior points
//Polygon convolutionPolygon = Polygon.ConvexMinkowskiConvolution(eachPolygon, vehiclePolygon);
Polygon convexHull;
if (eachPolygon.Count > 1)
{
convexHull = Polygon.GrahamScan(eachPolygon, .001);
}
else
{
convexHull = eachPolygon;
}
Polygon bloated = Polygon.ConvexMinkowskiConvolution(convexHull, vehiclePolygon);
//convexHull.Add(convexHull[0]);
ret.Add(bloated);
//convolutionPolygonList.Add(eachPolygon);
}
// convolutionPolygonList = DecomposePolygonList(convolutionPolygonList);
return(ret);
}
public void SetScan(ILidarScan<ILidar2DPoint> scan, RobotPose robotPose, int startIdx, int endIdx)
{
lock (this.drawLock)
{
this.scan = scan;
this.time = scan.Timestamp;
this.numPoints = endIdx - startIdx + 1;
this.robotPose = robotPose;
this.startIdx = startIdx;
this.endIdx = endIdx;
}
}
private void lidar_ScanReceived(object sender, ILidarScanEventArgs <ILidarScan <ILidar2DPoint> > e)
{
currentData = e.Scan;
}
public void ClearBuffer()
{
scan = null;
}
public LidarFilterPackageMessage(int robotID, PoseFilterState state, SensorPose sensorPose, ILidarScan <ILidar2DPoint> scan)
{
this.robotID = robotID;
this.state = state;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
public static double FindTargetDistance(ILidarScan<ILidar2DPoint> lidarScan, double u, double v, Dictionary<int, int> colorPix, Vector2 TLCorner, Vector2 RBCorner,
string cameraSize, string camType, RobotPose camPose, TargetTypes type, out ILidar2DPoint lidarPt, ref List<Vector2> ptInBox)
{
#region camera stuff
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);
#endregion
// 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);
List<ILidar2DPoint> lidarScanInBox = new List<ILidar2DPoint>();
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]));
if (ptImgPlane[0, 0] >= TLCorner.X && ptImgPlane[0, 0] <= RBCorner.X && ptImgPlane[1, 0] >= TLCorner.Y && ptImgPlane[1, 0] <= RBCorner.Y)
{
if (colorPix.Count > 0)
{
if (colorPix.ContainsKey((int)ptImgPlane[0, 0]) && colorPix[(int)ptImgPlane[0, 0]] == 255)
{
lidarScanInBox.Add(pt);
ptInBox.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0]));
}
}
else
{
lidarScanInBox.Add(pt);
ptInBox.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0]));
}
}
}
if (lidarScanInBox.Count == 0)
{
lidarPt = null;
return -1;
}
lidarPt = FineTargetDistanceClusterBased(lidarScanInBox);
if (lidarPt == null)
return -1;
return lidarPt.RThetaPoint.R;
}
void SLAMEKFMainLoop(object o)
{
k = 0;
while (running)
{
lock (dataLock)
{
if (currentScan == null || viconPose == null || odomPose == null || viconLastPose == viconPose)
continue; // no data, no update
else
{
#region discrete-time increment
k = k + 1;
#endregion
if (k == 1)
{
#region state initialization
initialOdomPose = odomPose;
transformedOdomPose = new RobotPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odomPose.timestamp);
transformedOdomPose.x = (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x - (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.y = (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x + (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.yaw = odomPose.yaw;
transformedOdomPose = transformedOdomPose - initialOdomPose + initialViconPose;
odomLastPose = transformedOdomPose;
xhatvPose = transformedOdomPose;
#endregion
#region map initialization and management
#region extract features -> fz
List<Vector2> currentScanXY = new List<Vector2>();
fz.Clear();
ibook = fExtract(currentScan, bAngle, maxRange);
if (ibook != null)
{
for (int i = 0; i < currentScan.Points.Count; i++)
currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2());
for (int i = 0; i < ibook.Count; i++)
fz.Add(rotateTranslate(currentScanXY[ibook[i]], xhatvPose));
}
#endregion
#region populate the map -> landmarkList
landmarkList.Clear();
for (int i = 0; i < fz.Count; i++)
landmarkList.Add(fz[i]);
#endregion
#region populate the map -> mhat
mhat = new Matrix(2 * landmarkList.Count, 1);
for (int i = 0; i < landmarkList.Count; i++)
{
mhat[2 * i, 0] = landmarkList[i].X;
mhat[2 * i + 1, 0] = landmarkList[i].Y;
}
#endregion
#region covariance management -> Paug
Qv = new Matrix(3, 3, Math.Pow(sigw, 2));
Pv = Qv;
Paug = Pv;
for (int i = 0; i < fz.Count; i++)
Paug = blkdiag(Paug, new Matrix(2, 2, Math.Pow(sigw, 2)));
#endregion
lastScan = currentScan;
#endregion
}
else
{
#region odometry processing
transformedOdomPose = new RobotPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odomPose.timestamp);
transformedOdomPose.x = (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x - (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.y = (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x + (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y;
transformedOdomPose.yaw = odomPose.yaw;
transformedOdomPose = transformedOdomPose - initialOdomPose + initialViconPose;
xStep = transformedOdomPose - odomLastPose;
double omega = odomLastPose.yaw + xStep.yaw;
if (xStep.x == 0)
uvk.x = 0;
else
{
double vx = xStep.x / Math.Cos(omega);
uvk.x = vx * Math.Cos(omega);
}
if (xStep.y == 0)
uvk.y = 0;
else
{
double vy = xStep.y / Math.Sin(omega);
uvk.y = vy * Math.Sin(omega);
}
uvk.yaw = xStep.yaw;
uvk.z = 0;
uvk.roll = 0;
uvk.pitch = 0;
uvk.timestamp = odomPose.timestamp;
odomLastPose = transformedOdomPose;
#endregion
#region time-update
xhatvPose = xhatvPose + uvk;
Matrix Uvk = new Matrix(3, 1);
Uvk[0, 0] = uvk.x;
Uvk[1, 0] = uvk.y;
Uvk[2, 0] = uvk.yaw;
Pv = Pv + ScalarMultiply(Math.Pow(0.5,2),Uvk * Uvk.Transpose()) + Qv;
Paug.SetSubMatrix(0, 2, 0, 2, Pv);
Matrix Qmv = new Matrix(0, 0, 1.0);
for (int i = 0; i < landmarkList.Count; i++)
Qmv = blkdiag(Qmv, ScalarMultiply(Math.Pow(sigw / 5, 2), Pv.Submatrix(0, 1, 0, 1)));
Matrix Qm = new Matrix(Paug.Rows - 3, Paug.Columns - 3, Math.Pow(sigw / 5, 2));
Paug.SetSubMatrix(3, Paug.Rows - 1, 3, Paug.Columns - 1, Paug.Submatrix(3, Paug.Rows - 1, 3, Paug.Columns - 1) + Qmv + Qm);
#endregion
#region feature exraction
numLandmarks = landmarkList.Count;
List<Vector2> currentScanXY = new List<Vector2>();
fz.Clear();
ibook = fExtract(currentScan, bAngle, maxRange);
if (ibook != null)
{
for (int i = 0; i < currentScan.Points.Count; i++)
currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2());
for (int i = 0; i < ibook.Count; i++)
fz.Add(rotateTranslate(currentScanXY[ibook[i]], xhatvPose));
}
#endregion
#region data association
#region variables
fzHat = new List<Vector2>(landmarkList);
List<Vector3> jObsv = new List<Vector3>();
List<Vector2> iObsv = new List<Vector2>();
List<Vector2> obsvFeatureList = new List<Vector2>();
Matrix cLP = new Matrix(fz.Count, fzHat.Count);
Matrix dapGate = new Matrix(fz.Count, fzHat.Count);
Matrix fzM = new Matrix(2,1);
Matrix fzHatM = new Matrix(2,1);
#endregion
#region initialization
// "lpsolver55.dll" directory:
// (note: Alt + F7 -> Build -> enable "Allow UnSafe Code")
lpsolve.Init("C:\\users\\labuser\\desktop\\MAGIC\\Framework\\SLAM");
double[] xOpt;
double ignoreThisEntry = 0;
int nDecision = fz.Count * fzHat.Count;
int nConstraints = fz.Count + fzHat.Count;
int lp = lpsolve.make_lp((int)ignoreThisEntry, nDecision);
//lpsolve.set_sense(lp, true);
#endregion
#region c-vector (objective function)
double[] cObj = new double[nDecision + 1];
cObj[0] = ignoreThisEntry;
int ic = 0;
for (int i = 0; i < fz.Count; i++)
{
for (int j = 0; j < fzHat.Count; j++)
{
ic = ic + 1;
cObj[ic] = -1000000.0 + ciGate(fz[i], Paug.Submatrix(0, 1, 0, 1), fzHat[j], Paug.Submatrix(2 * j + 3, 2 * j + 4, 2 * j + 3, 2 * j + 4), 1.0);
cLP[i, j] = cObj[ic];
fzM[0,0] = fz[i].X;
fzM[1,0] = fz[i].Y;
fzHatM[0,0] = fzHat[j].X;
fzHatM[1,0] = fzHat[j].Y;
dapGate[i, j] = ((fzM - fzHatM).Transpose() * Paug.Submatrix(2 * j + 3, 2 * j + 4, 2 * j + 3, 2 * j + 4).Inverse * (fzM - fzHatM))[0, 0];
}
}
lpsolve.set_obj_fn(lp, cObj);
//lpsolve.set_timeout(lp, 0);
#endregion
#region b-vector (RHS of LE)
double[] bVec = new double[nConstraints];
for (int i = 0; i < bVec.Length; i++)
bVec[i] = 1.0;
#endregion
#region A-matrix (constraints setup)
double[,] A = new double[nConstraints, nDecision];
int jc = 0;
for (int i = 0; i < fz.Count; i++)
{
int jr = 1;
for (int j = 0; j < fzHat.Count; j++)
{
A[i, j + jc] = 1;
A[fz.Count - 1 + jr, j + jc] = 1;
jr = jr + 1;
}
jc = jc + fzHat.Count;
}
List<double[]> lpConstraints = new List<double[]>();
lpConstraints.Clear();
for (int i = 0; i < nConstraints; i++)
{
double[] Aline = new double[nDecision + 1];
Aline[0] = ignoreThisEntry;
for (int j = 1; j < nDecision + 1; j++)
{
Aline[j] = A[i, j - 1];
}
lpConstraints.Add(Aline);
}
for (int i = 0; i < nConstraints; i++)
lpsolve.add_constraint(lp, lpConstraints[i], lpsolve.lpsolve_constr_types.LE, bVec[i]);
#endregion
#region optimization and results
lpsolve.solve(lp);
xOpt = new double[lpsolve.get_Ncolumns(lp)];
lpsolve.get_variables(lp, xOpt);
lpsolve.delete_lp(lp);
ic = 0;
double tau = 6.63;
for (int i = 0; i < fz.Count; i++)
{
for (int j = 0; j < fzHat.Count; j++)
{
if ((xOpt[ic] > 0.98) && (xOpt[ic] < 1.02) && (dapGate[i, j] < tau))
jObsv.Add(new Vector3(i, j, cLP[i, j]));
ic = ic + 1;
}
}
if (jObsv.Count > 0)
{
for (int j = 0; j < jObsv.Count; j++)
{
iObsv.Add(new Vector2(jObsv[j].X, jObsv[j].Y));
obsvFeatureList.Add(rotateTranslate(currentScanXY[ibook[(int)(jObsv[j].X)]], xhatvPose)); // dev-only
}
}
numObsv = iObsv.Count;
#endregion
#endregion
#region measurement-update
if (numObsv > 0)
{
#region spf parameters
double L = 3 + 2 * landmarkList.Count;
double alpha = 1.0;
double kappa = 0.0;
double beta = 2.0;
double lambda = Math.Pow(alpha, 2) * (L + kappa) - L;
double gam = Math.Sqrt(L + lambda);
double wm0 = lambda / (L + lambda);
double wc0 = lambda / (L + lambda) + (1 - Math.Pow(alpha, 2) + beta);
wm = new Matrix((int)(2 * L) + 1, 1);
for (int j = 0; j < (int)(2 * L) + 1; j++)
{
if (j == 0)
wm[j, 0] = wm0;
else
wm[j, 0] = 1 / (2.0 * (L + lambda));
}
wc = new Matrix((int)(2 * L) + 1, 1);
for (int j = 0; j < (int)(2.0 * L) + 1; j++)
{
if (j == 0)
wc[j, 0] = wc0;
else
wc[j, 0] = 1 / (2.0 * (L + lambda));
}
#endregion
#region spf sampler
CholeskyDecomposition PCholContainer = new CholeskyDecomposition(ScalarMultiply(L + lambda, Paug));
Matrix PChol = PCholContainer.LeftTriangularFactor;
chi0 = new Matrix((int)L, 1);
chi0[0, 0] = xhatvPose.x - uvk.x;
chi0[1, 0] = xhatvPose.y - uvk.y;
chi0[2, 0] = xhatvPose.yaw - uvk.yaw;
for (int i = 0; i < mhat.Rows; i++)
chi0[3 + i, 0] = mhat[i, 0];
chi1 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
chi1.SetSubMatrix(0, chi1.Rows - 1, i, i, chi0 + PChol.Submatrix(0, chi0.Rows - 1, i, i));
chi2 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
chi2.SetSubMatrix(0, chi2.Rows - 1, i, i, chi0 - PChol.Submatrix(0, chi0.Rows - 1, i, i));
chi = chi0;
chi = chi.Concat(2, chi1);
chi = chi.Concat(2, chi2);
#endregion
#region spf time-update
Matrix chiBar = new Matrix((int)L, (int)(2 * L) + 1);
Matrix uvkSPF = new Matrix((int)L, 1);
uvkSPF.Zero();
uvkSPF[0, 0] = uvk.x;
uvkSPF[1, 0] = uvk.y;
uvkSPF[2, 0] = uvk.yaw;
for (int i = 0; i < (int)(2 * L) + 1; i++)
chiBar.SetSubMatrix(0, chiBar.Rows - 1, i, i, chi.Submatrix(0, chi.Rows - 1, i, i) + uvkSPF);
Matrix xBar = chiBar * wm;
Matrix PBar = new Matrix((int)L, (int)L);
PBar.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
PBar = PBar + ScalarMultiply(wc[i, 0], (chiBar.Submatrix(0, chiBar.Rows - 1, i, i) - xBar) * (chiBar.Submatrix(0, chiBar.Rows - 1, i, i) - xBar).Transpose());
#endregion
#region sigma-point update
CholeskyDecomposition PBarCholContainer = new CholeskyDecomposition(PBar);
Matrix PBarChol = PBarCholContainer.LeftTriangularFactor;
chi0 = xBar;
chi1 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
chi1.SetSubMatrix(0, chi1.Rows - 1, i, i, xBar + ScalarMultiply(gam, PBarChol.Submatrix(0, chi0.Rows - 1, i, i)));
chi2 = new Matrix((int)L, (int)L);
for (int i = 0; i < (int)L; i++)
chi2.SetSubMatrix(0, chi2.Rows - 1, i, i, xBar - ScalarMultiply(gam, PBarChol.Submatrix(0, chi0.Rows - 1, i, i)));
chiUpdate = chi0;
chiUpdate = chiUpdate.Concat(2, chi1);
chiUpdate = chiUpdate.Concat(2, chi2);
#endregion
#region spf measurement-update setup
#region predicted measurement
Matrix YBar = new Matrix(2 * numObsv, (int)(2 * L) + 1);
YBar.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
{
chiUpdateObsv = new Matrix((int)(2 * numObsv), 1);
for (int j = 0; j < numObsv; j++)
{
chiUpdateObsv[2 * j, 0] = chiUpdate[(int)(2 * iObsv[j].Y + 3), i];
chiUpdateObsv[2 * j + 1, 0] = chiUpdate[(int)(2 * iObsv[j].Y + 1 + 3), i];
}
YBar.SetSubMatrix(0, YBar.Rows - 1, i, i, hFunction(chiUpdate.Submatrix(0, 2, i, i), chiUpdateObsv));
}
Matrix yBar = YBar * wm;
#endregion
#region actual measurement
Matrix z = new Matrix(2 * numObsv, 1);
for (int i = 0; i < numObsv; i++)
{
z[2 * i, 0] = currentScan.Points[ibook[(int)(iObsv[i].X)]].RThetaPoint.R;
z[2 * i + 1, 0] = currentScan.Points[ibook[(int)(iObsv[i].X)]].RThetaPoint.theta;
}
#endregion
#region innovation covariance
S = new Matrix(2 * numObsv, 2 * numObsv);
S.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
S = S + ScalarMultiply(wc[i, 0], (YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar) *
(YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar).Transpose());
S = S + new Matrix(2 * numObsv, 2 * numObsv, Math.Pow(sigv, 2));
#endregion
#region Kalman gain matrix
W = new Matrix((int)L, 2 * numObsv);
W.Zero();
for (int i = 0; i < (int)(2 * L) + 1; i++)
W = W + ScalarMultiply(wc[i, 0], (chiUpdate.Submatrix(0, chiUpdate.Rows - 1, i, i) - xBar) *
(YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar).Transpose());
W = W * S.Inverse;
#endregion
#region measurement wrapper
zCart = new Matrix(2 * numObsv, 1);
for (int i = 0; i < numObsv; i++)
{
zCart[2 * i, 0] = z[2 * i, 0] * Math.Cos(z[2 * i + 1, 0]);
zCart[2 * i + 1, 0] = z[2 * i, 0] * Math.Sin(z[2 * i + 1, 0]);
}
for (int i = 0; i < numObsv; i++)
{
z[2 * i, 0] = Math.Sqrt(Math.Pow(zCart[2 * i, 0], 2) + Math.Pow(zCart[2 * i + 1, 0], 2));
z[2 * i + 1, 0] = Math.Atan2(zCart[2 * i + 1, 0], zCart[2 * i, 0]);
}
yBarCart = new Matrix(2 * numObsv, 1);
for (int i = 0; i < numObsv; i++)
{
yBarCart[2 * i, 0] = yBar[2 * i, 0] * Math.Cos(yBar[2 * i + 1, 0]);
yBarCart[2 * i + 1, 0] = yBar[2 * i, 0] * Math.Sin(yBar[2 * i + 1, 0]);
}
for (int i = 0; i < numObsv; i++)
{
yBar[2 * i, 0] = Math.Sqrt(Math.Pow(yBarCart[2 * i, 0], 2) + Math.Pow(yBarCart[2 * i + 1, 0], 2));
yBar[2 * i + 1, 0] = Math.Atan2(yBarCart[2 * i + 1, 0], yBarCart[2 * i, 0]);
}
#endregion
#endregion
#region spf measurement-update
//(((z - yBar).Transpose() * S * (z - yBar))[0, 0] < 6.63)
xBar = xBar + W * (z - yBar);
Paug = PBar - W * S * W.Transpose();
#region SLAM-context of measurement update
#region localization
xhatvPose.x = xBar[0, 0];
xhatvPose.y = xBar[1, 0];
xhatvPose.yaw = xBar[2, 0];
Pv = Paug.Submatrix(0, 2, 0, 2);
#endregion
#region mapping
mhat = xBar.Submatrix(3, (int)L - 1, 0, 0);
int landmarkListCount = landmarkList.Count;
landmarkList.Clear();
for (int i = 0; i < landmarkListCount; i++)
landmarkList.Add(new Vector2(mhat[2 * i, 0], mhat[2 * i + 1, 0]));
#endregion
#endregion
#endregion
}
#endregion
#region show features
slamFeatures.Clear();
if (ibook != null)
{
for (int i = 0; i < ibook.Count; i++)
slamFeatures.Add(fz[i]);
}
#endregion
#region map management
double minDist = 0.5;
double sigMapFreeze = 0.5;
int landmarkMaxCount = 20;
for (int i = 0; i < iObsv.Count; i++)
fz.RemoveAt((int)(iObsv[i].X - i));
for (int i = 0; i < fz.Count; i++)
{
int nFlag = 0;
for (int j = 0; j < landmarkList.Count; j++)
{
if ((fz[i] - landmarkList[j]).Length > minDist)
nFlag = nFlag + 1;
}
if ((nFlag == landmarkList.Count) && (Pv.Trace < sigMapFreeze) && (landmarkList.Count - 1 <= landmarkMaxCount))
{
landmarkList.Add(fz[i]);
Paug = blkdiag(Paug, new Matrix(2, 2, (Pv.Submatrix(0, 1, 0, 1)).Trace));
Matrix mHatNew = new Matrix(2,1);
mHatNew[0, 0] = fz[i].X;
mHatNew[1, 0] = fz[i].Y;
mhat = mhat.Concat(1, mHatNew);
break; // add one feature at a time
}
}
#endregion
#region show landmarks
slamLandmarks.Clear();
slamLandmarksCovariance.Clear();
for (int i = 0; i < landmarkList.Count; i++)
{
slamLandmarks.Add(landmarkList[i]);
slamLandmarksCovariance.Add(Paug.Submatrix(2 * i + 3, 2 * i + 4, 2 * i + 3, 2 * i + 4));
}
#endregion
#region show correspondences
slamCorrespondences.Clear();
for (int i = 0; i < obsvFeatureList.Count; i++)
slamCorrespondences.Add(obsvFeatureList[i]);
#endregion
#region broadcast the results
viconLastPose = viconPose;
//filteredPose = transformedOdomPose;
filteredPose = xhatvPose;
lastScan = currentScan;
//covMatrix = Pv; // only vehicle xy-submatrix is used in the form
covMatrix = Pv; // only vehicle xy-submatrix is used in the form
//transformedOdomPose = viconPose;
#endregion
}
}
}
Thread.Sleep(100);
}
}
public LidarScanMessage(int robotID, ILidarScan<ILidar2DPoint> scan)
{
this.scan = scan;
this.robotID = robotID;
}
/// <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;
}
