1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
|
#ifndef main_INC
#define main_INC
#include <cmath>
#include <cstdio>
#include <cstring>
#include <Eigen/Dense>
#include <istream>
#include <iostream>
#include <vector>
#include "body.h"
#include "camera.h"
#include "ourerr.hpp"
#include "state.h"
#include "types.h"
#include "vision.h"
//#define ROS_PUBLISH /* Uncomment to publish ROS node */
//#define ROS_SUBSCRIBE /* Uncomment to subscribe to ROS */
#define ANGBIASX -2.795871394666666222e-03 /* */
#define ANGBIASY 6.984255690000021506e-03
#define ANGBIASZ 1.418145565750002614e-03
//#define ANGBIASX 1.019*-2.795871394666666222e-03 /* */
//#define ANGBIASY .980*6.984255690000021506e-03
//#define ANGBIASZ 0.96*1.418145565750002614e-03
//#define ACCBIASX 0.95*-0.03713532
//#define ACCBIASY 0.65*0.01465135
//#define ACCBIASZ -1*-0.00709229
//#define ACCBIASX 0.99*-0.03713532
//#define ACCBIASY 0.2*0.01465135
//#define ACCBIASZ -1*-0.00709229
#define ACCBIASX -0.03713532
#define ACCBIASY 0.01465135
#define ACCBIASZ -1*-0.00709229
#define CANOECENTER 0.88 /* center of gravity of canoe */
#define CANOEHEIGHT -0.40
//#define CANOEHEIGHT -0.75
#define DOWNSAMPLE 1 /* */
#define HEIGHT_FROM_ATTITUDE /* use the attitude to measure the height */
#define MEASURE_HEIGHT
#if ROS_PUBLISH
#include <ros/ros.h>
#include <ros/geometry_msgs.h>
#endif /* ----- ROS_PUBLISH ----- */
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Quaterniond;
using Eigen::Vector3d;
double aboveWater(const Quaterniond &q);
int parseLine(char *line, message *msg);
timestamp update_dt(const timestamp t, timestamp *t_old);
#if STATESIZE==13
void covCallback(const message &msg, State &mu, const attitude_t &att);
void imgCallback(message &msg, State &mu, Camera &cam);
void imuCallback(const message &msg, State &mu, const timestamp dt);
void utmCallback(const message &msg, State &mu);
#else
void covCallback(const message &msg, State &mu, const Quaterniond &q);
void imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q);
void imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt);
void utmCallback(const message &msg, State &mu, const Quaterniond &q);
#endif
void pvaCallback(const message &msg, Matrix<double,STATESIZE,1> &X, Quaterniond &q);
Matrix<double,4,3> attitude_jacobian(double roll, double pitch, double yaw);
#endif /* ----- #ifndef main_INC ----- */
|
