blob: 5a7e1e355ed446e0dc2539771eed57d760d3a4e6 (
plain)
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
|
Modify body Jacobian F. Junho has an identity in the db/db term, which is not
consistent with the handling of X and V.
Update covariance based on known estimate (GPS aided)
Dynamically adjusted state size. We can add and delete features as needed.
Junho's code requires a fixed state size.
Treat measurements as independent. Compute S_i individually for each measurement
and compose S as block diagonal for speed improvement.
Use quaternion reflection instead of Householder Transformation for determining
the predicted reflection measurement.
Junho sets all features to a fixed initial depth e.g 10m, 20m. I calculate the
initial depth from the reflection information. Better initialization should give
better results, because we can decrease P0 and rely on vision more. According to
Davison, setting P0 properly is important.
Using the intitial depth estimate we get some simple sanity checks: is the
feature above the water? Is the feature a reasonable distance away (greater than
some minimum distance, closer than some maximum)?
Use least square to determine initial depth.
Set the height measurement using the rotation of the body, the approximate
center of gravity, and the initial height.
Below here is not yet implemented:
---------------------------------------------
Add initial reflection view.
According to Davison, measurements near the center of the image (the principal
point) are more accurate than those near the edges. Thus, we can adjust R
according to the distance.
Junho had the ability to handle both reflection features and just monocular
points. I am adding the ability to promote a monocular point to a reflection
feature. This will be fit into the EKF framework quite nicely by just altering
the measurement model used during the filter update.
reparametrize features using spherical coordinates to avoid rho goes to infinity
problem
1-pt ransac to remove outliers
feature blacklist
filter and downsample sensor readings on the fly
|
