eig3.h
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2003
4  * Andrew Howard
5  * Brian Gerkey
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2 of the License, or
10  * (at your option) any later version.
11  *
12  * This program 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
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; 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 /* Eigen-decomposition for symmetric 3x3 real matrices.
25  Public domain, copied from the public domain Java library JAMA. */
26 
27 #ifndef _eig_h
28 
29 /* Symmetric matrix A => eigenvectors in columns of V, corresponding
30  eigenvalues in d. */
31 void eigen_decomposition(double A[3][3], double V[3][3], double d[3]);
32 
33 #endif
uint32_t maxqual
Maximum value for quality.
Definition: player_interfaces.h:2262
float position
The position to move to.
Definition: player_interfaces.h:3877
#define PLAYER_LIMB_REQ_BRAKES
Request/reply: brakes.
Definition: player_interfaces.h:4059
#define PLAYER_WARN1(msg, a)
Error message macros.
Definition: error.h:89
int32_t altitude
Altitude, in millimeters.
Definition: player_interfaces.h:1857
uint32_t blobs_count
The number of blobs.
Definition: player_interfaces.h:1103
#define PLAYER_ACTARRAY_DATA_STATE
Idle state code.
Definition: player_interfaces.h:3774
uint32_t data_count
Size of data as stored in buffer (bytes)
Definition: player_interfaces.h:3452
Data: detected blobs (PLAYER_BLOBFINDER_DATA_BLOBS)
Definition: player_interfaces.h:1096
uint8_t motor_state
power state
Definition: player_interfaces.h:3817
uint32_t color
A descriptive color for the blob (useful for gui's).
Definition: player_interfaces.h:1074
double proll
roll [rad]
Definition: player.h:236
double err_horz
Horizonal error [m].
Definition: player_interfaces.h:1876
uint32_t maxlevel
Maximum value for level.
Definition: player_interfaces.h:2264
float accel_z
The node's acceleration on Z-axis from an acceleration sensor.
Definition: player_interfaces.h:4397
#define PLAYER_GPS_DATA_STATE
Data: state (PLAYER_GPS_DATA_STATE)
Definition: player_interfaces.h:1834
double py
Y [m].
Definition: player.h:198
#define PLAYER_LASER_DATA_SCAN
Data subtype: scan.
Definition: player_interfaces.h:845
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:158
uint32_t num_sats
Number of satellites in view.
Definition: player_interfaces.h:1870
float pan
Pan [rad].
Definition: player_interfaces.h:1231
float temperature
The node's templerature measurement from a temperature sensor.
Definition: player_interfaces.h:4405
#define PLAYER_LIMB_REQ_SPEED
Request/reply: speed.
Definition: player_interfaces.h:4065
Data: state (PLAYER_LIMB_DATA_STATE)
Definition: player_interfaces.h:4072
#define PLAYER_POINTCLOUD3D_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:4854
uint32_t right
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1084
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
#define PLAYER_IMU_DATA_STATE
Data subtype: IMU position/orientation data.
Definition: player_interfaces.h:4684
player_fiducial_item_t * fiducials
List of detected fiducials.
Definition: player_interfaces.h:1705
#define PLAYER_PTZ_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:1208
#define PLAYER_WIFI_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:2188
float speed
Speed setting in m/s or rad/s.
Definition: player_interfaces.h:3978
int AddInterface(player_devaddr_t addr)
Add an interface.
double px
X [m].
Definition: player.h:230
uint32_t valid
Status bits.
Definition: player_interfaces.h:296
uint32_t qual_type
Indicates type of link quality info we have.
Definition: player_interfaces.h:2260
float tilt
Tilt [rad].
Definition: player_interfaces.h:1233
uint32_t height
The image dimensions.
Definition: player_interfaces.h:1101
Data: calibrated IMU data (PLAYER_IMU_DATA_STATE)
Definition: player_interfaces.h:4714
uint32_t left
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1082
uint32_t links_count
length of said list
Definition: player_interfaces.h:2250
Generic message header.
Definition: player.h:160
double pz
Z [m].
Definition: player.h:234
float max_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:888
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:657
uint32_t compression
Image compression; PLAYER_CAMERA_COMPRESS_RAW indicates no compression.
Definition: player_interfaces.h:2966
uint8_t state
The current state of the actuator.
Definition: player_interfaces.h:3804
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:663
uint32_t width
Image dimensions [pixels].
Definition: player_interfaces.h:2953
uint32_t quality
Quality of fix 0 = invalid, 1 = GPS fix, 2 = DGPS fix.
Definition: player_interfaces.h:1868
Data: state (PLAYER_PTZ_DATA_STATE)
Definition: player_interfaces.h:1228
Data: state (PLAYER_POSITION3D_DATA_STATE)
Definition: player_interfaces.h:2476
#define PLAYER_CAMERA_FORMAT_RGB888
Image format : 24-bit color (8 bits R, 8 bits G, 8 bits B).
Definition: player_interfaces.h:2942
player_pose3d_t pos
(x, y, z, roll, pitch, yaw) position [m, m, m, rad, rad, rad]
Definition: player_interfaces.h:2479
float * voltages
the samples [V]
Definition: player_interfaces.h:2058
player_pose3d_t vel
(x, y, z, roll, pitch, yaw) velocity [m, m, m, rad, rad, rad]
Definition: player_interfaces.h:2481
int32_t joint
The joint/actuator to command.
Definition: player_interfaces.h:3875
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:165
#define PLAYER_WARN3(msg, a, b, c)
Error message macros.
Definition: error.h:91
uint32_t bottom
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1088
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:73
double px
X [m].
Definition: player.h:219
#define PLAYER_POWER_DATA_STATE
Data subtype: voltage.
Definition: player_interfaces.h:274
#define PLAYER_POSITION3D_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:2432
Data: Get cloud (PLAYER_POINTCLOUD3D_DATA_STATE) The basic 3dcloudpoint data packet.
Definition: player_interfaces.h:4879
#define PLAYER_BLOBFINDER_DATA_BLOBS
Structure describing a single blob.
Definition: player_interfaces.h:1055
float zoom
Field of view [rad].
Definition: player_interfaces.h:1235
#define PLAYER_ACTARRAY_ACTSTATE_IDLE
Idle state code.
Definition: player_interfaces.h:3778
double pyaw
yaw [rad]
Definition: player.h:240
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:167
Command: Set end effector pose (PLAYER_LIMB_CMD_SETPOSE)
Definition: player_interfaces.h:4091
double ReadTupleFloat(int section, const char *name, int index, double value)
Read a float (double) from a tuple field.
#define PLAYER_LIMB_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:4034
virtual void Main(void)=0
Main method for driver thread.
float speed
The speed of the actuator in m/s or rad/s depending on the type.
Definition: player_interfaces.h:3798
#define PLAYER_RANGER_DATA_RANGE
Data subtype: range scan.
Definition: player_interfaces.h:4949
uint8_t blue
Blue color channel.
Definition: player.h:328
int32_t joint
Joint to set speed for.
Definition: player_interfaces.h:3976
uint32_t maxnoise
Maximum value for noise.
Definition: player_interfaces.h:2266
Command: Joint position control (PLAYER_ACTARRAY_CMD_POS)
Definition: player_interfaces.h:3872
#define PLAYER_LIMB_CMD_SETPOSITION
Command subtype: set position.
Definition: player_interfaces.h:4050
uint32_t image_count
Size of image data as stored in image buffer (bytes)
Definition: player_interfaces.h:2968
Structure containing a single actuator's information.
Definition: player_interfaces.h:3793
int ReadInt(int section, const char *name, int value)
Read an integer value.
double speed
Speed over ground, in meters/second .
Definition: player_interfaces.h:1859
uint32_t y
The blob centroid [pixels].
Definition: player_interfaces.h:1080
#define PLAYER_OPAQUE_DATA_STATE
Data subtype: generic state.
Definition: player_interfaces.h:3434
player_pose3d_t upose
Uncertainty in the measured pose .
Definition: player_interfaces.h:1693
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:94
uint8_t red
Red color channel.
Definition: player.h:324
double py
Y [m].
Definition: player.h:221
Request/reply: Speed.
Definition: player_interfaces.h:4157
float range
Range to the blob center [meters].
Definition: player_interfaces.h:1090
int32_t pos[8]
Current joystick position X, Y and Yaw (unscaled)
Definition: player_interfaces.h:3349
#define PLAYER_ACTARRAY_REQ_SPEED
Idle state code.
Definition: player_interfaces.h:3747
uint32_t area
The blob area [pixels].
Definition: player_interfaces.h:1076
float battery
The node's remaining battery voltage.
Definition: player_interfaces.h:4407
#define PLAYER_JOYSTICK_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:3337
player_wsn_node_data_t data_packet
The WSN node's data packet.
Definition: player_interfaces.h:4422
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:111
double px
X [m].
Definition: player.h:196
uint32_t ranges_count
Number of range readings.
Definition: player_interfaces.h:5027
#define PLAYER_ACTARRAY_CMD_HOME
Idle state code.
Definition: player_interfaces.h:3765
#define PLAYER_CAMERA_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:2922
float acceleration
The acceleration of the actuator in m/s^2 or rad/s^2 depending on the type.
Definition: player_interfaces.h:3800
uint8_t * image
Compressed image data (byte-aligned, row major order).
Definition: player_interfaces.h:2972
float current
The current of the actuator in A.
Definition: player_interfaces.h:3802
uint32_t id
A unique, increasing, ID for the scan.
Definition: player_interfaces.h:902
uint32_t id
Blob id.
Definition: player_interfaces.h:1071
float * ranges
Range readings [m].
Definition: player_interfaces.h:896
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
uint32_t mode
operating mode of device
Definition: player_interfaces.h:2258
int32_t longitude
Longitude in degrees / 1e7 (units are scaled such that the effective resolution is roughly 1cm).
Definition: player_interfaces.h:1854
float speed
Speed setting in m/s.
Definition: player_interfaces.h:4160
float magn_x
The node's magnetic measurement on X-axis from a magnetometer.
Definition: player_interfaces.h:4399
double ppitch
pitch [rad]
Definition: player.h:238
uint32_t vdop
Vertical dilution of position (VDOP), times 10.
Definition: player_interfaces.h:1874
float watts
estimated current energy consumption (negative values) or aquisition (positive values) [W].
Definition: player_interfaces.h:306
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:105
float tiltspeed
Current tilt velocity [rad/s].
Definition: player_interfaces.h:1239
float light
The node's light measurement from a light sensor.
Definition: player_interfaces.h:4389
char ap[32]
MAC address of current access point/cell.
Definition: player_interfaces.h:2268
uint32_t bitrate
current bitrate of device
Definition: player_interfaces.h:2256
#define PLAYER_MSGTYPE_RESP_NACK
A negative response message.
Definition: player.h:124
double course
Course made good (heading if the robot moves along its longitudinal axis), in radians.
Definition: player_interfaces.h:1862
#define PLAYER_LIMB_CMD_HOME
Command: home (PLAYER_LIMB_CMD_HOME)
Definition: player_interfaces.h:4039
int32_t joint
The joint/actuator to command - set to -1 to command all.
Definition: player_interfaces.h:3919
Data: voltage (PLAYER_POWER_DATA_STATE)
Definition: player_interfaces.h:291
Info on a single detected fiducial.
Definition: player_interfaces.h:1685
#define PLAYER_LIMB_CMD_SETPOSE
Command subtype: set pose.
Definition: player_interfaces.h:4047
uint32_t node_type
The type of WSN node.
Definition: player_interfaces.h:4416
Data: range scan (PLAYER_RANGER_DATA_RANGE)
Definition: player_interfaces.h:5024
float mic
The node's accoustic measurement from a microphone.
Definition: player_interfaces.h:4391
float panspeed
Current pan velocity [rad/s].
Definition: player_interfaces.h:1237
double err_vert
Vertical error [m].
Definition: player_interfaces.h:1878
uint32_t buttons
Button states (bitmask)
Definition: player_interfaces.h:3353
#define PLAYER_ACTARRAY_ACTSTATE_MOVING
Moving state code.
Definition: player_interfaces.h:3780
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
float accel_x
The node's acceleration on X-axis from an acceleration sensor.
Definition: player_interfaces.h:4393
Data: state (PLAYER_CAMERA_DATA_STATE)
Definition: player_interfaces.h:2950
uint32_t top
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1086
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1994
player_actarray_actuator_t * actuators
The actuator data.
Definition: player_interfaces.h:3815
uint32_t voltages_count
number of valid samples
Definition: player_interfaces.h:2056
Class for loading configuration file information.
Definition: configfile.h:195
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2053
int32_t charging
charge exchange status: if 1, the device is currently receiving charge from another energy device.
Definition: player_interfaces.h:311
double pz
Z [m].
Definition: player.h:200
#define PLAYER_CAPABILITIES_REQ
Capability request message type.
Definition: player.h:396
A device address.
Definition: player.h:144
An autopointer for the message queue.
Definition: message.h:72
double py
Y [m].
Definition: player.h:232
uint32_t fiducials_count
The number of detected fiducials.
Definition: player_interfaces.h:1703
uint32_t format
Image format (must be compatible with depth).
Definition: player_interfaces.h:2959
Data: state (PLAYER_GPS_DATA_STATE)
Definition: player_interfaces.h:1841
int32_t scale[8]
Scaling factors for X, Y and Yaw — not using these yet.
Definition: player_interfaces.h:3351
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:611
uint32_t node_parent_id
The ID of the WSN node's parent (if existing).
Definition: player_interfaces.h:4420
Data: detected fiducials (PLAYER_FIDUCIAL_DATA_SCAN)
Definition: player_interfaces.h:1700
#define PLAYER_ACTARRAY_CMD_POS
Idle state code.
Definition: player_interfaces.h:3753
uint32_t bits
bitfield of samples
Definition: player_interfaces.h:1999
#define PLAYER_LIMB_STATE_BRAKED
Brakes are on.
Definition: player_interfaces.h:4025
void SetError(int code)
Set/reset error code.
Definition: driver.h:144
float magn_z
The node's magnetic measurement on Z-axis from a magnetometer.
Definition: player_interfaces.h:4403
position2d data
Definition: player_interfaces.h:606
#define PLAYER_WSN_DATA_STATE
Data subtypes
Definition: player_interfaces.h:4369
player_wifi_link_t * links
A list of links.
Definition: player_interfaces.h:2252
player_pose3d_t pose
The complete pose of the IMU in 3D coordinates + angles.
Definition: player_interfaces.h:4717
uint32_t node_id
The ID of the WSN node.
Definition: player_interfaces.h:4418
uint32_t width
The image dimensions.
Definition: player_interfaces.h:1099
Command: Set end effector position (PLAYER_LIMB_CMD_SETPOSITION)
Definition: player_interfaces.h:4105
#define PLAYER_CAMERA_COMPRESS_RAW
Compression method: raw.
Definition: player_interfaces.h:2945
Data: state (PLAYER_JOYSTICK_DATA_STATE)
Definition: player_interfaces.h:3346
#define PLAYER_WARN6(msg, a, b, c, d, e, f)
Error message macros.
Definition: error.h:94
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:80
float min_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:886
#define PLAYER_LIMB_STATE_MOVING
Moving to target.
Definition: player_interfaces.h:4027
#define PLAYER_FIDUCIAL_DATA_SCAN
Info on a single detected fiducial.
Definition: player_interfaces.h:1663
player_devaddr_t device_addr
Default device address (single-interface drivers)
Definition: driver.h:268
Base class for drivers which oeprate with a thread.
Definition: driver.h:551
uint32_t actuators_count
The number of actuators in the array.
Definition: player_interfaces.h:3813
float joules
energy stored [J].
Definition: player_interfaces.h:303
Data: state (PLAYER_WIFI_DATA_STATE)
Definition: player_interfaces.h:2247
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
float max_range
Maximum range [m].
Definition: player_interfaces.h:892
player_blobfinder_blob_t * blobs
The list of blobs.
Definition: player_interfaces.h:1105
float percent
Percent of full charge [%].
Definition: player_interfaces.h:301
Data (PLAYER_WSN_DATA_STATE)
Definition: player_interfaces.h:4413
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:609
#define PLAYER_LIMB_REQ_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:4062
#define PLAYER_AIO_DATA_STATE
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2046
player_pose3d_t pose
Fiducial pose relative to the detector.
Definition: player_interfaces.h:1691
double utm_e
UTM WGS84 coordinates, easting [m].
Definition: player_interfaces.h:1864
Request/reply: Speed.
Definition: player_interfaces.h:3973
float volts
Battery voltage [V].
Definition: player_interfaces.h:299
Reference-counted message objects.
Definition: message.h:131
float magn_y
The node's magnetic measurement on Y-axis from a magnetometer.
Definition: player_interfaces.h:4401
int32_t id
The fiducial id.
Definition: player_interfaces.h:1689
double * ranges
Range readings [m].
Definition: player_interfaces.h:5029
uint32_t hdop
Horizontal dilution of position (HDOP), times 10.
Definition: player_interfaces.h:1872
uint8_t stall
Are the motors stalled?
Definition: player_interfaces.h:2483
3D Pointcloud element structure An element as stored in a 3D pointcloud, containing a 3D position plu...
Definition: player_interfaces.h:4861
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:98
uint32_t count
number of samples
Definition: player_interfaces.h:1997
data
Definition: player_interfaces.h:3449
Command: Joint home (PLAYER_ACTARRAY_CMD_HOME)
Definition: player_interfaces.h:3916
float position
The position of the actuator in m or rad depending on the type.
Definition: player_interfaces.h:3796
uint8_t alpha
Alpha (transparency) channel.
Definition: player.h:322
#define PLAYER_DIO_DATA_VALUES
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1984
double pa
yaw [rad]
Definition: player.h:223
Base class for all drivers.
Definition: driver.h:107
int32_t latitude
Latitude in degrees / 1e7 (units are scaled such that the effective resolution is roughly 1cm).
Definition: player_interfaces.h:1850
uint32_t throughput
mysterious throughput calculated by driver
Definition: player_interfaces.h:2254
Structure describing a single blob.
Definition: player_interfaces.h:1068
uint32_t intensity_count
Number of intensity readings.
Definition: player_interfaces.h:898
uint32_t time_sec
GPS (UTC) time, in seconds and microseconds since the epoch.
Definition: player_interfaces.h:1844
#define PLAYER_MSG0(level, msg)
General messages.
Definition: error.h:104
uint32_t time_usec
GPS (UTC) time, in seconds and microseconds since the epoch.
Definition: player_interfaces.h:1846
#define PLAYER_LIMB_REQ_POWER
Request/reply: power.
Definition: player_interfaces.h:4056
float accel_y
The node's acceleration on Y-axis from an acceleration sensor.
Definition: player_interfaces.h:4395
uint32_t x
The blob centroid [pixels].
Definition: player_interfaces.h:1078
double utm_n
UTM WGS84 coordinates, northing [m].
Definition: player_interfaces.h:1866
uint8_t stall
Are the motors stalled?
Definition: player_interfaces.h:613
float resolution
Angular resolution [rad].
Definition: player_interfaces.h:890
uint32_t height
Image dimensions [pixels].
Definition: player_interfaces.h:2955
uint32_t ranges_count
Number of range readings.
Definition: player_interfaces.h:894
Data: state (PLAYER_ACTARRAY_DATA_STATE)
Definition: player_interfaces.h:3810
Data: scan (PLAYER_LASER_DATA_SCAN)
Definition: player_interfaces.h:883
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:75
uint8_t green
Green color channel.
Definition: player.h:326
uint8_t * data
The data we will be sending.
Definition: player_interfaces.h:3454
uint8_t * intensity
Intensity readings.
Definition: player_interfaces.h:900
uint32_t bpp
Image bits-per-pixel (8, 16, 24, 32).
Definition: player_interfaces.h:2957