public VFHFollower(SensorPose sickPose, SensorPose hokuyoPose, double angularResolution, bool useHokuyo)
{
pose = null;
this.useHokuyo = useHokuyo;
this.sickPose = sickPose;
sickDCM = Matrix4.FromPose(this.sickPose);
if (this.useHokuyo)
{
this.hokuyoPose = hokuyoPose;
hokuyoDCM = Matrix4.FromPose(this.hokuyoPose);
}
sickHistory = new List<Vector2[]>();
hokuyoHistory = new List<Vector2[]>();
this.angularResolution = angularResolution;
numBuckets = (int) Math.Round(360 / this.angularResolution);
trapped = false;
kThreshold = (int) Math.Round(160 / this.angularResolution);
rHistogram = new Double[numBuckets];
}
public void SetSensorPose(SensorPose laserPose, bool forCamera)
{
laserToRobot = Matrix4.FromPose(laserPose);
if (forCamera)
laserToRobot[2, 3] = 0;
//laserToRobot[0, 2] = laserPose.x;
//laserToRobot[1, 2] = laserPose.y;
//laserToRobot[2, 2] = 0;
//laserToRobot[3, 3] = 1.0;
}
double thresholdHeight; // above 5 cm
#endregion Fields
#region Constructors
public LidarTargetDetector(double thresholdHeight, SensorPose laserPose, bool forCamera)
{
this.thresholdHeight = thresholdHeight;
laserToRobot = Matrix4.FromPose(laserPose);
if (forCamera)
laserToRobot[2, 3] = 0;
//laserToRobot[0, 2] = laserPose.x;
//laserToRobot[1, 2] = laserPose.y;
//laserToRobot[2, 2] = laserPose.z;
//laserToRobot[3, 3] = 1.0;
}
public 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 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 static Matrix4 YPR(double yaw, double pitch, double roll)
{
double cy = Math.Cos(yaw), sy = Math.Sin(yaw);
double cp = Math.Cos(pitch), sp = Math.Sin(pitch);
double cr = Math.Cos(roll), sr = Math.Sin(roll);
//Matrix4 rz = new Matrix4(
// cy, -sy, 0, 0,
// sy, cy, 0, 0,
// 0, 0, 1, 0,
// 0, 0, 0, 1
// );
//Matrix4 ry = new Matrix4(
// cp, 0, sp, 0,
// 0, 1, 0, 0,
// -sp, 0, cp, 0,
// 0, 0, 0, 1
// );
//Matrix4 rx = new Matrix4(
// 1, 0, 0, 0,
// 0, cr, -sr, 0,
// 0, sr, cr, 0,
// 0, 0, 0, 1
// );
Matrix4 rz = new Matrix4(
cy, sy, 0, 0,
-sy, cy, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
);
Matrix4 ry = new Matrix4(
cp, 0, -sp, 0,
0, 1, 0, 0,
sp, 0, cp, 0,
0, 0, 0, 1
);
Matrix4 rx = new Matrix4(
1, 0, 0, 0,
0, cr, sr, 0,
0, -sr, cr, 0,
0, 0, 0, 1
);
return rx * ry * rz;
}
public static Matrix4 TransformationMatrix(double dx, double dy, double dz, double ez, double ey, double ex, bool useDCMTranspose)
{
Matrix4 T = new Matrix4();
Matrix3 dcm = Matrix3.DCM(ez, ey, ex);
//Assign the full transformation matrix
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
//Test if using the dcm transpose
if (useDCMTranspose)
T[i, j] = dcm[j, i];
else
T[i, j] = dcm[i, j];
}
}
//Assign the translation
T[0, 3] = dx;
T[1, 3] = dy;
T[2, 3] = dz;
T[3, 3] = 1.0;
return T;
}
public static Matrix4 FromSubmatrix(
Matrix2 A, Matrix2 B,
Matrix2 C, Matrix2 D)
{
Matrix4 ret = new Matrix4();
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < 2; j++)
{
ret[i, j] = A[i, j];
ret[i + 2, j] = C[i, j];
ret[i, j + 2] = B[i, j];
ret[i + 2, j + 2] = D[i, j];
}
}
return ret;
}
public static Matrix4 operator *(Matrix4 l, Matrix4 r)
{
// [ ld0*rd0+ld1*rd4+ld2*rd8+ld3*rd12, ld0*rd1+ld1*rd5+ld2*rd9+ld3*rd13, ld0*rd2+ld1*rd6+ld2*rd10+ld3*rd14, ld0*rd3+ld1*rd7+ld2*rd11+ld3*rd15]
// [ ld4*rd0+ld5*rd4+ld6*rd8+ld7*rd12, ld4*rd1+ld5*rd5+ld6*rd9+ld7*rd13, ld4*rd2+ld5*rd6+ld6*rd10+ld7*rd14, ld4*rd3+ld5*rd7+ld6*rd11+ld7*rd15]
// [ ld8*rd0+ld9*rd4+ld10*rd8+ld11*rd12, ld8*rd1+ld9*rd5+ld10*rd9+ld11*rd13, ld8*rd2+ld9*rd6+ld10*rd10+ld11*rd14, ld8*rd3+ld9*rd7+ld10*rd11+ld11*rd15]
// [ ld12*rd0+ld13*rd4+ld14*rd8+ld15*rd12, ld12*rd1+ld13*rd5+ld14*rd9+ld15*rd13, ld12*rd2+ld13*rd6+ld14*rd10+ld15*rd14, ld12*rd3+ld13*rd7+ld14*rd11+ld15*rd15]
Matrix4 ret = new Matrix4();
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
ret[i, j] = l[i, 0] * r[0, j] + l[i, 1] * r[1, j] + l[i, 2] * r[2, j] + l[i, 3] * r[3, j];
}
}
return ret;
}
