public static void NewScan(ts_scan_t scan, ts_position_t position)
{
//a.ts_distance_scan_to_map(scan, position);
if (a != null)
{
a.ts_map_update(scan, position, 50);
updates++;
}
}
public static void NewScan(ts_scan_t scan, ts_position_t position)
{
//a.ts_distance_scan_to_map(scan, position);
if (a != null)
{
a.ts_map_update(scan, position, 50);
updates++;
}
}
internal void ts_map_update(ts_scan_t scan, ts_position_t pos, int quality)
{
//cos sin quality
double c, s, q;
//Pos scan rotated
double x2p, y2p;
//x1y1: Pos scaled ||xpyp: Absolute pos scan
int i, x1, y1, x2, y2, xp, yp, value;
double add, dist;
c = Math.Cos(pos.theta * Math.PI / 180);
s = Math.Sin(pos.theta * Math.PI / 180);
x1 = (int)Math.Floor(pos.x * TS_MAP_SCALE + 0.5);
y1 = (int)Math.Floor(pos.y * TS_MAP_SCALE + 0.5);
// Translate and rotate scan to robot position
for (i = 0; i != scan.nb_points; i++)
{
//Before TS_HOLE_WIDTH
x2p = (c * scan.x[i] - s * scan.y[i]);
y2p = (s * scan.x[i] + c * scan.y[i]);
xp = (int)Math.Floor((pos.x + x2p) * TS_MAP_SCALE + 0.5);
yp = (int)Math.Floor((pos.y + y2p) * TS_MAP_SCALE + 0.5);
/*************************************************************************/
//CAUTION: If x2p or y2p are too high, the Sqrt operation will be very slow
double x2p_meter = x2p / 1000;
double y2p_meter = y2p / 1000;
dist = Math.Sqrt(x2p_meter * x2p_meter + y2p_meter * y2p_meter);
dist *= 1000;
/************************************************************************/
add = TS_HOLE_WIDTH / 2 / dist;
//After TS_HOLE_WIDTH
x2p *= TS_MAP_SCALE * (1 + add);
y2p *= TS_MAP_SCALE * (1 + add);
x2 = (int)Math.Floor(pos.x * TS_MAP_SCALE + x2p + 0.5);
y2 = (int)Math.Floor(pos.y * TS_MAP_SCALE + y2p + 0.5);
if (scan.value[i] == TS_NO_OBSTACLE)
{
q = quality / 2;
value = TS_NO_OBSTACLE;
}
else
{
q = quality;
value = TS_OBSTACLE;
}
ts_map_laser_ray(x1, y1, x2, y2, xp, yp, value, (int)q);
}
}
internal void ts_map_update(ts_scan_t scan, ts_position_t pos, int quality)
{
//cos sin quality
double c, s, q;
//Pos scan rotated
double x2p, y2p;
//x1y1: Pos scaled ||xpyp: Absolute pos scan
int i, x1, y1, x2, y2, xp, yp, value;
double add, dist;
c = Math.Cos(pos.theta * Math.PI / 180);
s = Math.Sin(pos.theta * Math.PI / 180);
x1 = (int)Math.Floor(pos.x * TS_MAP_SCALE + 0.5);
y1 = (int)Math.Floor(pos.y * TS_MAP_SCALE + 0.5);
// Translate and rotate scan to robot position
for (i = 0; i != scan.nb_points; i++)
{
//Before TS_HOLE_WIDTH
x2p = (c * scan.x[i] - s * scan.y[i]);
y2p = (s * scan.x[i] + c * scan.y[i]);
xp = (int)Math.Floor((pos.x + x2p) * TS_MAP_SCALE + 0.5);
yp = (int)Math.Floor((pos.y + y2p) * TS_MAP_SCALE + 0.5);
/*************************************************************************/
//CAUTION: If x2p or y2p are too high, the Sqrt operation will be very slow
double x2p_meter = x2p / 1000;
double y2p_meter = y2p / 1000;
dist = Math.Sqrt(x2p_meter * x2p_meter + y2p_meter * y2p_meter);
dist *= 1000;
/************************************************************************/
add = TS_HOLE_WIDTH / 2 / dist;
//After TS_HOLE_WIDTH
x2p *= TS_MAP_SCALE * (1 + add);
y2p *= TS_MAP_SCALE * (1 + add);
x2 = (int)Math.Floor(pos.x * TS_MAP_SCALE + x2p + 0.5);
y2 = (int)Math.Floor(pos.y * TS_MAP_SCALE + y2p + 0.5);
if (scan.value[i] == TS_NO_OBSTACLE)
{
q = quality / 2;
value = TS_NO_OBSTACLE;
}
else
{
q = quality;
value = TS_OBSTACLE;
}
ts_map_laser_ray(x1, y1, x2, y2, xp, yp, value, (int)q);
}
}
private int ts_distance_scan_to_map(ts_scan_t scan, ts_position_t pos)
{
double c, s;
int i, x, y, nb_points = 0;
Int64 sum;
c = Math.Cos(pos.theta * Math.PI / 180);
s = Math.Sin(pos.theta * Math.PI / 180);
// Translate and rotate scan to robot position
// and compute the distance
for (i = 0, sum = 0; i != scan.nb_points; i++)
{
if (scan.value[i] != TS_NO_OBSTACLE)
{
//ORIGINAL
//x = (int)Math.Floor((pos.x + c * scan.x[i] - s * scan.y[i]) * TS_MAP_SCALE + 0.5);
//y = (int)Math.Floor((pos.y + s * scan.x[i] + c * scan.y[i]) * TS_MAP_SCALE + 0.5);
//MINE
x = (int)Math.Floor((pos.x + c * scan.x[i] - s * scan.y[i]) * TS_MAP_SCALE + 0.5);
y = (int)Math.Floor((pos.y - s * scan.x[i] + c * scan.y[i]) * TS_MAP_SCALE + 0.5);
//Check boundaries
if (x >= 0 && x < TS_MAP_SIZE && y >= 0 && y < TS_MAP_SIZE)
{
sum += map.map[y * TS_MAP_SIZE + x];
//sum += Math.Abs(scan.value[i] - map.map[y * TS_MAP_SIZE + x]);
nb_points++;
}
}
}
if (nb_points > 0)
{
sum = sum * 1024 / nb_points;
}
else
{
sum = int.MaxValue;
}
return((int)sum);
}
private int ts_distance_scan_to_map(ts_scan_t scan, ts_position_t pos)
{
double c, s;
int i, x, y, nb_points = 0;
Int64 sum;
c = Math.Cos(pos.theta * Math.PI / 180);
s = Math.Sin(pos.theta * Math.PI / 180);
// Translate and rotate scan to robot position
// and compute the distance
for (i = 0, sum = 0; i != scan.nb_points; i++)
{
if (scan.value[i] != TS_NO_OBSTACLE)
{
//ORIGINAL
//x = (int)Math.Floor((pos.x + c * scan.x[i] - s * scan.y[i]) * TS_MAP_SCALE + 0.5);
//y = (int)Math.Floor((pos.y + s * scan.x[i] + c * scan.y[i]) * TS_MAP_SCALE + 0.5);
//MINE
x = (int)Math.Floor((pos.x + c * scan.x[i] - s * scan.y[i]) * TS_MAP_SCALE + 0.5);
y = (int)Math.Floor((pos.y - s * scan.x[i] + c * scan.y[i]) * TS_MAP_SCALE + 0.5);
//Check boundaries
if (x >= 0 && x < TS_MAP_SIZE && y >= 0 && y < TS_MAP_SIZE)
{
sum += map.map[y * TS_MAP_SIZE + x];
//sum += Math.Abs(scan.value[i] - map.map[y * TS_MAP_SIZE + x]);
nb_points++;
}
}
}
if (nb_points > 0)
{
sum = sum * 1024 / nb_points;
}
else
{
sum = int.MaxValue;
}
return (int)sum;
}
public ts_position_t ts_monte_carlo_search(ts_scan_t scan, ts_position_t start_pos, double sigma_xy, double sigma_theta, int stop, int bd, out int quality)
{
ts_position_t currentpos, bestpos, lastbestpos;
int currentdist;
int bestdist, lastbestdist;
int counter = 0;
if (stop < 0)
{
stop = -stop;
}
currentpos = new ts_position_t();
bestpos = new ts_position_t();
lastbestpos = new ts_position_t();
currentpos.x = lastbestpos.x = start_pos.x;
currentpos.y = lastbestpos.y = start_pos.y;
currentpos.theta = lastbestpos.theta = start_pos.theta;
currentdist = ts_distance_scan_to_map(scan, currentpos);
bestdist = lastbestdist = currentdist;
do
{
currentpos.x = lastbestpos.x;
currentpos.y = lastbestpos.y;
currentpos.theta = lastbestpos.theta;
//currentpos.x = ts_random_normal(currentpos.x, sigma_xy);
//currentpos.y = ts_random_normal(currentpos.y, sigma_xy);
//currentpos.theta = ts_random_normal(currentpos.theta, sigma_theta);
currentpos.x = randomPosition(currentpos.x, sigma_xy);
currentpos.y = randomPosition(currentpos.y, sigma_xy);
currentpos.theta = randomPosition(currentpos.theta, sigma_theta);
currentdist = ts_distance_scan_to_map(scan, currentpos);
if (currentdist < bestdist)
{
bestdist = currentdist;
bestpos.x = currentpos.x;
bestpos.y = currentpos.y;
bestpos.theta = currentpos.theta;
}
else
{
counter++;
}
if (counter > stop / 3)
{
if (bestdist < lastbestdist)
{
lastbestpos.x = bestpos.x;
lastbestpos.y = bestpos.y;
lastbestpos.theta = bestpos.theta;
lastbestdist = bestdist;
counter = 0;
sigma_xy *= 0.5;
sigma_theta *= 0.5;
}
}
} while (counter < stop);
if (bd != 0)
{
bd = bestdist;
}
quality = bd;
return(bestpos);
}
public static ts_position_t MonteCarlo_UpdatePosition(ts_scan_t scan, ts_position_t startPos, double sigma_xy, double sigma_theta, int stop, int bd, out int quality)
{
return(a.ts_monte_carlo_search(scan, startPos, sigma_xy, sigma_theta, stop, bd, out quality));
//return a.montecarlo_position;
}
public ts_position_t ts_monte_carlo_search(ts_scan_t scan, ts_position_t start_pos, double sigma_xy, double sigma_theta, int stop, int bd, out int quality)
{
ts_position_t currentpos, bestpos, lastbestpos;
int currentdist;
int bestdist, lastbestdist;
int counter = 0;
if (stop < 0)
{
stop = -stop;
}
currentpos = new ts_position_t();
bestpos = new ts_position_t();
lastbestpos = new ts_position_t();
currentpos.x = lastbestpos.x = start_pos.x;
currentpos.y = lastbestpos.y = start_pos.y;
currentpos.theta = lastbestpos.theta = start_pos.theta;
currentdist = ts_distance_scan_to_map(scan, currentpos);
bestdist = lastbestdist = currentdist;
do
{
currentpos.x = lastbestpos.x;
currentpos.y = lastbestpos.y;
currentpos.theta = lastbestpos.theta;
//currentpos.x = ts_random_normal(currentpos.x, sigma_xy);
//currentpos.y = ts_random_normal(currentpos.y, sigma_xy);
//currentpos.theta = ts_random_normal(currentpos.theta, sigma_theta);
currentpos.x = randomPosition(currentpos.x, sigma_xy);
currentpos.y = randomPosition(currentpos.y, sigma_xy);
currentpos.theta = randomPosition(currentpos.theta, sigma_theta);
currentdist = ts_distance_scan_to_map(scan, currentpos);
if (currentdist < bestdist)
{
bestdist = currentdist;
bestpos.x = currentpos.x;
bestpos.y = currentpos.y;
bestpos.theta = currentpos.theta;
}
else
{
counter++;
}
if (counter > stop / 3)
{
if (bestdist < lastbestdist)
{
lastbestpos.x = bestpos.x;
lastbestpos.y = bestpos.y;
lastbestpos.theta = bestpos.theta;
lastbestdist = bestdist;
counter = 0;
sigma_xy *= 0.5;
sigma_theta *= 0.5;
}
}
} while (counter < stop);
if (bd != 0)
bd = bestdist;
quality = bd;
return bestpos;
}
private ts_scan_t doScan()
{
ts_scan_t scan = new ts_scan_t();
int j = 0;
int angle = 0;
//CENTRAL SENSOR:
if (lastCentral > 0)
{
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (float)(angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(lastCentral * s);
scan.y[j] = (float)(lastCentral * c + 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_OBSTACLE;
}
}
else if (lastCentral == -1)
{
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (float)(angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(800 * s);
scan.y[j] = (float)(800 * c + 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_NO_OBSTACLE;
}
}
//WALL SENSOR
if (lastWall > 0)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(lastWall * c + 185 / 2) * (-1);
scan.y[j] = (float)(230 / 2 + lastWall * s) * (-1);
scan.value[j] = SLAMAlgorithm.TS_OBSTACLE;
}
}
else if (lastWall == -1)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(300 * c + 185 / 2) * (-1);
scan.y[j] = (float)(230 / 2 + 300 * s) * (-1);
scan.value[j] = SLAMAlgorithm.TS_NO_OBSTACLE;
}
}
//WALLBACK SENSOR
if (lastWallBack > 0)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(lastWallBack * c + 185 / 2) * (-1);
scan.y[j] = (float)(lastWallBack * s - 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_OBSTACLE;
}
}
else if (lastWallBack == -1)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double c = Math.Cos(angleRad);
double s = Math.Sin(angleRad);
scan.x[j] = (float)(300 * c + 185 / 2) * (-1);
scan.y[j] = (float)(300 * s - 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_NO_OBSTACLE;
}
}
//RIGHT SENSOR
if (lastRight > 0)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double rad45 = (45 * Math.PI / 180);
double c = Math.Cos(angleRad + rad45);
double s = Math.Sin(angleRad + rad45);
scan.x[j] = (float)(lastRight * c + 185 / 2);
scan.y[j] = (float)(lastRight * s + 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_OBSTACLE;
}
}
else if (lastRight == -1)
{
angle = 0;
angle -= 10;
for (int i = 0; i < 20; angle++, i++, j++)
{
double angleRad = (angle * Math.PI / 180);
double rad45 = (45 * Math.PI / 180);
double c = Math.Cos(angleRad + rad45);
double s = Math.Sin(angleRad + rad45);
scan.x[j] = (float)(800 * c + 185 / 2);
scan.y[j] = (float)(800 * s + 230 / 2) * (-1);
scan.value[j] = SLAMAlgorithm.TS_NO_OBSTACLE;
}
}
scan.nb_points = (ushort)j;
return scan;
}
public static ts_position_t MonteCarlo_UpdatePosition(ts_scan_t scan,ts_position_t startPos,double sigma_xy,double sigma_theta,int stop, int bd,out int quality)
{
return a.ts_monte_carlo_search(scan, startPos, sigma_xy, sigma_theta, stop, bd, out quality);
//return a.montecarlo_position;
}