public SLAMAlgorithm()
{
montecarlo_position = new ts_position_t();
random = new ts_randomizer_t();
this.ts_random_init(random, 0xdead);
this.ts_map_init();
}
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;
}
void MonteCarlo_UpdatePosition(Object state)
{
ts_position_t position = new ts_position_t();
position.theta = (float)(actualPosition.angle * 180 / Math.PI);
position.x = 1000 * actualPosition.x + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
position.y = 1000 * actualPosition.y + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
ts_scan_t scan = this.doScan();
int quality;
ts_position_t newPos = tinySLAM.MonteCarlo_UpdatePosition(scan, position, 1, 1, 10000, 50, out quality);
Position p = new Position();
p.x = (newPos.x - SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2)) * 1e-3;
p.y = (newPos.y - SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2)) * 1e-3;
p.angle = newPos.theta * Math.PI / 180;
Position diff = p - actualPosition;
//Only the new position will be sent if is near of the actual position
if (diff.x < 3e-2 && diff.y < 3e-2 && diff.angle < 5 * Math.PI / 180)
coder.Send(Message.UpdatePosition, p);
}
public void Scan(object state)
{
UpdaterInt d = new UpdaterInt(updateIterationsLabel);
this.Dispatcher.Invoke(d, tinySLAM.NumUpdates());
ts_scan_t scan = this.doScan();
ts_position_t position = new ts_position_t();
position.theta = (float)(actualPosition.angle * 180 / Math.PI);
position.x = 1000 * actualPosition.x + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
position.y = 1000 * actualPosition.y + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
tinySLAM.NewScan(scan, position);
}
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 SLAMAlgorithm()
{
montecarlo_position = new ts_position_t();
random = new ts_randomizer_t();
this.ts_random_init(random, 0xdead);
this.ts_map_init();
}
