-
Notifications
You must be signed in to change notification settings - Fork 19
/
TODO
122 lines (100 loc) · 10.9 KB
/
TODO
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
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
Bugs
------------------------------------
- in constant velocity, when setting more dynamic, landmarks are converging faster...
- with Ransac when we try an outlier last, as we redo matching, the wrong results are displayed
-
- sticky landmarks !
- trajectories are not repeatable and not always right
- debug inertial... why sometimes uncertainty on quaternion grows significantly after move, and sometimes not (88,89,90) ?
- do simulation with inertial
- gdhe crashes when too many landmarks (probably), check if this is the number of objects or the size of a command or... and it make rtslam crashing with a sigpipe, check if we could avoid that!
- in simulation without noise, with a slow start going forward, most of the time it works perfectly, but depending on the random seed sometimes it goes frankly on the side (left/right and/or top/down, up to 45 deg). It is the same with ransac iterative and with active search, but they can behave differently with the same random seed...
- [ok] backproject-project is not identity !!! -> distortion correction was not precise enough
- [ok] euclidean points are searched at another place with a long and thin ellipse through the whole image
- [ok] do something when no display (blocked on while(!(*world)->display_rendered) (*world)->display_condition.wait(display_lock);)
Features
------------------------------------
- segments
- multimap
- inertial slam <- in progress
- estimation hardware <- IN PROGRESS FOR MTI
- set up inertial simulation
- config files:
* slam-config: description of robots, sensors, hardware, calib, with uncertainties and their simu-factor
* env-config: a simulated environment with description of landmarks, robots and sensors trajectories
* [or] a file defines preset trajectories, and we make reference to them in env-config
* [or] simu-lmk and simu-traj
* display-config: colors...
- reorganize and update the developer doc
- when doing replay, allow to have a sequence without missing images and simulate "real-time" (ignoring image loading of course)
- make maps really abstract, and all rtslam not depending specifically on EKF
- allow to freeze landmarks, in order to use them for localization but stopping to estimate them, and try to densify the map, to have a better 3D mesh and ease passive map matching.
- try to verify if the surface inside 3 points is a plane, and if not add points inside (or segments)
- [ok] make rtslam Loggable with DataLogger
Organization
------------------------------------
- prepare for as-is distribution (doc + doc install + credits + license)
- move QuickHarrisDetector to fdetect: attention to the prototype of detectIn() !!!
- move ExtendedKalmanFilterIndirect to filter
- move Gaussian to jmath
- is it possible to set up subdirectories with jafar/cmake ?
- [ok] move dma branch to master
Performance
------------------------------------
- when the landmark was observed the previous frame, we can use a very fast matcher from t to t+1 to reduce uncertainty (eg SAD), before applying the robust matcher (eg ZNCC) in a 3x3 max window.
- zncc optimizations :
6. start testing with a roi shrinked by 2, then the rest, it should speed up the process because in general the result is close to the center and thanks to 2.
7. to do in slam : if the search area is really to big, reduce n_sigmas to 2 or 1, if it is found it's nice, if it isn't it is not important
9. try to use cpu vectorialization (compute 4 zncc at once)
1. [ok] use integral images in explorer
2. [ok] test when partial correlation has been done if it is still possible to reach the goal score
3. [ok] work first with half resolution images (small investment at first, but 16 times faster then)
4. [ok] use a real ellipse roi (mahalanobis dist)
5. [ok] progressively enable optimizations when search area are larger
8. [ok] try to copy the search area in another image for a better use of cpu cache -> we already do it when doing half res search, that is when the search area is large enough, so it should be ok
- [ok] check algorithm speed <- Good 60Hz. See issues with RT and OS -> reboot and stop the most possible programs and services
- [ok] project only mean for visibility test, and compute jacobians and covariance only for visible obs
- find a more clever way to manipulate the map used space: ia_indirectArray() is possibly an expensive way to go, or maybe use eigen2 instead of ublas...
Precision
------------------------------------
- to extract patch and match, compute an homography with the 4 corners, in order to take distortion a little bit into account
- correlation with interpolation is not the the most precise method, LK tracker should be more precise
- if we are not sure that we are still tracking the same 3D landmark, it is better to create a new landmark, using the previous one to reasonably initialize it
- define and implement measure->P in matcher, with the curvature, or at least delete points with one curvature too low
- extend to n-point ransac and reduce the lowInnov ? when there is a very high dynamic, one point is not enough to get a good estimation and low innovations. We can also set a dynamic lowInnov to increase it when dynamic is too high. If doing n-point ransac, maybe start with 1, and increase to 2 or 3 if the first few one are bad. Also avoid to take simultaneously points too close because it doesn't bring more information, so let's avoid useless computations. Also as buffered updates are faster, if only a few points were updated with the first ransac set, try to find another ransac set next, in order to limit the number of updates made with true active search policy to 3-5.
- if no feature is found, try with larger ellipse, because there is little chance you find them back later (except if this one image is bad), if you never find them back you'll be for sure inconsistent, and if you try later the ellipses will be greater and greater.
- Finish implementing RobotCentric Kalman
- To investigate : predict, compute jacobians, correct mean offline, recompute jacobians, correct online = better consistence but how much more computations ?
- harris: using square mask is suboptimal because it doesn't locate correctly the corner (adds a shift), but can we refine around the max with a gaussian mask in order to improve the precision of the corner ? We can also approximate a gaussian max with a few weighted square masks.
- harris: making an interpolation in detection would also allow a better stability for changes of point of view, as only the real corner is invariant, a shift adds errors
- write different map managers :
- visual odometry (forget about landmarks shortly after they are lost or out of the frame)
- local map (try to keep the right amount of landmarks to allow close looping inside a map)
- reconstruction (like local map but try to have the most possible landmarks by stopping to update some of them, getting some of them out of the map, and trying to add more points for better coverage)
- test predicted patch variance: if too low, discard exploration and declare not matched. Or rather test the variance of where you search, we can add an option minSigma to explorer that don't do correlation if sigma too low.
- when a point is detected, do a matching with itself with interpolation to find subpixel position
- when a point is matched, try to estimate if there is a possible ambiguity with another point in the ellipse
- inertial: ignore image if movement estimated by move() is too small ? More generally and even without inertial, we can avoid updates with innovation too low wrt observation noise. Inconsistency comes from too large update, but also from too many updates even small, so there is a compromise to make.
- inertial: estimate the time offset between the IMU and the camera timestamps, and take it into account to increase the incertainty when the dynamic is large
- [ok] recompute all infoGains for a few first updates in active search
- [ok] Finish implementing 1 point Ransac to avoid outliers
- [ok] limit n_updates of ransac, and ensure that some active search is done after ransac.
- [ok] when all the ellipses are too large (because we lost a lot of frames), before deleting all of them try to match with subregions, and maybe limit ransac number of tries... and don't delete obs once they have been updated! it's too late!
- [ok] there are still some 'display library' objects construction in display objects constructors, which are possibly time consuming, they should be moved in render with '== NULL' test. Maybe this is why quite often there is a gap in the few first frames.
- [ok] port the multiview descriptor from the old slam in order to match landmarks in more different point of views
- [ok] don't remove landmarks when they are not found because the point of view is too different
- [ok] initialize new points if the existing points are not observed, especially if it is because the point of view is too different.
Display
------------------------------------
- [ok but no ground truth] set up display for simulation ! all the landmarks of the environment should be displayed (in a light color), landmarks in the map with a different color, as well as the robot's frame. It should be set up in the more general framework of "ground truth", eg if we get ground truth for the robot with gps, or offline bundle adjustment. Maybe provide an "OracleAbstract" interface, but find a way to properly make the link between slam objects, simu objects and oracle objects, and to also get objects not yet estimated by slam (give names to robots and sensors ? draw a line between the estimation and the truth ?). And how to compare without scale factor ?
- display the robot frame in addition to its 3d model and its uncertainty ellipse
- display max frame rate in addition to average frame rate, as well as raw processing time and filter time
- Always do only 1 sensor update by loop, in order to associate a precise timestamp with time step, and correctly manage in display which elements have been modified in order to only refresh them (maps, landmarks of the map, sensors, observations...)
- Implement double color labels for better visibility on light and dark backgrounds -> need to use Viewer::setStatusMessage, or not, because it won't be in the dumps...
- in "pause" mode display sensors at t-1 and t
- with a key shortcut in qdisplay, switch between manual and automatic robot following views in 3D display. More generally add control for the 3D display, like switching on/off the textures when it will be implemented, etc
- [ok] do not use so much cpu when waiting
- [ok] and transfer the bufferize step from the display thread to the slam thread, to get the higher priority, because it is not sure that the display thread inherit from the slam thread priority when the latter is waiting for the former.
- [ok] Allow exhaustive rendering when offline, for videos
- [ok] Catch keyboard and mouse events in the windows, in order to allow to play/pause/next, and to display infos about an obs and its lmk when it clicked on...
- [ok] display visual information for observations and descriptors when clicking on it