robot_location.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#ifndef ROBOTLOCATION_H_
24#define ROBOTLOCATION_H_
25
26#include "uloc.hh"
27#include "segment_map.hh"
28#include "scan.hh"
29#include "feature.hh"
30
32
33public:
34 RobotLocation(double odom_noise_x,
35 double odom_noise_y,
36 double odom_noise_th);
37
38 // Initializers
39
41 void SetPoses(double ox, double oy, double oth,
42 double gx, double gy, double gth);
45 void SetCurrentError(double ex, double ey, double eth);
46
47 SegmentMap& map(void) { return map_; };
48 const SegmentMap& map(void) const { return map_; };
49
50 // Usage
51
52 bool Locate(const Transf odom, const Scan s); // true if performed
53 void PrintState() const;
54
55 Pose EstimatedPose(void) const;
56 MatrixXd Covariance(void) const;
57
58 // Constants
59 const double odom_noise_x_;
60 const double odom_noise_y_;
61 const double odom_noise_th_;
62private:
63 void Prediction();
64 void Update(ObservedFeatures obs);
65
66 Uloc XwRk_1;
67 Uloc XwRk;
68 Transf odomk_1;
69 Transf odomk;
70 SegmentMap map_;
71
72 bool first_update_;
73};
74
75#endif /* ROBOTLOCATION_H_ */
Definition feature.hh:68
Definition types.hh:43
Definition robot_location.hh:31
void SetPoses(double ox, double oy, double oth, double gx, double gy, double gth)
Inform the current odometric and global (map) pose.
Definition robot_location.cc:40
void SetCurrentError(double ex, double ey, double eth)
Inform the error for the initial pose Robot is expected to be at x +/- (ex/2)
Definition robot_location.cc:51
Definition scan.hh:31
Definition segment_map.hh:30
Definition transf.hh:36
Definition uloc.hh:32