uloc.hh
1/*
2 * Player - One Hell of a Robot Server
3 * Copyright (C) 2010
4 * Mayte Lázaro, Alejandro R. Mosteo
5 *
6 *
7 * This library is free software; you can redistribute it and/or
8 * modify it under the terms of the GNU Lesser General Public
9 * License as published by the Free Software Foundation; either
10 * version 2.1 of the License, or (at your option) any later version.
11 *
12 * This library is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 * Lesser General Public License for more details.
16 *
17 * You should have received a copy of the GNU Lesser General Public
18 * License along with this library; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20 */
21
22
23
24#ifndef ULOC_H_
25#define ULOC_H_
26
27#include "transf.hh"
28#include <ostream>
29
30enum GeometricEntityKinds { POINT, EDGE, ROBOT };
31
32class Uloc {
33
34public:
35 GeometricEntityKinds entity;
36
37 Uloc(GeometricEntityKinds ge);
38 virtual ~Uloc();
39
40 GeometricEntityKinds uGEntity();
41
42 MatrixXd& Loc() { return x_; };
43 MatrixXd& Pert() { return p_; };
44 MatrixXd& Bind() { return b_; };
45 MatrixXd& Cov() { return c_; };
46
47 const Transf& kX() const { return x_; };
48 const MatrixXd& kPert() const { return p_; };
49 const MatrixXd& kBind() const { return b_; };
50 const MatrixXd& kCov() const { return c_; };
51
52 void SetLoc(Transf loc);
53 void SetPert(MatrixXd pert);
54 void SetBind(MatrixXd bind);
55 void SetCov(MatrixXd cov);
56
57 void CenterUloc ();
58 Transf DifferentialLocation ();
59 void ChangeBinding (MatrixXd newb);
60 void FilterFeatureRobotDirect (Uloc Lre, Transf Xmw, MatrixXd &Fk, MatrixXd &Nk);
61 void IntegrateEdge (Uloc Lre, Transf Xma);
62
63private:
64 Transf x_;
65 MatrixXd p_;
66 MatrixXd b_;
67 MatrixXd c_;
68};
69
70inline ostream& operator << (ostream& ostrm, Uloc& u)
71{
72
73 if (u.uGEntity() == POINT) ostrm << "Uloc: Point" << endl;
74 if (u.uGEntity() == EDGE) ostrm << "Uloc: Edge" << endl;
75 if (u.uGEntity() == ROBOT) ostrm << "Uloc: Robot" << endl;
76
77 ostrm << "X:" << endl;
78 ostrm << u.kX();
79 ostrm << "Pert:" << endl;
80 ostrm << u.kPert();
81 ostrm << "Bind:" << endl;
82 ostrm << u.kBind();
83 ostrm << "Cov:" << endl;
84 ostrm << u.kCov();
85
86 return ostrm;
87}
88
89Uloc inverse_uloc (Uloc Lab);
90Uloc compose_uloc_transf (Uloc Lwf, Transf Xfe);
91Uloc compose_uloc (Uloc Lwf, Uloc Lfe);
92Uloc compose_transf_uloc (Transf Xwf, Uloc Lfe);
93Uloc CalculateAnalyticalEdge (Transf xp1, Transf xp2);
94void information_filter (MatrixXd Hk, MatrixXd Gk, MatrixXd hk, MatrixXd Sk, MatrixXd &Fk, MatrixXd &Nk);
95Uloc integrateEndpointsInEdge(Uloc Lsp1, Uloc Lsp2);
96void estimate_relative_location (Uloc Lwe, Uloc Lwm, Transf &Xem, MatrixXd &Cem);
97double mahalanobis_distance (Uloc Lwa, Uloc Lwb, MatrixXd Bab);
98double mahalanobis_distance_edge_point(Uloc Lwe, Uloc Lwp);
99
100#endif /* ULOC_H_ */
Definition transf.hh:36
Definition uloc.hh:32