feature.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 #ifndef FEATURE_H_
23 #define FEATURE_H_
24 
25 
26 #define MAX_OBS_FEATURES 100
27 
28 #include <vector>
29 #include "scan.hh"
30 #include "types.hh"
31 #include "uloc.hh"
32 
33 class Feature
34 {
35 public:
37  Feature(GeometricEntityKinds entity_kind);
38  virtual ~Feature();
39 
40  double dimension() const;
41  double codimension() const;
42  const Transf& Loc() const;
43 
44  const MatrixXd& Cov() const;
45  double Cov(int i, int j) const;
46  void SetCov(const MatrixXd& c) { uloc_.SetCov(c); };
47 
48  const Uloc& uloc(void) const { return uloc_; };
49  void set_uloc(const Uloc& u) { uloc_ = u; };
50 
51  void ComputeSegmentLength(Uloc p1, Uloc p2);
52 
53  void SetScan(const GuiSplit &split) { split_ = split; };
54  // Keep the original raw data for debug
55  const GuiSplit &GetScan(void) const { return split_; };
56 private:
57  void GeometricRelationsObservationPointToPoint(Uloc Lsp1, Uloc Lsp2);
58 
59  double dimension_;
60  double codimension_;
61  Uloc uloc_;
62 
63  GuiSplit split_;
64 };
65 
66 
68 {
69 public:
71  virtual ~ObservedFeatures();
72 
73  void AddObservedFeature(Feature f);
74  int Count() const;
75  //void SetPaired(int i, bool b);
76  const Feature & features(int f) const;
77 
78  void Clear(void) { features_.clear(); }
79 
80 private:
81  vector<Feature> features_;
82  vector<bool> is_paired_;
83 };
84 
85 void ScanDataSegmentation(const Scan &laser_raw_data, ObservedFeatures *feat_table);
86 
87 #endif /* FEATURE_H_ */
Definition: feature.hh:33
double cov[6]
The covariance matrix pose estimate (lower half, symmetric matrix) (cov(xx) in m$^2$,...
Definition: player_interfaces.h:2342
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
uint32_t pending_count
The number of pending (unprocessed observations)
Definition: player_interfaces.h:2354
player_pose2d_t mean
The mean value of the pose estimate (m, m, rad).
Definition: player_interfaces.h:2338
#define PLAYER_SIMULATION_REQ_GET_POSE2D
Request/reply subtype: set 2D pose.
Definition: player_interfaces.h:2655
Generic message header.
Definition: player.h:161
unsigned int GetDataSize()
Size of message data.
Definition: message.h:190
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:658
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:664
player_pose2d_t mean
The best (?) pose (mm, mm, arc-seconds).
Definition: player_interfaces.h:2395
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:166
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:74
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
double px
X [m].
Definition: player.h:220
Data: hypotheses (PLAYER_LOCALIZE_DATA_HYPOTHS)
Definition: player_interfaces.h:2351
uint32_t hypoths_count
The number of pose hypotheses.
Definition: player_interfaces.h:2358
virtual void Main(void)=0
Main method for driver thread.
void * GetPayload()
Get pointer to payload.
Definition: message.h:188
Request/reply: get/set 2D pose of a named simulation object.
Definition: player_interfaces.h:2710
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
double py
Y [m].
Definition: player.h:222
Hypothesis format.
Definition: player_interfaces.h:2335
player_localize_hypoth_t * hypoths
The array of the hypotheses.
Definition: player_interfaces.h:2360
player_pose2d_t pose
The particle's pose (m,m,rad)
Definition: player_interfaces.h:2382
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
double alpha
The weight coefficient for linear combination (alpha)
Definition: player_interfaces.h:2384
uint32_t particles_count
The number of particles included.
Definition: player_interfaces.h:2399
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: types.hh:73
Definition: scan.hh:30
double alpha
The weight coefficient for linear combination (alpha)
Definition: player_interfaces.h:2344
Definition: uloc.hh:32
Definition: feature.hh:67
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
#define PLAYER_LOCALIZE_DATA_HYPOTHS
Data subtype: pose hypotheses.
Definition: player_interfaces.h:2320
Integer property class.
Definition: property.h:114
#define PLAYER_LOCALIZE_REQ_SET_POSE
Request/reply subtype: set pose hypothesis.
Definition: player_interfaces.h:2323
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
player_pose2d_t pose
the desired pose in (m, m, rad)
Definition: player_interfaces.h:2717
Request/reply: Get particles.
Definition: player_interfaces.h:2392
Class for loading configuration file information.
Definition: configfile.h:196
#define PLAYER_LOCALIZE_REQ_GET_PARTICLES
Request/reply subtype: get particle set.
Definition: player_interfaces.h:2326
#define PLAYER_CAPABILITIES_REQ
Capability request message type.
Definition: player.h:397
A device address.
Definition: player.h:145
An autopointer for the message queue.
Definition: message.h:73
Feature(GeometricEntityKinds entity_kind)
Defaults to EDGE.
Definition: feature.cc:27
position2d data
Definition: player_interfaces.h:606
A particle.
Definition: player_interfaces.h:2379
uint32_t name_count
Length of name.
Definition: player_interfaces.h:2713
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
player_pose2d_t mean
The mean value of the pose estimate (m, m, rad).
Definition: player_interfaces.h:2370
Base class for drivers which oeprate with a thread.
Definition: driver.h:552
double pending_time
The time stamp of the last observation processed.
Definition: player_interfaces.h:2356
player_localize_particle_t * particles
The particles.
Definition: player_interfaces.h:2401
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
Request/reply: Set the robot pose estimate.
Definition: player_interfaces.h:2367
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:170
double variance
The variance of the best (?) pose (mm^2)
Definition: player_interfaces.h:2397
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:609
char * name
the identifier of the object we want to locate
Definition: player_interfaces.h:2715
Reference-counted message objects.
Definition: message.h:132
double pa
yaw [rad]
Definition: player.h:224
Base class for all drivers.
Definition: driver.h:108
player_msghdr_t * GetHeader()
Get pointer to header.
Definition: message.h:186
#define PLAYER_SIMULATION_REQ_SET_POSE2D
Request/reply subtype: get 2D pose.
Definition: player_interfaces.h:2658
Definition: transf.hh:36
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76