Update of /cvsroot/playerstage/code/player/server/drivers/mixed/mricp/src
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv6130/server/drivers/mixed/mricp/src
Added Files:
lasermodel.cpp geometry2D.cpp map.cpp mricp_driver.cpp icp.cpp
nn.cpp Timer.cpp
Log Message:
added mricp driver, thanks to Tarek Taha
--- NEW FILE: nn.cpp ---
/* K-nearest-neighbours for 2D point-sets.
* Tim Bailey 2004.
*/
#include "nn.h"
#include "geometry2D.h"
#include <algorithm>
#include <cmath>
#include <cassert>
using namespace Geom2D;
using namespace std;
SweepSearch::SweepSearch(const vector<Point> &p, double lim) : limit(lim)
// Input: set of points, p, and maximum search distance, lim.
// Function: create data structure for nearest-neighbour search.
{
// store points and their order
dataset.reserve(p.size());
for (int i=0; i < (int)p.size(); ++i)
dataset.push_back(PointIdx(p[i], i));
// sort points
sort(dataset.begin(), dataset.end(), yorder);
}
int SweepSearch::query(const Point& q) const
// Input: query point.
// Output: index of nearest-neighbour.
{
double d2min = sqr(limit);
int idxmin = NOT_FOUND;
// get upper bound
PointIdx qi(q, 0);
vector<PointIdx>::const_iterator upper =
upper_bound(dataset.begin(), dataset.end(), qi, yorder);
vector<PointIdx>::const_iterator i;
// search backwards
double min_y = q.y - limit;
for (i = upper-1; i >= dataset.begin() && i->p.y > min_y; --i)
if (is_nearer(d2min, idxmin, q, *i))
min_y = q.y - sqrt(d2min);
// search forward
double max_y = q.y+sqrt(d2min);
for (i = upper; i < dataset.end() && i->p.y < max_y; ++i)
if (is_nearer(d2min, idxmin, q, *i))
max_y = q.y+sqrt(d2min);
return idxmin;
}
inline
bool SweepSearch::is_nearer(double &d2min, int &idxmin, const Point &q, const
PointIdx &pi) const
// Check whether new point is closer than previous points. If so, update d2min
and idxmin.
{
double d2 = dist_sqr(q, pi.p);
if (d2 < d2min) {
d2min = d2;
idxmin = pi.i;
return true;
}
return false;
}
std::vector<double>& SweepSearch::query(const Point &q, std::vector<int> &idx)
// Input: query point and vector<int> of size k (number of neighbours)
// Output: vector<int> indices of nearest-neighbours.
// Returns: set of distances to k nn's.
{
// can't have more nn's than reference
points
assert(idx.size() > 0);
int size = idx.size();
if ((int)dataset.size() < size) {
size = dataset.size();
idx.resize(size);
}
if ((int)nndists.size() != size)
nndists.resize(size);
// initialise set
for (int j=0; j<size; ++j) {
idx[j] = NOT_FOUND;
nndists[j] = sqr(limit);
}
// get upper bound
PointIdx qi(q, 0);
vector<PointIdx>::const_iterator di =
upper_bound(dataset.begin(), dataset.end(), qi, yorder);
vector<PointIdx>::const_iterator i;
// sweep forwards
double max_y = q.y + limit;
for (i = di; i < dataset.end() && i->p.y < max_y; ++i)
if (insert_neighbour(q, *i, nndists, idx))
max_y = q.y + sqrt(nndists[size-1]);
// sweep backwards
double min_y = q.y - sqrt(nndists[size-1]);
for (i = di-1; i >= dataset.begin() && i->p.y > min_y; --i)
if (insert_neighbour(q, *i, nndists, idx))
min_y = q.y - sqrt(nndists[size-1]);
// convert square distances to distances
{ for (int i=0; i<size; ++i)
nndists[i] = sqrt(nndists[i]);
}
return nndists;
}
bool SweepSearch::insert_neighbour(const Point &q, const PointIdx &pi,
std::vector<double> &nndists, std::vector<int> &idx)
// Check if new point is closer than any of the existing near-neighbours. If so,
// place the new point into near-neighbour set (in ascending order).
{
const int size = nndists.size();
double d2 = dist_sqr(q, pi.p);
if (d2 >= nndists[size-1]) // no improvement possible
return false;
int i;
for (i=0; d2 >= nndists[i]; ++i) // find location for new neighbour
;
for (int j = size-1; j > i; --j) { // shuffle in order
nndists[j] = nndists[j-1];
idx[j] = idx[j-1];
}
nndists[i] = d2;
idx[i] = pi.i;
return true;
}
--- NEW FILE: geometry2D.cpp ---
/* Simple 2D geometric operations with points, poses, and lines.
* Tim Bailey 2004.
*/
#include "geometry2D.h"
#include <cmath>
#include <cassert>
using namespace std;
namespace Geom2D {
#if 0
double pi_to_pi(double angle)
// An alternative implementation that uses fmod() rather than while-loops.
{
angle = fmod(angle, 2.*PI);
if (angle < -PI)
angle += 2.*PI;
else if (angle > PI)
angle -= 2.*PI;
return angle;
}
#endif
double dist(const Point& p, const Point& q)
// Distance between two points.
{
return sqrt(dist_sqr(p,q));
}
Pose compute_relative_pose(const vector<Point>& a, const vector<Point>& b)
// Determine the relative Pose that best aligns two point-sets.
// INPUTS: a, b are two equal-length sets of points, such that a[i] aligns with
b[i].
// OUTPUT: Best-fit alignment.
//
// Closed-form algorithm from Appendix C of:
// F. Lu and E. Milios. Robot pose estimation in unknown environments by
matching
// 2D range scans. Journal of Intelligent and Robotic Systems, 18:249�275,
1997.
{
assert(a.size() == b.size() && a.size() != 0);
int n= a.size();
double x1,x2,y1,y2,xx,yy,xy,yx;
x1=x2=y1=y2=xx=yy=xy=yx=0.0;
for (int i=0; i<n; ++i) { // calculate sums
const Point& p1 = a[i];
const Point& p2 = b[i];
x1 += p1.x;
x2 += p2.x;
y1 += p1.y;
y2 += p2.y;
xx += p1.x*p2.x;
yy += p1.y*p2.y;
xy += p1.x*p2.y;
yx += p1.y*p2.x;
}
double N = static_cast<double>(n);
double Sxx = xx - x1*x2/N; // calculate S
double Syy = yy - y1*y2/N;
double Sxy = xy - x1*y2/N;
double Syx = yx - y1*x2/N;
double xm1 = x1/N; // calculate means
double xm2 = x2/N;
double ym1 = y1/N;
double ym2 = y2/N;
double phi = atan2(Sxy-Syx, Sxx+Syy);
Pose pse; // calculate pose
pse.p.x = xm2 - (xm1*cos(phi) - ym1*sin(phi));
pse.p.y = ym2 - (xm1*sin(phi) + ym1*cos(phi));
pse.phi = phi;
return pse;
}
Transform2D::Transform2D(const Pose& ref) : base(ref)
{
c = cos(ref.phi);
s = sin(ref.phi);
}
bool intersection_line_line (Point& p, const Line& l, const Line& m)
// Compute the intersection point of two lines.
// Returns false for parallel lines.
{
double gl, gm, bl, bm;
bool lVert = true, mVert = true;
// calculate gradients
if ((gl = l.second.x - l.first.x) != 0.0) {
gl = (l.second.y - l.first.y)/gl;
lVert = false;
}
if ((gm = m.second.x - m.first.x) != 0.0) {
gm = (m.second.y - m.first.y)/gm;
mVert = false;
}
if (lVert == mVert) { // check for parallelism
if (gl == gm)
return false;
}
bl = l.first.y - gl*l.first.x; // calculate y intercepts
bm = m.first.y - gm*m.first.x;
if (lVert) { // calculate intersection
p.x = l.first.x;
p.y = gm*p.x + bm;
}
else if (mVert) {
p.x = m.first.x;
p.y = gl*p.x + bl;
}
else {
p.x = (bm - bl)/(gl - gm);
p.y = gm*p.x + bm;
}
return true;
}
double distance_line_point (const Line& lne, const Point& p)
// Note: distance is -ve if point is on left of line and +ve if it is on
// the right (when looking from first to second).
{
Point v;
v.x= lne.second.x - lne.first.x;
v.y= lne.second.y - lne.first.y;
return ((lne.second.y - p.y)*v.x
- (lne.second.x - p.x)*v.y)
/ sqrt(v.x*v.x + v.y*v.y);
}
void intersection_line_point(Point& p, const Line& l, const Point& q)
// Compute the perpendicular intersection from a point to a line
{
Line m; // create line through q perpendicular to l
m.first = q;
m.second.x = q.x + (l.second.y - l.first.y);
m.second.y = q.y - (l.second.x - l.first.x);
bool not_parallel = intersection_line_line(p, l, m);
//assert(not_parallel);
if (not_parallel == 0)
printf("\n Lines are parallel !!!");
}
} // namespace Geom2D
--- NEW FILE: map.cpp ---
/***************************************************************************
* Copyright (C) 2005 by Tarek Taha *
* [EMAIL PROTECTED] *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include "map.h"
#include <math.h>
#include <cassert>
#include <iostream>
using namespace std;
MAP::MAP(char * Mapname, double pixel_size, double map_size)
{
this->Mapname = Mapname;
this->pixel_size = pixel_size;
this->negate = TRUE;
this->occ_grid = NULL;
this->pixbuf = NULL;
this->size_x = (int)ceil(map_size/pixel_size); // Square MAP
this->size_y = this->size_x + 1;
}
void MAP::ResetProb()
{
if (!this->occ_grid)
{
cout<<"\n Occupancy Grid not Allocated Yet!!!";
return;
}
for (int i=0; i < this->size_x; i++)
for(int j =0; j < this->size_y;j++)
{
occ_grid[i][j].prob_occ = occ_grid[i][j].prob_free =
double(1)/3;
occ_grid[i][j].added =false;
}
}
GdkPixbuf * MAP::CreateMap()
{
g_type_init();// Initialize glib
if (this->occ_grid)
{
for (int i=0; i < this->size_x; i++)
delete [] this->occ_grid[i];
delete [] this->occ_grid;
}
if (!this->pixbuf)
{
cout<<"\n --->>> Creating Image Pixel
Buffer:"<<this->Mapname;
fflush(stdout);
if(!(this->pixbuf = gdk_pixbuf_new (GDK_COLORSPACE_RGB, FALSE,
8, this->size_x,this->size_y))) // Read the image
{
printf("\nfailed to Create Map Buffer %s",
this->Mapname);
return(NULL);
}
gdk_pixbuf_fill (pixbuf, 0x7f7f7f00);
}
assert(this->size_x == gdk_pixbuf_get_width(this->pixbuf));
assert(this->size_y == gdk_pixbuf_get_height(this->pixbuf));
if (!(this->occ_grid= new mapgrid * [size_x]))
perror("\nCould not allocated the requested memory space");
for(int m=0;m<size_x;m++)
{
if(!(this->occ_grid[m] = new mapgrid [size_y]))
perror("\nCould not allocated the requested memory
space");
}
ClearData();
printf("\n --->>> MAP Created with Height=%d Width=%d
Resolution=%.3f <<<---",this->size_y, this->size_x, this->pixel_size);
fflush(stdout);
return this->pixbuf;
}
void MAP::ClearData()
{
for (int i=0; i < this->size_x; i++)
for(int j =0; j < this->size_y;j++)
{
this->occ_grid[i][j].prob_occ = this->occ_grid[i][j].prob_free
= 1/3;
}
if(pixbuf)
gdk_pixbuf_fill (pixbuf, 0x7f7f7f00);
}
MAP::~ MAP()
{
for (int i=0; i < this->size_x; i++)
delete [] this->occ_grid[i];
delete [] this->occ_grid;
if(this->pixbuf)
gdk_pixbuf_unref(this->pixbuf);
cout <<"\n <<<--- IMAGE DATA FREED <<<---";
fflush(stdout);
};
MapInfo MAP::GetMapInfo()
{
MapInfo mapinfo;
if(this->occ_grid == NULL)
{
mapinfo.pixel_size=-1;
return mapinfo;
}
mapinfo.width = this->size_x;
mapinfo.height =this->size_y;
mapinfo.pixel_size=this->pixel_size;
return mapinfo;
};
void MAP::DrawPixel (int red,int green,int blue,int i,int j)
{
int rowstride=0, n_channels, bps;
guchar *pixels;
guchar * p;
rowstride = gdk_pixbuf_get_rowstride(this->pixbuf);
bps = gdk_pixbuf_get_bits_per_sample(this->pixbuf)/8;
n_channels = gdk_pixbuf_get_n_channels(this->pixbuf);
pixels = gdk_pixbuf_get_pixels(this->pixbuf);
if(gdk_pixbuf_get_has_alpha(this->pixbuf))
n_channels++;
p= pixels +j*rowstride + i*n_channels;
p[0]=red;
p[1]=green;
p[2]=blue;
//p[3]=;
return;
}
void MAP::SavePixelBufferToFile()
{
char command[40],filename[40];
struct stat stat_buf;
if(!this->pixbuf)
{
cout<<" --->>> Nothing To SAVE Buffer Empty !!! ";
return;
}
sprintf(filename,"%s%s",this->Mapname,".png");
// Delete the file if it exists
if (stat(filename,&stat_buf) != 0 || (stat_buf.st_mode & S_IFMT) ==
S_IFREG)
{
sprintf(command,"%s%s","rm -f -r ",filename);
if(system(command)==-1)
{
perror("\nError Happened while trying to Delete
Existing File");
exit(1);
}
else
cout<<"\n --->>> Map already existed with the
same name : Deleted Successfully";
}
cout<<"\n --->>> Saving the map into: "<<filename; fflush(stdout);
// Save the file
gdk_pixbuf_save(this->pixbuf,filename,"png",NULL,NULL);
cout<<"\n --->>> PIXEL BUFFER SAVED <<<--- ";
fflush(stdout);
};
// Save as PGM format
int MAP::SavePgm()
{
char filename[40];
int i, j;
signed char c;
unsigned char d;
FILE *file;
sprintf(filename,"%s%s",this->Mapname,".pgm");
file = fopen(filename, "w+");
if (file == NULL)
{
fprintf(stderr, "error writing to %s ", filename);
return -1;
}
fprintf(file, "P5 %d %d 255\n",this->size_x , this->size_y -1);
for (j = 0; j < this->size_y-1; j++)
{
for (i = 0; i < this->size_x; i++)
{
c = (int)(occ_grid[i][j].prob_occ * 255.0);
d = (unsigned char) (255 - c);
fwrite(&d, 1, 1, file);
}
}
cout<<"\n --->>> Pgm Map Saved <<<--- "; fflush(stdout);
fclose(file);
return 0;
};
--- NEW FILE: Timer.cpp ---
#include "Timer.h"
Timer::Timer()
{
gettimeofday(&start_time,NULL);
}
double Timer::TimeElapsed() // return in usec
{
gettimeofday(&end_time,NULL);
time_diff = ((double) end_time.tv_sec*1e6 + (double)end_time.tv_usec)
-
((double) start_time.tv_sec*1e6 +
(double)start_time.tv_usec);
return time_diff;
}
Timer::~Timer()
{
}
void Timer::Reset()
{
gettimeofday(&start_time,NULL);
}
void Timer::Synch(double period)
{
double time_elapsed = this->TimeElapsed();
if( time_elapsed < period*1e3)
usleep((int)(period*1e3 -time_elapsed));
}
--- NEW FILE: mricp_driver.cpp ---
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000-2003
* Andras Szekely, Aaron Skelsey, Kent Williams
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
[...1392 lines suppressed...]
// Perform the ICP on Next Laser Scan
laser_set_1 = laser_set_2;
if(this->position_device) pose_1 = pose_2;
};
void MrIcpDriver::ConnectPatches()
{
int patch_number=0;
gchar * patch;
patch = g_strdup_printf("%sMAP_PATCH%d.png",map_path,patch_number++);
while(is_file(patch))
{
cout<<patch<<endl; fflush(stdout);
patch =
g_strdup_printf("%sMAP_PATCH%d.png",map_path,patch_number++);
}
}
--- NEW FILE: icp.cpp ---
/* Iterated closest point (ICP) algorithm.
* Tim Bailey 2004.
*/
#include "icp.h"
#include <cassert>
#include <iostream>
using namespace std;
namespace Geom2D
{
ICP::ICP()
{
index.push_back(0);
index.push_back(0);
};
ICP::~ICP()
{
a.clear();
b.clear();
index.clear();
ref.clear();
}
Pose ICP::align(vector<Point> reference, vector<Point> obs,Pose init, double
gate, int nits, bool interp)
{
ref = reference;
nn = new SweepSearch(reference, gate);
Pose pse = init;
double gate_sqr = sqr(gate);
int size_obs = obs.size();
// used if interp == true
while (nits-- > 0)
{
Transform2D tr(pse);
a.clear();
b.clear();
// For each point in obs, find its NN in ref
for (int i = 0; i < size_obs; ++i)
{
Point p = obs[i];
tr.transform_to_global(p); // transform obs[i] to
estimated ref coord-frame
Point q;
// simple ICP
if (interp == false)
{
int idx = nn->query(p);
if (idx == SweepSearch::NOT_FOUND)
continue;
q = ref[idx];
}
// ICP with interpolation between 2 nearest points in
ref
else
{
(void) nn->query(p, index);
assert(index.size() == 2);
if (index[1] == SweepSearch::NOT_FOUND)
continue;
Line lne;
lne.first = ref[index[0]];
lne.second = ref[index[1]];
intersection_line_point(q, lne, p);
}
if (dist_sqr(p,q) < gate_sqr) // check if NN is close
enough
{
a.push_back(obs[i]);
b.push_back(q);
}
}
//cout<<"\nObs size:"<<obs.size()<<" Ref:"<<ref.size()<<"
Paired set size:"<<a.size();
//If Less than half of the Observation is paired => not good
alignment
if( a.size() < obs.size()/2.0)
{
cout<<"\n Detected Possible misalignment --- Skipping
this Laser Set! Gate is:"<<gate;;
pse.p.x = -1 ; pse.p.y=-1 ; pse.phi = -1;
break;
}
if (a.size() > 2)
pse = compute_relative_pose(a, b); // new iteration
result
}
delete nn;
return pse;
}
} // namespace Geom2D
--- NEW FILE: lasermodel.cpp ---
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include "map.h"
#include "lasermodel.h"
#define LASER_MAX_RANGES 401
// Create an sensor model
LaserModel::LaserModel()
{
//Empty Constructor
};
LaserModel::LaserModel(mapgrid * * map)
{
this->map = map;
this->range_cov = 0.10 * 0.10;
this->range_bad = 0.50;
PreCompute();
this->range_count = 0;
this->ranges = (laser_range_t *)calloc(LASER_MAX_RANGES,
sizeof(laser_range_t));
};
// Free an sensor model
LaserModel::~LaserModel()
{
if(this->lut_probs)
free(this->lut_probs);
if(this->ranges)
free(this->ranges);
return;
}
// Clear all existing range readings
void LaserModel::ClearRanges()
{
this->range_count = 0;
return;
}
// Set the laser range readings that will be used.
void LaserModel::AddRange(double range, double bearing)
{
laser_range_t *beam;
assert(this->range_count < LASER_MAX_RANGES);
beam = this->ranges + this->range_count++;
beam->range = range;
beam->bearing = bearing;
return;
}
// Pre-compute the range sensor probabilities.
// We use a two-dimensional array over the O.G
void LaserModel::PreCompute()
{
double max;
double c, z, p;
double mrange, orange;
int i, j;
// Laser max range and resolution
max = 8.00;
this->lut_res = 0.01;
this->lut_size = (int) ceil(max / this->lut_res);
this->lut_probs = (double *)malloc(this->lut_size * this->lut_size *
sizeof(this->lut_probs[0]));
for (i = 0; i < this->lut_size; i++)
{
mrange = i * this->lut_res;
for (j = 0; j < this->lut_size; j++)
{
orange = j * this->lut_res;
// TODO: proper sensor model (using Kolmagorov?)
// Simple gaussian model
c = this->range_cov;
z = orange - mrange;
p = this->range_bad + (1 - this->range_bad) * exp(-(z * z) / (2
* c));
//printf("%f %f %f\n", orange, mrange, p);
//assert(p >= 0 && p <= 1.0);
this->lut_probs[i + j * this->lut_size] = p;
}
//printf("\n");
}
return;
}
// Determine the probability for the given range reading
double LaserModel::RangeProb(double obs_range, double map_range)
{
int i, j;
double p;
i = (int) (map_range / this->lut_res + 0.5);
j = (int) (obs_range / this->lut_res + 0.5);
assert(i >= 0);
if (i >= this->lut_size)
i = this->lut_size - 1;
assert(j >= 0);
if (j >= this->lut_size)
j = this->lut_size - 1;
p = this->lut_probs[i + j * this->lut_size];
//assert(p >= 0 && p <= 1.0);
return p;
}
// Determine the probability for the given pose
double LaserModel::PoseProb()
{
int i;
double p;
double map_range;
laser_range_t *obs;
p = 1.0;
for (i = 0; i < this->range_count; i++)
{
obs = this->ranges + i;
map_range = 1; //TODO
if (obs->range >= 8.0 && map_range >= 8.0)
p *= 1.0;
else if (obs->range >= 8.0 && map_range < 8.0)
p *= this->range_bad;
else if (obs->range < 8.0 && map_range >= 8.0)
p *= this->range_bad;
else
p *= RangeProb(obs->range, map_range);
}
//printf("%e\n", p);
assert(p >= 0);
return p;
}
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2005.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
