//---------------------------------------------------------
/* Matlab version
* %% forward substitution
* % use to find Z for L*Z = C, given L and C
* function Z = ForwardSub(L,C)
* Z = zeros(size(C));
* for c=1:size(C,2)
* Z(1,c) = C(1,c)/L(1,1);
* for r=2:size(C,1)
* sum = 0;
* for j=1:(r-1)
* sum = sum + L(r,j)*Z(j,c);
* end
* Z(r,c) = (C(r,c)-sum)/L(r,r);
* end
* end
* end
*/
/// <summary> added by Rahul Rastogi
/// Forward Substitution function to find Z for L*Z = C, given L and C
/// </summary>
/// <param name="L">Lower Triangular Matrix, must be square</param>
/// <param name="C">Matrix to divide into, must have same number of rows as L</param>
public static UMatrix ForwardSub(UMatrix L, UMatrix C)
{
if (L.numCols != L.numRows || L.numRows != C.numRows)
{
throw new ArgumentException("Matrix dimensions are not valid.");
}
int rows = L.numCols;
int cols = C.numCols;
UMatrix Z = new UMatrix(rows, cols);
fixed(double *ptrZ = Z.data, ptrL = L.data, ptrC = C.data)
{
for (int c = 0; c < C.numCols; c++)
{
(*(ptrZ + c)) = (*(ptrC + c)) / (*(ptrL));
for (int r = 1; r < C.numRows; r++)
{
double sum = 0;
for (int j = 0; j <= (r - 1); j++)
{
sum += (*(ptrL + j + (L.numCols * r))) * (*(ptrZ + c + (Z.numCols * j)));
}
(*(ptrZ + c + (Z.numCols * r))) = (*(ptrC + c + (C.numCols * r)) - sum) / (*(ptrL + r + (L.numCols * r)));
}
}
return(Z);
}
}
//-----------------------------------------------------------------
/* Matlab version
* %% backward substitution
* % use to find X for U*X = Z, given U and Z
* function X = BackSub(U,Z)
* N = size(U,1);
* X = zeros(N);
* for c=1:size(Z,2)
* X(N,c) = Z(N,c)/U(N,N);
* for r=(N-1):-1:1
* sum = 0;
* for j=(r+1):N
* sum = sum + U(r,j)*X(j,c);
* end
* X(r,c) = (Z(r,c)-sum)/U(r,r);
* end
* end
* end
*/
/// <summary> added by Rahul Rastogi
/// Backward Substitution function to find X for U*X = Z, given U and Z
/// </summary>
/// <param name="U">Upper Triangular Matrix, must be square</param>
/// <param name="Z">Matrix to divide into, must have same number of rows as U</param>
public static UMatrix BackwardSub(UMatrix U, UMatrix Z)
{
if (U.numCols != U.numRows || U.numRows != Z.numRows)
{
throw new ArgumentException("Matrix dimensions are not valid.");
}
int rows = U.numCols;
int cols = Z.numCols;
int n = (U.numRows - 1);
UMatrix X = new UMatrix(rows, cols);
fixed(double *ptrX = X.data, ptrU = U.data, ptrZ = Z.data)
{
for (int c = 0; c < Z.numCols; c++)
{
(*(ptrX + c + (X.numCols * n))) = (*(ptrZ + c + (Z.numCols * n))) / (*(ptrU + n + (U.numCols * n)));
for (int r = (n - 1); r >= 0; r--)
{
double sum = 0;
for (int j = (r + 1); j <= n; j++)
{
sum += (*(ptrU + j + (U.numCols * r))) * (*(ptrX + c + (X.numCols * j)));
}
(*(ptrX + c + (X.numCols * r))) = (*(ptrZ + c + (Z.numCols * r)) - sum) / (*(ptrU + r + (U.numCols * r)));
}
}
return(X);
}
}
/// <summary>Matrix-matrix multiplication.</summary>
public static UMatrix Multiply(UMatrix left, UMatrix right)
{
if (right.numRows != left.numCols)
{
throw new ArgumentException("Matrix dimensions are not valid.");
}
int rows = left.numRows;
int columns = right.numCols;
int size = left.numCols;
UMatrix X = new UMatrix(rows, columns);
fixed(double *ptrX = X.data, ptrL = left.data, ptrR = right.data)
{
for (int rl = 0; rl < left.numRows; rl++)
{
for (int cr = 0; cr < right.numCols; cr++)
{
double sum = 0;
for (int rr = 0; rr < right.numRows; rr++)
{
sum += (*(ptrL + rr + (left.numCols * rl))) * (*(ptrR + cr + (right.numCols * rr)));
//sum += left.data[rr + (left.numCols * rl)] * right.data[cr + (right.numCols * rr)];
}
//X.data[cr + (X.numCols * rl)] = sum;
(*(ptrX + cr + (X.numCols * rl))) = sum;
}
}
}
//-----------------------------------------------------
return(X);
}
/// <summary>Matrix subtraction.</summary>
public static UMatrix Subtract(UMatrix left, UMatrix right)
{
int rows = left.numRows;
int columns = left.numCols;
if ((rows != right.numRows) || (columns != right.numCols))
{
throw new ArgumentException("Matrix dimension do not match.");
}
UMatrix X = new UMatrix(rows, columns);
fixed(double *ptrX = X.data, ptrL = left.data, ptrR = right.data)
{
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < columns; j++)
{
//X.data[j + (columns * i)] = left.data[j + (columns * i)] - right.data[j + (columns * i)];
(*(ptrX + j + (columns * i))) = (*(ptrL + j + (columns * i))) - (*(ptrR + j + (columns * i)));
}
}
}
return(X);
}
/// <summary>Unary minus.</summary>
public static UMatrix Negate(UMatrix value)
{
if (value == null)
{
throw new ArgumentNullException("value");
}
int rows = value.numRows;
int columns = value.numCols;
UMatrix X = new UMatrix(rows, columns);
fixed(double *ptrX = X.data, ptrThis = value.data)
{
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < columns; j++)
{
//X.data[j + (columns * i)] = -value.data[j + (columns * i)];
(*(ptrX + j + (columns * i))) = -(*(ptrThis + j + (columns * i)));
}
}
}
return(X);
}
public UMatrix Inverse()
{
UMatrix upper, lower, c, z;
LuDecomposition(out lower, out upper);
c = new UMatrix(numRows, numCols, 1);
z = UMatrix.ForwardSub(lower, c);
return(UMatrix.BackwardSub(upper, z));
}
public UMatrix(UMatrix m)
{
this.numRows = m.numRows;
this.numCols = m.numCols;
this.data = new double[numRows * numCols];
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numCols; j++)
{
this[i, j] = m[i, j];
}
}
}
public UMatrix(UMatrix m)
{
this.numRows = m.numRows;
this.numCols = m.numCols;
this.data = new double[numRows * numCols];
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numCols; j++)
{
this[i, j] = m[i, j];
}
}
}
/// <summary>Returns the transposed matrix.</summary>
public UMatrix Transpose()
{
UMatrix X = new UMatrix(numCols, numRows);
fixed(double *ptrX = X.data, ptrThis = data)
{
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numCols; j++)
{
//X.data[i + (numRows * j)] = data[j + (numCols * i)];
(*(ptrX + i + (numRows * j))) = (*(ptrThis + j + (numCols * i)));
}
}
}
return(X);
}
/// <summary>Matrix-scalar multiplication.</summary>
public static UMatrix Multiply(UMatrix left, double right)
{
int rows = left.numRows;
int columns = left.numCols;
UMatrix X = new UMatrix(rows, columns);
fixed(double *ptrX = X.data, ptrL = left.data)
{
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < columns; j++)
{
//X.data[j + (columns * i)] = left.data[j + (columns * i)] * right;
(*(ptrX + j + (columns * i))) = ((*(ptrL + j + (columns * i))) * right);
}
}
}
return(X);
}
/// <summary>Returns a sub matrix extracted from the current matrix.</summary>
/// <param name="startRow">Start row index</param>
/// <param name="endRow">End row index</param>
/// <param name="startColumn">Start column index</param>
/// <param name="endColumn">End column index</param>
public UMatrix Submatrix(int startRow, int endRow, int startColumn, int endColumn)
{
if ((startRow > endRow) || (startColumn > endColumn) || (startRow < 0) || (startRow >= this.numRows) || (endRow < 0) || (endRow >= this.numRows) || (startColumn < 0) || (startColumn >= this.numCols) || (endColumn < 0) || (endColumn >= this.numCols))
{
throw new ArgumentException("Argument out of range.");
}
UMatrix X = new UMatrix(endRow - startRow + 1, endColumn - startColumn + 1);
fixed(double *ptrX = X.data, ptrThis = data)
{
for (int i = startRow; i <= endRow; i++)
{
for (int j = startColumn; j <= endColumn; j++)
{
//X.data[j + (X.numCols * i)] = data[j + (numCols * i)];
(*(ptrX + j + (X.numCols * i))) = (*(ptrThis + j + (numCols * i)));
}
}
}
return(X);
}
/// <summary>Matrix-matrix multiplication.</summary>
public static UMatrix Multiply(UMatrix left, UMatrix right)
{
if (right.numRows != left.numCols)
{
throw new ArgumentException("Matrix dimensions are not valid.");
}
int rows = left.numRows;
int columns = right.numCols;
int size = left.numCols;
UMatrix X = new UMatrix(rows, columns);
fixed (double* ptrX = X.data, ptrL = left.data, ptrR = right.data)
{
for (int rl = 0; rl < left.numRows; rl++)
{
for (int cr = 0; cr < right.numCols; cr++)
{
double sum = 0;
for (int rr = 0; rr < right.numRows; rr++)
{
sum += (*(ptrL + rr + (left.numCols * rl))) * (*(ptrR + cr + (right.numCols * rr)));
//sum += left.data[rr + (left.numCols * rl)] * right.data[cr + (right.numCols * rr)];
}
//X.data[cr + (X.numCols * rl)] = sum;
(*(ptrX + cr + (X.numCols * rl))) = sum;
}
}
}
//-----------------------------------------------------
return X;
}
UMatrix ComputeJacobianQ(double q1, double q2, double q3, double q4)
{
UMatrix toReturn = new UMatrix(16, 7);
toReturn.Zero();
toReturn[12, 0] = 1;
toReturn[13, 1] = 1;
toReturn[14, 2] = 1;
toReturn[0, 3] = 2 * q1;
toReturn[1, 3] = 2 * q2;
toReturn[2, 3] = 2 * q3;
toReturn[4, 3] = 2 * q2;
toReturn[5, 3] = -2 * q1;
toReturn[6, 3] = 2 * q4;
toReturn[8, 3] = 2 * q3;
toReturn[9, 3] = -2 * q4;
toReturn[10, 3] = -2 * q1;
toReturn[0, 4] = -2 * q2;
toReturn[1, 4] = 2 * q1;
toReturn[2, 4] = -2 * q4;
toReturn[4, 4] = 2 * q1;
toReturn[5, 4] = 2 * q2;
toReturn[6, 4] = 2 * q3;
toReturn[8, 4] = 2 * q4;
toReturn[9, 4] = 2 * q3;
toReturn[10, 4] = -2 * q2;
toReturn[0, 5] = -2 * q3;
toReturn[1, 5] = 2 * q4;
toReturn[2, 5] = 2 * q1;
toReturn[4, 5] = -2 * q4;
toReturn[5, 5] = -2 * q3;
toReturn[6, 5] = 2 * q2;
toReturn[8, 5] = 2 * q1;
toReturn[9, 5] = 2 * q2;
toReturn[10, 5] = 2 * q3;
toReturn[0, 6] = 2 * q4;
toReturn[1, 6] = 2 * q3;
toReturn[2, 6] = -2 * q2;
toReturn[4, 6] = -2 * q3;
toReturn[5, 6] = 2 * q4;
toReturn[6, 6] = 2 * q1;
toReturn[8, 6] = 2 * q2;
toReturn[9, 6] = -2 * q1;
toReturn[10, 6] = 2 * q4;
return toReturn;
}
//---------------------------------------------------------
/* Matlab version
%% forward substitution
% use to find Z for L*Z = C, given L and C
function Z = ForwardSub(L,C)
Z = zeros(size(C));
for c=1:size(C,2)
Z(1,c) = C(1,c)/L(1,1);
for r=2:size(C,1)
sum = 0;
for j=1:(r-1)
sum = sum + L(r,j)*Z(j,c);
end
Z(r,c) = (C(r,c)-sum)/L(r,r);
end
end
end
*/
/// <summary> added by Rahul Rastogi
/// Forward Substitution function to find Z for L*Z = C, given L and C
/// </summary>
/// <param name="L">Lower Triangular Matrix, must be square</param>
/// <param name="C">Matrix to divide into, must have same number of rows as L</param>
public static UMatrix ForwardSub(UMatrix L, UMatrix C)
{
if (L.numCols != L.numRows || L.numRows != C.numRows)
{
throw new ArgumentException("Matrix dimensions are not valid.");
}
int rows = L.numCols;
int cols = C.numCols;
UMatrix Z = new UMatrix(rows, cols);
fixed (double* ptrZ = Z.data, ptrL = L.data, ptrC = C.data)
{
for (int c = 0; c < C.numCols; c++)
{
(*(ptrZ + c)) = (*(ptrC + c)) / (*(ptrL));
for (int r = 1; r < C.numRows; r++)
{
double sum = 0;
for (int j = 0; j <= (r - 1); j++)
{
sum += (*(ptrL + j + (L.numCols * r))) * (*(ptrZ + c + (Z.numCols * j)));
}
(*(ptrZ + c + (Z.numCols * r))) = (*(ptrC + c + (C.numCols * r)) - sum) / (*(ptrL + r + (L.numCols * r)));
}
}
return Z;
}
}
/// <summary>Matrix-scalar multiplication.</summary>
public static UMatrix Multiply(UMatrix left, double right)
{
int rows = left.numRows;
int columns = left.numCols;
UMatrix X = new UMatrix(rows, columns);
fixed (double* ptrX = X.data, ptrL = left.data)
{
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < columns; j++)
{
//X.data[j + (columns * i)] = left.data[j + (columns * i)] * right;
(*(ptrX + j + (columns * i))) = ((*(ptrL + j + (columns * i))) * right);
}
}
}
return X;
}
public RobotTwoWheelCommand GetCommand()
{
double g = 3;
double zeta = .7; //parameters for STC
double dt = .4; // adjust to change speed
double deltadist = .10; //incremental distance for looking ahead
// deltadist/dt = nominal speed along path
//get trajectory // this step is performed outside of this function by BehavioralDirector
//discretize trajectory into [xr, yr]
//generate reference velocities at each trajectory point [vr, wr, thetar] if new path
if (!pathsame)
{
this.GenrefVWHuniformdt(pathCurrentlyTracked, deltadist, dt);
}
//use current timestamp to find corresponding index in reference vectors
//double currenttime = TimeSync.CurrentTime;
double currenttime = (DateTime.Now.Ticks - 621355968000000000) / 10000000.0;
int indextotrack = (int) (Math.Ceiling((currenttime - timestamps[0]) / dt));
if (indextotrack >= timestamps.Length)
indextotrack = timestamps.Length - 1;
Console.WriteLine("indextotrack = {0}", indextotrack);
Console.WriteLine("time = {0}", currenttime);
//unwrap thetar against thetarold, ignore if null (1337)
if (!endpointsame) { thetarold = 1337; }
if (thetarold != 1337)
{
while (hr[indextotrack] - thetarold >= Math.PI)
{
hr[indextotrack] -= 2 * Math.PI; ;
}
while (hr[indextotrack] - thetarold <= -Math.PI)
{
hr[indextotrack] += 2 * Math.PI;
}
}
//set thetarold to thetar
thetarold = hr[indextotrack];
double[] qrefdata = { xr[indextotrack], yr[indextotrack], hr[indextotrack] };
UMatrix qref = new UMatrix(3, 1, qrefdata);
//print to qref to screen // use only for debugging
//string qrefstring = qref.ToString();
//Console.WriteLine("QREF\n {0}", qrefstring);
//establish pose vector q = [x, y, theta];
double x, y, theta;
x = this.currentPoint.x;
y = this.currentPoint.y;
theta = this.currentPoint.yaw; // in radians
double[] qdata = { x, y, theta };
UMatrix q = new UMatrix(3, 1, qdata);
//unwrap pose against qref
while (q[2, 0] - qref[2, 0] >= Math.PI)
{
q[2, 0] -= 2 * Math.PI; ;
}
while (q[2, 0] - qref[2, 0] <= -Math.PI)
{
q[2, 0] += 2 * Math.PI;
}
//establish kinematic state error
UMatrix delq = new UMatrix(3, 1);
delq = qref - q;
//if (!endpointsame) { delqthetarold = 1337; }
//if (delqthetarold != 1337)
//{
// while (delq[2,0] - delqthetarold >= Math.PI)
// {
// delq[2,0] -= 2 * Math.PI; ;
// }
// while (delq[2,0] - delqthetarold <= -Math.PI)
// {
// delq[2,0] += 2 * Math.PI;
// }
//}
////set thetarold to thetar
//delqthetarold = delq[2,0];
//print delq to screen // use only for debugging
string delqstring = delq.ToString();
//Console.WriteLine("ERROR\n {0}", delqstring);
//Console.ReadLine();
//create rotation matrix
double[] Rotdata = { Math.Cos(theta), Math.Sin(theta), 0, -Math.Sin(theta), Math.Cos(theta), 0, 0, 0, 1 };
UMatrix Rot = new UMatrix(3, 3, Rotdata);
//convert to robot frame
UMatrix e = new UMatrix(3, 1);
e = Rot * delq; //error in the robot reference frame: first element is along track error, second element is cross track error, last element is angle error
string estring = e.ToString();
Console.WriteLine("ERROR\n {0}", estring);
// calculate Uf = [vf, wf]
double vf = vr[indextotrack] * Math.Cos(e[2, 0]); // in meters/sec
double wf = wr[indextotrack] * 180 / Math.PI; // in degrees/sec
Console.WriteLine("vf = {0}, wf = {1}",vf,wf);
//calculate wn = sqrt(wr^2 + g*vr^2)
double wn = Math.Sqrt(Math.Pow(wr[indextotrack], 2) + g * (Math.Pow(vr[indextotrack], 2)));
//calculte STC gain matrix
double k1, k2, k3;
k1 = 2 * zeta * wn;
k3 = k1;
k2 = g * vr[indextotrack];
double[] Kdata = { k1, 0, 0, 0, k2, k3 };
UMatrix K = new UMatrix(2, 3, Kdata);
//find control values Ub = K*e
UMatrix Ub = new UMatrix(2, 1);
Ub = K * e;
Console.WriteLine("vb = {0}, wb = {1}", Ub[0, 0], Ub[1, 0]);
//find total control u = Uf + Ub
double vcommand = (Ub[0, 0] + vf) * 3.6509; // multiply by 3.6509 to get "segway units"
double wcommand = (Ub[1, 0] * 180 / Math.PI + wf) * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec)
RobotTwoWheelCommand command = new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand);
if (Math.Abs(wcommand) >= 600)
Console.WriteLine("W saturation");
//create text file of information
if (indextotrack < timestamps.Length && counter < 15)
{
if (indextotrack == timestamps.Length - 1) { counter = counter + 1; }
tw2.WriteLine("{0} {1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} ", indextotrack, timestamps[indextotrack], x, y, e[0, 0], e[1, 0], e[2, 0], vf, wf, Ub[0, 0], Ub[1, 0]);
}
else { tw2.Close(); }
return command;
}
//-----------------------------------------------------------------
/* Matlab version
%% backward substitution
% use to find X for U*X = Z, given U and Z
function X = BackSub(U,Z)
N = size(U,1);
X = zeros(N);
for c=1:size(Z,2)
X(N,c) = Z(N,c)/U(N,N);
for r=(N-1):-1:1
sum = 0;
for j=(r+1):N
sum = sum + U(r,j)*X(j,c);
end
X(r,c) = (Z(r,c)-sum)/U(r,r);
end
end
end
*/
/// <summary> added by Rahul Rastogi
/// Backward Substitution function to find X for U*X = Z, given U and Z
/// </summary>
/// <param name="U">Upper Triangular Matrix, must be square</param>
/// <param name="Z">Matrix to divide into, must have same number of rows as U</param>
public static UMatrix BackwardSub(UMatrix U, UMatrix Z)
{
if (U.numCols != U.numRows || U.numRows != Z.numRows)
{
throw new ArgumentException("Matrix dimensions are not valid.");
}
int rows = U.numCols;
int cols = Z.numCols;
int n = (U.numRows-1);
UMatrix X = new UMatrix(rows, cols);
fixed (double* ptrX = X.data, ptrU = U.data, ptrZ = Z.data)
{
for (int c = 0; c < Z.numCols; c++)
{
(*(ptrX + c + (X.numCols * n))) = (*(ptrZ + c + (Z.numCols * n))) / (*(ptrU + n + (U.numCols * n)));
for (int r = (n-1); r >= 0; r--)
{
double sum = 0;
for (int j = (r+1); j <= n; j++)
{
sum += (*(ptrU + j + (U.numCols * r))) * (*(ptrX + c + (X.numCols * j)));
}
(*(ptrX + c + (X.numCols * r))) = (*(ptrZ + c + (Z.numCols * r)) - sum) / (*(ptrU + r + (U.numCols * r)));
}
}
return X;
}
}
private UMatrix ComputeJacobianZYX(double yaw, double pitch, double roll)
{
UMatrix m = new UMatrix(16, 6);
m.Zero();
m[12, 0] = 1;
m[13, 1] = 1;
m[14, 2] = 1;
m[1, 3] = Math.Cos(roll) * Math.Sin(pitch) * Math.Cos(yaw) + Math.Sin(roll) * Math.Sin(yaw);
m[2, 3] = -Math.Sin(roll) * Math.Sin(pitch) * Math.Cos(yaw) + Math.Cos(roll) * Math.Sin(yaw);
m[5, 3] = Math.Cos(roll) * Math.Sin(pitch) * Math.Sin(yaw) - Math.Sin(roll) * Math.Cos(yaw);
m[6, 3] = -Math.Sin(roll) * Math.Sin(pitch) * Math.Sin(yaw) - Math.Cos(roll) * Math.Cos(yaw);
m[9, 3] = Math.Cos(roll) * Math.Cos(pitch);
m[10, 3] = -Math.Sin(roll) * Math.Cos(pitch);
m[0, 4] = -Math.Sin(pitch) * Math.Cos(yaw);
m[1, 4] = Math.Sin(roll) * Math.Cos(pitch) * Math.Cos(yaw);
m[2, 4] = Math.Cos(roll) * Math.Cos(pitch) * Math.Cos(yaw);
m[4, 4] = -Math.Sin(pitch) * Math.Sin(yaw);
m[5, 4] = Math.Sin(roll) * Math.Cos(pitch) * Math.Sin(yaw);
m[6, 4] = Math.Cos(roll) * Math.Cos(pitch) * Math.Sin(yaw);
m[8, 4] = -Math.Cos(pitch);
m[9, 4] = -Math.Sin(roll) * Math.Sin(pitch);
m[10, 4] = -Math.Cos(roll) * Math.Sin(pitch);
m[0, 5] = -Math.Cos(pitch) * Math.Sin(yaw);
m[1, 5] = -Math.Sin(roll) * Math.Sin(pitch) * Math.Sin(yaw) - Math.Cos(roll) * Math.Cos(yaw);
m[2, 5] = -Math.Cos(roll) * Math.Sin(pitch) * Math.Sin(yaw) + Math.Sin(roll) * Math.Cos(yaw);
m[4, 5] = Math.Cos(pitch) * Math.Cos(yaw);
m[5, 5] = Math.Sin(roll) * Math.Sin(pitch) * Math.Cos(yaw) - Math.Cos(roll) * Math.Sin(yaw);
m[6, 5] = Math.Cos(roll) * Math.Sin(pitch) * Math.Cos(yaw) + Math.Sin(roll) * Math.Sin(yaw);
return m;
}
private void UpdateCovariance(Matrix robotCov)
{
lock (locker)
{
UMatrix abc = new UMatrix(6, 6);
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6; j++)
{
abc[i, j] = robotCov[i, j]; // this is a heck !!!!!
}
if (abc[i, i] < 0)
abc[i, i] = Math.Abs(abc[i, i]);
}
if (abc[2, 2] == 0) abc[2, 2] = 0.01;
if (abc[4, 4] == 0) abc[4, 4] = 0.01;
if (abc[5, 5] == 0) abc[5, 5] = 0.01;
covRobotPose = abc;
}
}
//----------------------------------------------------------------------------
public UMatrix LuDecomposition(out UMatrix l, out UMatrix u)
{
double[] lu = new double[data.Length];
Array.Copy(data, lu, data.Length);
//double[] luRowI;
double *luRowIPtr;
double[] luColJ = new double[numRows];
double[] lData = new double[numRows * numRows];
double[] uData = new double[numRows * numRows];
fixed(double *luPtr = lu, luColJPtr = luColJ, lDataPtr = lData, uDataPtr = uData)
{
int pivotSign = 1;
int[] pivotVector = new int[numRows];
for (int i = 0; i < numRows; i++)
{
pivotVector[i] = i;
}
// Outer loop.
for (int j = 0; j < numCols; j++)
{
// Make a copy of the j-th column to localize references.
for (int i = 0; i < numRows; i++)
{
*(luColJPtr + i) = *(luPtr + j + i * numCols);
}
// Apply previous transformations.
for (int i = 0; i < numRows; i++)
{
luRowIPtr = luPtr + i * numCols;
// Most of the time is spent in the following dot product.
//int kmax = (i > j) ? j : i;
int kmax = Math.Min(i, j);
double s = 0.0;
for (int k = 0; k < kmax; k++)
{
s += *(luRowIPtr + k) * *(luColJPtr + k);
}
*(luRowIPtr + j) = *(luColJPtr + i) -= s;
}
// Find pivot and exchange if necessary.
int p = j;
for (int i = j + 1; i < numRows; i++)
{
if (Math.Abs(*(luColJPtr + i)) > Math.Abs(*(luColJPtr + p)))
{
p = i;
}
}
if (p != j)
{
for (int k = 0; k < numCols; k++)
{
double t = *(luPtr + k + p * numCols);
*(luPtr + k + p * numCols) = *(luPtr + k + j * numCols);
*(luPtr + k + j * numCols) = t;
}
int v = pivotVector[p];
pivotVector[p] = pivotVector[j];
pivotVector[j] = v;
pivotSign = -pivotSign;
}
// Compute multipliers.
if (j < numRows & *(luPtr + j + j * numCols) != 0.0)
{
for (int i = j + 1; i < numRows; i++)
{
*(luPtr + j + i * numCols) /= *(luPtr + j + j * numCols);
}
}
}
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numCols; j++)
{
*(lDataPtr + j + i * numCols) = (j == i) ? 1 : (i < j) ? 0 : *(luPtr + j + i * numCols);
*(uDataPtr + j + i * numCols) = (i > j) ? 0 : *(luPtr + j + i * numCols);
}
}
l = new UMatrix(numRows, numCols, lData);
u = new UMatrix(numRows, numCols, uData);
}
return(new UMatrix(numRows, numCols, lu));
}
/// <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
}
/// <summary>Returns a sub matrix extracted from the current matrix.</summary>
/// <param name="startRow">Start row index</param>
/// <param name="endRow">End row index</param>
/// <param name="startColumn">Start column index</param>
/// <param name="endColumn">End column index</param>
public UMatrix Submatrix(int startRow, int endRow, int startColumn, int endColumn)
{
if ((startRow > endRow) || (startColumn > endColumn) || (startRow < 0) || (startRow >= this.numRows) || (endRow < 0) || (endRow >= this.numRows) || (startColumn < 0) || (startColumn >= this.numCols) || (endColumn < 0) || (endColumn >= this.numCols))
{
throw new ArgumentException("Argument out of range.");
}
UMatrix X = new UMatrix(endRow - startRow + 1, endColumn - startColumn + 1);
fixed (double* ptrX = X.data, ptrThis = data)
{
for (int i = startRow; i <= endRow; i++)
{
for (int j = startColumn; j <= endColumn; j++)
{
//X.data[j + (X.numCols * i)] = data[j + (numCols * i)];
(*(ptrX + j + (X.numCols * i))) = (*(ptrThis + j + (numCols * i)));
}
}
}
return X;
}
public UMatrix Inverse()
{
UMatrix upper, lower, c, z;
LuDecomposition(out lower, out upper);
c = new UMatrix(numRows, numCols, 1);
z = UMatrix.ForwardSub(lower, c);
return UMatrix.BackwardSub(upper, z);
}
/// <summary>Unary minus.</summary>
public static UMatrix Negate(UMatrix value)
{
if (value == null)
{
throw new ArgumentNullException("value");
}
int rows = value.numRows;
int columns = value.numCols;
UMatrix X = new UMatrix(rows, columns);
fixed (double* ptrX = X.data, ptrThis = value.data)
{
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < columns; j++)
{
//X.data[j + (columns * i)] = -value.data[j + (columns * i)];
(*(ptrX + j + (columns * i))) = -(*(ptrThis + j + (columns * i)));
}
}
}
return X;
}
public GaussianMixMappingQ(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover, int cellToUpdate)
{
//currentLaserPoseToRobot = laserRelativePositionToRover;
this.rangeToApply = cellToUpdate;
Matrix4 laser2RobotDCM = Matrix4.FromPose(laserRelativePositionToRover);
laserToRobotDCM = new UMatrix(4, 4);
robotToGlocalDCM = new UMatrix(4, 4);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
}
}
covRobotPose = new UMatrix(6, 6);
covLaserPose = new UMatrix(6, 6);
covLaserScan = new UMatrix(2, 2);
pij_sum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
pu_hat = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
pu_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
psig_u_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
uhatGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
sigSqrGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
laserHit = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
thresholdedHeightMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
resetCountMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
indexMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
this.numCellX = ocToUpdate.NumCellX;
this.numCellY = ocToUpdate.NumCellY;
#region Initial setup
double[] Qp_robot = new double[36]{0.1, 0, 0, 0, 0, 0,
0, 0.1, 0, 0, 0, 0,
0, 0, 0.1, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
double[] Qp_laser = new double[36]{0.001, 0, 0, 0, 0, 0,
0, 0.001, 0, 0, 0, 0,
0, 0, 0.001, 0, 0, 0,
0, 0, 0, 0.0001, 0, 0,
0, 0, 0, 0, 0.0001, 0,
0, 0, 0, 0, 0, 0.0001};
double[] Qr = new double[4] { .01 * .01, 0, 0, (0.1 * Math.PI / 180.0) * (0.1 * Math.PI / 180.0) };
// assign to our UMatrix
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6; j++)
{
covRobotPose[j, i] = Qp_robot[j * 6 + i];// *1e-6;
covLaserPose[j, i] = Qp_laser[j * 6 + i];// *1e-6;
}
}
covLaserScan[0, 0] = Qr[0];
covLaserScan[0, 1] = 0;
covLaserScan[1, 0] = 0;
covLaserScan[1, 1] = Qr[3];
#endregion
int k = 0;
sinLookupHokuyo = new double[682];
cosLookupHokuyo = new double[682];
for (double i = -120; i <= 120; i += 0.35190615835777126099706744868035)
{
sinLookupHokuyo[k] = (Math.Sin(i * Math.PI / 180.0));
cosLookupHokuyo[k] = (Math.Cos(i * Math.PI / 180.0));
k++;
}
laserHalfAngleHokuyo = 120;
MAXRANGEHokuyo = 5.0;
MINRANGEHokuyo = 0.5;
k = 0;
sinLookupSick = new double[361];
cosLookupSick = new double[361];
for (double i = -90; i <= 90; i += .5)
{
sinLookupSick[k] = (Math.Sin(i * Math.PI / 180.0));
cosLookupSick[k] = (Math.Cos(i * Math.PI / 180.0));
k++;
}
laserHalfAngleSick = 90;
MAXRANGESick = 30.0;
MINRANGESick = 0.5;
hokuyoStartIdx = 100;
hokuyoEndIdx = 500;
indicesList = new List<Index>();
heightList = new List<float>();
covList = new List<float>();
pijSumList = new List<float>();
laser2RobotTransMatrixDictionary = new Dictionary<int, Dictionary<int, UMatrix>>();
jacobianLaserPoseDictionary = new Dictionary<int, Dictionary<int, UMatrix>>();
indicesDictionary = new Dictionary<Index, int>();
}
/// <summary>
/// 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
}
/// <summary>Matrix subtraction.</summary>
public static UMatrix Subtract(UMatrix left, UMatrix right)
{
int rows = left.numRows;
int columns = left.numCols;
if ((rows != right.numRows) || (columns != right.numCols))
{
throw new ArgumentException("Matrix dimension do not match.");
}
UMatrix X = new UMatrix(rows, columns);
fixed (double* ptrX = X.data, ptrL = left.data, ptrR = right.data)
{
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < columns; j++)
{
//X.data[j + (columns * i)] = left.data[j + (columns * i)] - right.data[j + (columns * i)];
(*(ptrX + j + (columns * i))) = (*(ptrL + j + (columns * i))) - (*(ptrR + j + (columns * i)));
}
}
}
return X;
}
private void UpdateCovarianceQ(Matrix state)
{
UMatrix abc = new UMatrix(7, 7);
for (int i = 0; i < 7; i++)
{
for (int j = 0; j < 7; j++)
abc[i, j] = state[i, j];
}
covRobotPoseQ = abc;
}
//----------------------------------------------------------------------------
public UMatrix LuDecomposition(out UMatrix l, out UMatrix u)
{
double[] lu = new double[data.Length];
Array.Copy(data, lu, data.Length);
//double[] luRowI;
double* luRowIPtr;
double[] luColJ = new double[numRows];
double[] lData = new double[numRows * numRows];
double[] uData = new double[numRows * numRows];
fixed (double* luPtr = lu, luColJPtr = luColJ, lDataPtr = lData, uDataPtr = uData)
{
int pivotSign = 1;
int[] pivotVector = new int[numRows];
for (int i = 0; i < numRows; i++)
{
pivotVector[i] = i;
}
// Outer loop.
for (int j = 0; j < numCols; j++)
{
// Make a copy of the j-th column to localize references.
for (int i = 0; i < numRows; i++)
{
*(luColJPtr + i) = *(luPtr + j + i * numCols);
}
// Apply previous transformations.
for (int i = 0; i < numRows; i++)
{
luRowIPtr = luPtr + i * numCols;
// Most of the time is spent in the following dot product.
//int kmax = (i > j) ? j : i;
int kmax = Math.Min(i, j);
double s = 0.0;
for (int k = 0; k < kmax; k++)
{
s += *(luRowIPtr + k) * *(luColJPtr + k);
}
*(luRowIPtr + j) = *(luColJPtr + i) -= s;
}
// Find pivot and exchange if necessary.
int p = j;
for (int i = j + 1; i < numRows; i++)
{
if (Math.Abs(*(luColJPtr + i)) > Math.Abs(*(luColJPtr + p)))
{
p = i;
}
}
if (p != j)
{
for (int k = 0; k < numCols; k++)
{
double t = *(luPtr + k + p * numCols);
*(luPtr + k + p * numCols) = *(luPtr + k + j * numCols);
*(luPtr + k + j * numCols) = t;
}
int v = pivotVector[p];
pivotVector[p] = pivotVector[j];
pivotVector[j] = v;
pivotSign = -pivotSign;
}
// Compute multipliers.
if (j < numRows & *(luPtr + j + j * numCols) != 0.0)
{
for (int i = j + 1; i < numRows; i++)
{
*(luPtr + j + i * numCols) /= *(luPtr + j + j * numCols);
}
}
}
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numCols; j++)
{
*(lDataPtr + j + i * numCols) = (j == i) ? 1 : (i < j) ? 0 : *(luPtr + j + i * numCols);
*(uDataPtr + j + i * numCols) = (i > j) ? 0 : *(luPtr + j + i * numCols);
}
}
l = new UMatrix(numRows, numCols, lData);
u = new UMatrix(numRows, numCols, uData);
}
return new UMatrix(numRows, numCols, lu);
}
public GaussianMixMapping(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover)
{
//currentLaserPoseToRobot = laserRelativePositionToRover;
Matrix4 laser2RobotDCM = Matrix4.FromPose(laserRelativePositionToRover);
laserToRobotDCM = new UMatrix(4, 4);
robotToGlocalDCM = new UMatrix(4, 4);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
}
}
covRobotPose = new UMatrix(6, 6);
covLaserPose = new UMatrix(6, 6);
covLaserScan = new UMatrix(2, 2);
pij_sum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
pu_hat = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
pu_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
psig_u_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
uhatGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
sigSqrGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
laserHit = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
normalisedPijSum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
#region Initial setup
// initial setup
// these matrices were obtained from Jon using Vicon with Pioneer and HOKUYO laser
// these matrices need to be redefined
// these matrices are in row-wise
//double[] Qp_robot = new double[36]{0.0026, 0.0011, -0.0008, -0.0019, 0.0125, -0.0047,
// 0.0011, 0.0082, -0.0054, 0.0098, 0.0116, -0.0330,
// -0.0008, -0.0054, 0.0132, -0.0173, -0.0154, 0.0115,
// -0.0019, 0.0098, -0.0173, 0.0542, 0.0076, -0.0319,
// 0.0125, 0.0116, -0.0154, 0.0076, 0.0812, -0.0397,
// -0.0047, -0.0330, 0.0115, -0.0319, -0.0397, 0.1875};
//double[] Qp_laser = new double[36]{0.0077, -0.0009, -0.0016, -0.0127, -0.0415, -0.0011,
// -0.0009, 0.0051, -0.0013, -0.0035, 0.0045, 0.0197,
// -0.0016, -0.0013, 0.0101, -0.0167, 0.0189, 0.0322,
// -0.0127, -0.0035, -0.0167, 0.2669, -0.0548, -0.2940,
// -0.0415, 0.0045, 0.0189, -0.0548, 0.8129, 0.3627,
// -0.0011, 0.0197, 0.0322, -0.2940, 0.3627, 0.7474};
double[] Qp_robot = new double[36]{0.1, 0, 0, 0, 0, 0,
0, 0.1, 0, 0, 0, 0,
0, 0, 0.1, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
double[] Qp_laser = new double[36]{0.001, 0, 0, 0, 0, 0,
0, 0.001, 0, 0, 0, 0,
0, 0, 0.001, 0, 0, 0,
0, 0, 0, 0.0001, 0, 0,
0, 0, 0, 0, 0.0001, 0,
0, 0, 0, 0, 0, 0.0001};
double[] Qr = new double[4] { .01 * .01, 0, 0, (0.1 * Math.PI / 180.0) * (0.1 * Math.PI / 180.0) };
// assign to our UMatrix
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6; j++)
{
covRobotPose[j, i] = Qp_robot[j * 6 + i];// *1e-6;
covLaserPose[j, i] = Qp_laser[j * 6 + i];// *1e-6;
}
}
covLaserScan[0, 0] = Qr[0];
covLaserScan[0, 1] = 0;
covLaserScan[1, 0] = 0;
covLaserScan[1, 1] = Qr[3];
#endregion
//JTpl = ComputeJacobian(currentLaserPoseToRobot.yaw, currentLaserPoseToRobot.pitch, currentLaserPoseToRobot.roll);
// int thetaDegIndex = (int)((p.RThetaPoint.theta * 180.0/Math.PI) * 2) + 180;
int k = 0;
for (double i = -90; i <= 90; i += .5)
{
sinLookup[k] = (Math.Sin(i * Math.PI / 180.0));
cosLookup[k] = (Math.Cos(i * Math.PI / 180.0));
k++;
}
// initialize arrays
//pijToSend = new float[361 * rangeLong * rangeLong];
//largeUToSend = new float[361 * rangeLong * rangeLong];
//largeSigToSend = new float[361 * rangeLong * rangeLong];
//colIdxChange = new int[361 * rangeLong * rangeLong];
//rowIdxChange = new int[361 * rangeLong * rangeLong];
indicesList = new List<Index>();
heightList = new List<float>();
covList = new List<float>();
pijSumList = new List<float>();
laser2RobotTransMatrixDictionary = new Dictionary<int, UMatrix>();
jacobianLaserPoseDictionary = new Dictionary<int, UMatrix>();
indicesDictionary = new Dictionary<Index, int>();
}
/// <summary>Returns the transposed matrix.</summary>
public UMatrix Transpose()
{
UMatrix X = new UMatrix(numCols, numRows);
fixed (double* ptrX = X.data, ptrThis = data)
{
for (int i = 0; i < numRows; i++)
{
for (int j = 0; j < numCols; j++)
{
//X.data[i + (numRows * j)] = data[j + (numCols * i)];
(*(ptrX + i + (numRows * j))) = (*(ptrThis + j + (numCols * i)));
}
}
}
return X;
}
public UMatrix ComputeJacobian(double yaw, double pitch, double roll)
{
UMatrix m = new UMatrix(16, 6);
m.Zero();
double cp = Math.Cos(pitch);
double sp = Math.Sin(pitch);
double cy = Math.Cos(yaw);
double sy = Math.Sin(yaw);
double cr = Math.Cos(roll);
double sr = Math.Sin(roll);
m[0, 3] = -cp * sy;
m[0, 4] = -sp * cy;
m[1, 3] = -sp * sr * sy - cy * cr;
m[1, 4] = cp * sr * cy;
m[1, 5] = sp * cr * cy + sy * sr;
m[2, 3] = -sp * cr * sy + cy * sr;
m[2, 4] = cp * cr * cy;
m[2, 5] = -sp * sr * cy + sy * cr;
m[4, 3] = cp * cy;
m[4, 4] = -sp * sy;
m[5, 3] = sp * sr * cy - sy * cr;
m[5, 4] = cp * sr * sy;
m[5, 5] = sp * cr * sy - cy * sr;
m[6, 3] = sp * cr * cy + sy * sr;
m[6, 4] = cp * cr * sy;
m[6, 5] = -sp * sr * sy - cy * cr;
m[8, 4] = -cp;
m[9, 4] = -sp * sr;
m[9, 5] = cp * cr;
m[10, 4] = -sp * cr;
m[10, 5] = -cp * sr;
m[12, 0] = 1;
m[13, 1] = 1;
m[14, 2] = 1;
return m;
}