It can be seen from Figure 4 that the outputs of tightly
coupled
INS
/Stereo
VO
(e.g., position, velocity, and azimuth)
are used for fuzzy logic map matching together with the digi-
tal map information from
OSM
. The fuzzy logic map matching
algorithms illustrated in the previous section are used to find
out the corresponding map matched points on the digital map,
which are further used as feedback to the
IMU
mechanization
in the next epoch, in order to limit the drift of
DR
sensors. It
is of great importance that the map matched points found by
the fuzzy logic map matching algorithms are correct since
it would cause significant error in positioning if incorrect
map matching feedbacks are used for
INS
/Stereo
VO
. To avoid
incorrect map matched points, a feedback filter is designed to
guarantee the correctness of map matched points. The criteria
of the feedback filter are listed as follows. First, the
SMP
-1 has
to show that the vehicle is still on the same road link. Second,
the
HE
needs to be smaller than a threshold. Third, the verti-
cal rotation angle has to be smaller than a threshold, which
means that the vehicle is not turning. Last, the distance to
junction should be greater than a threshold. The map matched
points have to meet all the four requirements to pass the feed-
back filter to be considered reliable and correct.
When map matched points can pass the feedback filter, the
point position and the road azimuth information would be
used for the
IMU
mechanization in next epoch. Specifically,
the integrated output roll, pitch, and the road link azimuth
are used to calculate
R
l
b
(the rotation matrix from the body
frame to the local-level frame), the map matched latitude
and longitude are used to calculate
R
e
l
(the rotation matrix
from the local-level frame to the
ECEF
frame), the position is
updated with the map matched results as well. In this way,
the estimated position of
INS
/Stereo
VO
is forced to the road
links on the digital map, which could largely reduce the
quick drift. The effectiveness of the proposed method will be
verified land vehicle tests shown in next section.
Tests and Results
The
KITTI
(Karlsruhe Institute of Technology and Toyota
Technological Institute) datasets are used to test the
INS
/Ste-
reo
VO
/
MM
algorithm proposed in this paper. The
MEMS
-based
IMU
used by
KITTI
is OXTS RT 3003, and the stereo grayscale
cameras used are Point Grey Flea 2 (FL2-14S3M-C). All
the sensors are mounted on a land vehicle’s roof. The lever
arms between the
IMU
and the stereo cameras are accurately
measured before fields tests. The stereo cameras and the
IMU
are synchronized with 10
Hz
data sampling rate. Besides, the
stereo images are rectified and calibrated. The
RTK
(Real Time
Kinematic)
GPS
(Global Positioning System)/
IMU
solution is
used as the reference to verify the proposed
INS
/Stereo
VO
/
MM
method in different scenarios. For
VO
, a Harris corner detector
is used to find features on images and
KLT
(Kanade-Lucas-
Tomasi) feature tracker is applied to track corresponding fea-
tures between consecutive epochs. In order to avoid too many
mismatched features and optimize the distribution of features
on the image, bucketing is also applied. In bucketing, the
image is divided into nine rectangle parts in which a certain
number of features are randomly selected. It is still inevitable
to encounter mismatched features and moving objects, which
have to be removed in tightly coupled
INS
/Stereo
VO
. In order
to filter out the outliers of visual measurements,
RANSAC
(RANdom SAmple Consensus) method is applied. Specifi-
cally, 200 times iterations are applied in resection to find out
the rotation and translation between two adjacent epochs ob-
tained by a certain number of points randomly selected with
a maximum number of inliers. Only the inliers are applied as
the measurement update in the
EKF
. The premise of validation
of
RANSAC
to remove outliers is that the majority of tracked
features are inliers. For the datasets used in this work, there
are few moving objects, which appear occasionally. Therefore,
RANSAC
is valid to remove the outliers in this work.
The first dataset contains around 5,186 epochs (around
8.6 minutes), which was collected in a residential area with
several maneuvers in the City of Karlsruhe in Germany. There
are seldom pedestrians, bicycles, and vehicles in this dataset.
The average number of valid features used in this dataset is
81. The maximum and minimum number of valid features
used are 137 and 23, respectively. The horizontal distance
traveled by car for the first dataset is approximately 4,130
meters. It has been proved that the tightly coupled integration
of
INS
/Stereo
VO
outperforms the individual sensor (Asadi and
Bottasso, 2014; Liu
et al
., 2015). Thus, only the trajectories of
reference, tightly coupled
INS
/Stereo
VO
, and
INS
/Stereo
VO
/
MM
are plotted in red, green, and blue respectively shown in
Figure 5. As can be seen that it is inevitable that the
INS
/Stereo
VO
suffers drift after a certain time. With the application of
map matching feedback, the solution of
INS
/Stereo
VO
is forced
to the road links on the digital map, which largely reduces
the quick drift. This is the reason that the
INS
/Stereo
VO
/
MM
trajectory is not continuous. In order to avoid incorrect map
matching feedback, it is obvious that there is no map match-
ing point when the land vehicle is turning. The position
RMSE
(root-mean-square error) in each direction is shown in Table 4.
It can be seen that the
RMSE
in the horizontal plane is largely
reduced after the application of map matching algorithms.
Start Point
End Point
Figure 5. Trajectories of the first dataset.
Table 4. Position
RMSE
of different methods for the first
dataset (unit: meter).
East
North Height Horizontal Total
INS
/Stereo
VO
52.720 23.308 9.621 57.642
58.440
INS
/
VO
/
MM
11.588 8.109 12.522 14.144
18.891
In addition to the improvement in position, the azimuth
estimation has also been improved. Since the azimuth of road
link has been applied for the
INS
mechanization, the accuracy
of azimuth depends mainly on the accuracy of the digital
map. The
RMSE
of azimuth error with map matching feed-
back is reduced to 5.01 degrees from 8.03 degrees. With the
improvement for the azimuth estimation, the velocity estima-
tion is slightly better as well. The horizontal velocity
RMSE
is
decreased from 1.49 m/s to 1.05 m/s.
The second dataset was collected in suburban with trees on
both sides. The distance of this dataset is about 4,980 meters.
At the beginning of the dataset, not too many valid features
can be tracked due to similar scenes around. In two epochs,
no valid visual measurements can be used, which means only
mechanization results of
IMU
are provided for map match-
ing. On average, 84 valid features are used in each epoch.
PHOTOGRAMMETRIC ENGINEERING & REMOTE SENSING
January 2018
21
