Skip to content

Commit 8f4f362

Browse files
WallongWallong
committed
Version 1.0
1 parent 28b19c4 commit 8f4f362

18 files changed

+1592
-2311
lines changed

‎README.md

+14-3
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,13 @@
11
# VINS-GPS-Wheel
2-
## Visual-Inertial Odometry Tightly Coupled with Wheel Encoder and GPS
2+
## Visual-Inertial Odometry Coupled with Wheel Encoder and GNSS
33
This repo couples wheel encoder data and GPS data on the basis of [VINS_Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono). The project is tested on [KAIST](https://irap.kaist.ac.kr/dataset/) dataset and is suitable for automatic driving scenario.
44

5-
<img src="https://github.com/Wallong/VINS-GPS-Wheel/blob/master/support_files/image/kaist.png" width = 80% height = 80% div align=middle />
5+
<img src="https://github.com/Wallong/VINS-GPS-Wheel/blob/master/support_files/image/kaist.png" width = 80% height = 80% div align=center />
66

77
The wheel encoder data is tightly coupled, referred to the paper[[1]](https://ieeexplore.ieee.org/abstract/document/8967607). GPS fusion adopts loose coupling, and the fusion method is consistent with [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion).
88

99
Detailed derivations can be found in: https://blog.csdn.net/ewtewtewrt/article/details/117249295
10+
The method has tested in [KAIST](https://irap.kaist.ac.kr/dataset/) dataset (urban28-pankyo) [video](https://www.bilibili.com/video/BV1q64y1R7hW/)
1011

1112
# Install
1213
## 1. Prerequisites
@@ -25,7 +26,7 @@ Clone the repository and catkin_make:
2526
```
2627
cd ~/catkin_ws/src
2728
git clone https://github.com/Wallong/VINS-GPS-Wheel.git
28-
cd ../
29+
cd ..
2930
catkin_make
3031
source ~/catkin_ws/devel/setup.bash
3132
```
@@ -41,6 +42,16 @@ Open four terminals, launch the vins_estimator, rviz and pubish the data file re
4142
rosrun vins_estimator kaist_pub YOUR_PATH_TO_DATASET/KAIST/urban28/urban28-pankyo
4243
roslaunch vins_estimator vins_rviz.launch
4344
```
45+
## 5. Plan
46+
| Module | Status |
47+
| ---- | ---- |
48+
| Encoder Pre-integration | Done |
49+
| Initialization with encoder | Done |
50+
| Optimization with encoder | Done |
51+
| Online Extrinsic Calibration about encoder | Doing |
52+
| Loosely coupled with GNSS | Done |
53+
| Initialization with GNSS | Will do |
54+
| Tightly coupled with GNSS | Will do |
4455

4556
## References
4657
* J. Liu, W. Gao and Z. Hu, "Visual-Inertial Odometry Tightly Coupled with Wheel Encoder Adopting Robust Initialization and Online Extrinsic Calibration," 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019, pp. 5391-5397, doi: 10.1109/IROS40897.2019.8967607.

‎multisensor_fusion/include/factor/Factors.1.h

-104
This file was deleted.

‎multisensor_fusion/include/factor/Factors.h

+87-97
Original file line numberDiff line numberDiff line change
@@ -1,114 +1,104 @@
1-
/*******************************************************
2-
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
3-
*
4-
* This file is part of VINS.
5-
*
6-
* Licensed under the GNU General Public License v3.0;
7-
* you may not use this file except in compliance with the License.
8-
*
9-
* Author: Qin Tong (qintonguav@gmail.com)
10-
*******************************************************/
11-
121
#pragma once
2+
133
#include <ceres/ceres.h>
144
#include <ceres/rotation.h>
155

166
template <typename T> inline
177
void QuaternionInverse(const T q[4], T q_inverse[4])
188
{
19-
q_inverse[0] = q[0];
20-
q_inverse[1] = -q[1];
21-
q_inverse[2] = -q[2];
22-
q_inverse[3] = -q[3];
9+
q_inverse[0] = q[0];
10+
q_inverse[1] = -q[1];
11+
q_inverse[2] = -q[2];
12+
q_inverse[3] = -q[3];
2313
};
2414

25-
2615
struct TError
2716
{
28-
TError(double t_x, double t_y, double t_z, double var1, double var2, double var3)
29-
:t_x(t_x), t_y(t_y), t_z(t_z), var1(var1), var2(var2), var3(var3){}
30-
31-
template <typename T>
32-
bool operator()(const T* tj, T* residuals) const
33-
{
34-
residuals[0] = (tj[0] - T(t_x)) / T(var1);
35-
residuals[1] = (tj[1] - T(t_y)) / T(var2);
36-
residuals[2] = (tj[2] - T(t_z)) / T(var3);
37-
38-
return true;
39-
}
40-
41-
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z, const double var1, const double var2, const double var3)
42-
{
43-
return (new ceres::AutoDiffCostFunction<
44-
TError, 3, 3>(
45-
new TError(t_x, t_y, t_z, var1, var2, var3)));
46-
}
47-
48-
double t_x, t_y, t_z, var1, var2, var3;
49-
17+
TError(double t_x, double t_y, double t_z, double latVar, double lonVar, double altVar)
18+
:t_x(t_x), t_y(t_y), t_z(t_z), latVar(latVar), lonVar(lonVar), altVar(altVar){}
19+
20+
template <typename T>
21+
bool operator()(const T* tj, T* residuals) const
22+
{
23+
residuals[0] = (tj[0] - T(t_x)) / T(latVar);
24+
residuals[1] = (tj[1] - T(t_y)) / T(lonVar);
25+
residuals[2] = (tj[2] - T(t_z)) / T(altVar);
26+
27+
return true;
28+
}
29+
30+
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
31+
const double latVar, const double lonVar, const double altVar)
32+
{
33+
return (new ceres::AutoDiffCostFunction<TError, 3, 3>(
34+
new TError(t_x, t_y, t_z, latVar, lonVar, altVar)
35+
));
36+
}
37+
38+
double t_x, t_y, t_z;
39+
double latVar, lonVar, altVar;
5040
};
5141

5242
struct RelativeRTError
5343
{
54-
RelativeRTError(double t_x, double t_y, double t_z,
55-
double q_w, double q_x, double q_y, double q_z,
56-
double t_var, double q_var)
57-
:t_x(t_x), t_y(t_y), t_z(t_z),
58-
q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z),
59-
t_var(t_var), q_var(q_var){}
60-
61-
template <typename T>
62-
bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const
63-
{
64-
T t_w_ij[3];
65-
t_w_ij[0] = tj[0] - ti[0];
66-
t_w_ij[1] = tj[1] - ti[1];
67-
t_w_ij[2] = tj[2] - ti[2];
68-
69-
T i_q_w[4];
70-
QuaternionInverse(w_q_i, i_q_w);
71-
72-
T t_i_ij[3];
73-
ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);
74-
75-
residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var);
76-
residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);
77-
residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);
78-
79-
T relative_q[4];
80-
relative_q[0] = T(q_w);
81-
relative_q[1] = T(q_x);
82-
relative_q[2] = T(q_y);
83-
relative_q[3] = T(q_z);
84-
85-
T q_i_j[4];
86-
ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);
87-
88-
T relative_q_inv[4];
89-
QuaternionInverse(relative_q, relative_q_inv);
90-
91-
T error_q[4];
92-
ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q);
93-
94-
residuals[3] = T(2) * error_q[1] / T(q_var);
95-
residuals[4] = T(2) * error_q[2] / T(q_var);
96-
residuals[5] = T(2) * error_q[3] / T(q_var);
97-
98-
return true;
99-
}
100-
101-
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
102-
const double q_w, const double q_x, const double q_y, const double q_z,
103-
const double t_var, const double q_var)
104-
{
105-
return (new ceres::AutoDiffCostFunction<
106-
RelativeRTError, 6, 4, 3, 4, 3>(
107-
new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var)));
108-
}
109-
110-
double t_x, t_y, t_z, t_norm;
111-
double q_w, q_x, q_y, q_z;
112-
double t_var, q_var;
44+
RelativeRTError(double t_x, double t_y, double t_z,
45+
double q_w, double q_x, double q_y, double q_z,
46+
double t_var, double q_var)
47+
: t_x(t_x), t_y(t_y), t_z(t_z),
48+
q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z),
49+
t_var(t_var), q_var(q_var){}
50+
51+
template <typename T>
52+
bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const
53+
{
54+
T t_w_ij[3];
55+
t_w_ij[0] = tj[0] - ti[0];
56+
t_w_ij[1] = tj[1] - ti[1];
57+
t_w_ij[2] = tj[2] - ti[2];
58+
59+
T i_q_w[4];
60+
QuaternionInverse(w_q_i, i_q_w);
61+
62+
T t_i_ij[3];
63+
ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);
64+
65+
residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var);
66+
residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);
67+
residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);
68+
69+
T relative_q[4];
70+
relative_q[0] = T(q_w);
71+
relative_q[1] = T(q_x);
72+
relative_q[2] = T(q_y);
73+
relative_q[3] = T(q_z);
74+
75+
T q_i_j[4];
76+
ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);
77+
78+
T relative_q_inv[4];
79+
QuaternionInverse(relative_q, relative_q_inv);
80+
81+
T error_q[4];
82+
ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q);
83+
84+
residuals[3] = T(2) * error_q[1] / T(q_var);
85+
residuals[4] = T(2) * error_q[2] / T(q_var);
86+
residuals[5] = T(2) * error_q[3] / T(q_var);
87+
88+
return true;
89+
}
90+
91+
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
92+
const double q_w, const double q_x, const double q_y, const double q_z,
93+
const double t_var, const double q_var)
94+
{
95+
return (new ceres::AutoDiffCostFunction<RelativeRTError, 6, 4, 3, 4, 3>(
96+
new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var)
97+
));
98+
}
99+
100+
double t_x, t_y, t_z;
101+
double q_w, q_x, q_y, q_z;
102+
double t_var, q_var;
113103

114104
};

‎multisensor_fusion/include/optimization/multisensorOpt.h

+3
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ class MultisensorOptimization
2424
void inputOdom(double t, Eigen::Vector3d odomP, Eigen::Quaterniond odomQ);
2525
void getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &odomQ);
2626
nav_msgs::Path globalPath;
27+
nav_msgs::Path gpsPath;
2728

2829
private:
2930
void GPS2XYZ(double latitude, double longtitude, double altitude, double* xyz);
@@ -43,4 +44,6 @@ class MultisensorOptimization
4344
Eigen::Quaterniond lastQ;
4445
std::thread threadOpt;
4546

47+
int windowLength;
48+
Eigen::Vector3d lastLocalP;
4649
};

0 commit comments

Comments
 (0)