Update of /cvsroot/playerstage/code/player/server/drivers/mixed/mricp/include
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv6130/server/drivers/mixed/mricp/include
Added Files:
map.h nn.h lasermodel.h geometry2D.h icp.h Timer.h
Log Message:
added mricp driver, thanks to Tarek Taha
--- NEW FILE: Timer.h ---
#ifndef TIMER_H_
#define TIMER_H_
#include <sys/time.h>
#include <iostream>
class Timer
{
private:
struct timeval start_time,end_time;
double time_diff;
public:
Timer();
double TimeElapsed(); // time elapsed in usec since last call
void Reset(); // resets timer
virtual ~Timer();
/* Synchronize the loop within a period
* To use this u will have to initialize the timer
* reset the timer at the beginning of the loop
* and call the Synch function at the end of the loop
*/
void Synch(double period); // period should be in msec
};
#endif /*TIMER_H_*/
--- NEW FILE: nn.h ---
/* Nearest-neighbours algorithm for 2D point-sets.
* Compute the single nearest-neighbour of a point, or the set of k-nearest
neighbours.
*
* This is a very simple algorithm that is reasonably efficient for planar
* points. However much faster algorithms are possible.
*
* Tim Bailey 2004.
*/
#ifndef NN_HEADER
#define NN_HEADER
#include "geometry2D.h"
#include <vector>
namespace Geom2D {
// Simple 2D k-nearest-neighbours search
//
class SweepSearch {
public:
enum { NOT_FOUND = -1 };
SweepSearch(const std::vector<Point> &p, double dmax);
int query(const Point &q) const;
std::vector<double>& query(const Point &q, std::vector<int> &idx);
private:
struct PointIdx {
PointIdx() {}
PointIdx(const Point &p_, const int& i_) : p(p_), i(i_) {}
Point p;
int i;
};
const double limit;
std::vector<PointIdx> dataset;
std::vector<double> nndists;
bool is_nearer(double &d2min, int &idxmin, const Point &q, const
PointIdx &pi) const;
bool insert_neighbour(const Point &q, const PointIdx &pi,
std::vector<double> &nndists, std::vector<int> &idx);
static bool yorder (const PointIdx& p, const PointIdx& q) {
return p.p.y < q.p.y;
}
};
}
#endif
--- NEW FILE: geometry2D.h ---
/* Simple 2D geometric operations with points, poses, and lines.
*
* These algorithms were worked out by me from first principles several years
* ago. They work ok, but are not particularly good implementations. I recommend
* the geometric source-code and resources by David Eberly on the "Magic
Software"
* website: www.magic-software.com.
*
* Tim Bailey 2004.
*/
#ifndef GEOMETRY_2D_H_
#define GEOMETRY_2D_H_
#include <vector>
namespace Geom2D {
//
// Basic Structures
//
struct Point
{
double x;
double y;
short int laser_index;
};
struct Pose
{
Point p;
double phi;
};
struct Line {
Point first;
Point second;
};
//
// Utility functions
//
const double PI = 3.14159265358979;
inline
double sqr(double x) { return x*x; }
inline
double abs(double x) { return (x<0.) ? -x : x; }
inline
double round(double x) {
return (x<0.) ? -static_cast<int>(0.5-x) : static_cast<int>(0.5+x);
}
/*
template<class T>
inline
void swap(T& a, T& b)
{
T tmp(a);
a = b;
b = tmp;
}
*/
inline
double pi_to_pi(double angle) { // normalise an angle to within +/- PI
while (angle < -PI)
angle += 2.*PI;
while (angle > PI)
angle -= 2.*PI;
return angle;
}
//
// Point and Pose algorithms
//
inline
double dist_sqr(const Point& p, const Point& q) { // squared distance between
two Points
return (sqr(p.x-q.x) + sqr(p.y-q.y));
}
double dist(const Point& p, const Point& q);
Pose compute_relative_pose(const std::vector<Point>& a, const
std::vector<Point>& b);
//
// Line algorithms
//
bool intersection_line_line (Point& p, const Line& l, const Line& m);
double distance_line_point (const Line& lne, const Point& p);
void intersection_line_point(Point& p, const Line& l, const Point& q);
//
// Basic transformations on 2-D Points (x,y) and Poses (x,y,phi).
//
class Transform2D {
public:
Transform2D(const Pose& ref);
void transform_to_relative(Point &p);
void transform_to_relative(Pose &p);
void transform_to_global(Point &p);
void transform_to_global(Pose &p);
private:
const Pose base;
double c;
double s;
};
inline
void Transform2D::transform_to_relative(Point &p) {
p.x -= base.p.x;
p.y -= base.p.y;
double t(p.x);
p.x = p.x*c + p.y*s;
p.y = p.y*c - t*s;
}
inline
void Transform2D::transform_to_global(Point &p) {
double t(p.x);
p.x = base.p.x + c*p.x - s*p.y;
p.y = base.p.y + s*t + c*p.y;
}
inline
void Transform2D::transform_to_relative(Pose &p) {
transform_to_relative(p.p);
p.phi= pi_to_pi(p.phi-base.phi);
}
inline
void Transform2D::transform_to_global(Pose &p) {
transform_to_global(p.p);
p.phi= pi_to_pi(p.phi+base.phi);
}
} // namespace Geom2D
#endif
--- NEW FILE: lasermodel.h ---
#ifndef LASERMODEL_H_
#define LASERMODEL_H_
// Info for a single range measurement
typedef struct
{
double range, bearing;
} laser_range_t;
class LaserModel
{
private:
mapgrid * * map; // Pointer to the OG map
double range_cov; // Covariance in the range reading
double range_bad; // Probability of spurious range
readings
// Pre-computed laser sensor model
int lut_size;
double lut_res;
double *lut_probs;
int range_count;
laser_range_t *ranges;
public :
void ClearRanges();
void AddRange(double,double);
void PreCompute();
double RangeProb(double,double);
double PoseProb();
LaserModel();
~LaserModel();
LaserModel(mapgrid * * );
};
#endif /*LASERMODEL_H_*/
--- NEW FILE: map.h ---
/***************************************************************************
* 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. *
***************************************************************************/
#ifndef map_HEADER
#define map_HEADER
#include <gdk-pixbuf/gdk-pixbuf.h>
#include <gtk/gtk.h>
#include <glib/gprintf.h>
#include <sys/stat.h>
class MapInfo
{
public :
int height;
int width;
double pixel_size;
};
typedef struct mapgrid
{
double prob_occ,prob_free;
bool added;
} mapgrid_t;
class MAP
{
private:
double pixel_size;
int negate;
GdkPixbuf * pixbuf;
public:
int size_x, size_y;
mapgrid * * occ_grid;
char * Mapname;
MapInfo GetMapInfo();
void SavePixelBufferToFile();
void DrawPixel(int,int,int,int,int);
void ClearData();
void ResetProb();
int SavePgm();
GdkPixbuf * CreateMap();
~MAP();
MAP(char*, double,double);
};
#endif
--- NEW FILE: icp.h ---
/* Iterated closest point (ICP) algorithm.
*
* This is a very simple implementation of ICP, with one simple extension where
the
* association may be chosen as an interpolation between two nearest neighbours
rather
* than just point-to-point with the nearest neighbour.
*
* A lot of extensions to this basic algorithm are possible. For example,
* 1. Record at each iteration the set of NN associations for each
observation.
* If these do not change, then ICP has converged completely.
*
* 2. Various speed ups given in the following papers:
*
* (a) P.J. Besl and N.D. McKay. A method for registration of 3-D shapes.
IEEE
* Transactions on Pattern Analysis and Machine Intelligence,
14(2):239256, 1992.
*
* (b) F. Lu and E. Milios. Robot pose estimation in unknown environments
by matching
* 2D range scans. Journal of Intelligent and Robotic Systems, 18:249275,
1997.
*
* (c) S. Rusinkiewicz and M. Levoy. Efficient variants of the ICP
algorithm. In Third
* International Conference on 3D Digital Imaging and Modeling, pages
145152, 2001.
*
* 3. Methods for estimating the error introduced by point-wise
correspondence in
* the paper (b) above and also:
*
* S.T. P?ster, K.L. Kriechbaum, S.I. Roumeliotis, and J.W. Burdick.
Weighted range
* sensor matching algorithms for mobile robot displacement estimation. In
IEEE
* International Conference on Robotics and Automation, 2002.
*
* Tim Bailey 2004.
*/
#include <vector>
#include "geometry2D.h"
#include "nn.h"
#include <memory>
using namespace std;
namespace Geom2D {
class ICP {
public:
ICP();
~ICP();
Pose align(std::vector<Point> , std::vector<Point>,Pose , double , int
, bool );
const std::vector<Point> get_ref_points() { return b; }
const std::vector<Point> get_obs_points() { return a; }
private:
vector<Point> ref;
SweepSearch * nn;
vector<Point> a;
vector<Point> b;
vector<int> index;
};
} // namespace Geom2D
-------------------------------------------------------------------------
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
