| Age | Commit message (Collapse) | Author |
|---|
|
Add the feature motion model update to the state method. It is
implemented in two ways: one is the for loop that operates on each
feature instance directly, which is how it has been implemented in the
past. The second method is to compose a large L matrix for all features
and compute the per feature dx in one go. It is expected that the second
method is faster, but it is not yet tested for speed.
|
|
Previously State::Pkk1() was only being computed for the body states.
Methods State::F() and State::Q() were written to compose the full
versions of their respective matrices and the results are used to update
Pk|k-1.
|
|
And also fixed F computation for features.
I realized that the features were transformed from image to body frame
prior to being passed to State. This makes sense because the State
really doesn't need to know about Camera objects. Since the camera was
no longer necessary inside of State it made sense to move the depth
computation from Camera::ref2body() into a method in the Feature class
that does not rely on camera parameters.
Additionally, the depth solver was changed from a simple matrix
inversion to a least squares calculation.
Fx in the feature class was fixed to take into account dt.
Updating Pkk1 is in the process of being modified to handle features.
|
|
|
|
This fixes a bug that was present in the pyslam code. The Jacobian of the motion
model needs to be replaced with:
F = I + F*dt
|
|
|
|
Rather than maintain X and P in the main function they are moved into
the body and state classes, respectively. This will become much more
important when features are added and the accounting becomes more
complicated.
|
|
|
|
The State class contains the body and feature classes. It is responsible
for composing matrices, and performing Kalman updates.
|
