Age | Commit message (Collapse) | Author |
|
Added a method to test if a feature is sane. It looks to see if the
feature has negative depth or if it is too close or too far away.
Added a method to output the feature location as a UTM struct.
|
|
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.
|
|
|
|
Method converts a source reflection pair into a body coordinate with
depth.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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
|
|
We can hardcode block sizes using templates for a possible performance
increase. Body::skewSymmetric() was modified to return matrix instead of
modifying one in place.
|
|
types.h contains all of the type definitions, they were moved out of
main.h because I couldn't successfully compile that way.
|
|
|
|
|
|
|
|
This is a method for computing S for an independent measurement. It can
provide a speed improvement over computing a full, correlated S, but may
reduce quality if correlated measurements are treated as independent.
|
|
The motion model is broken into two parts, the creation of the L matrix
and the state update. This means that ydot can be computed from the
state object for many features at once, saving computation time.
|
|
Used a slightly simplified expression to compute pib0. Replaced the
Householder Transform with quaternion based reflection (untested).
|
|
|
|
|
|
|
|
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.
|
|
The position and velocity covariances are rotated into the correct
frames: NED and FRD.
|
|
The new coordinates frame for the world and the body correspond to
Junho's code and work well in other versions of this code.
|
|
The coordinates are not working correctly. When I run integration on a
known dataset the body seems to turn in the opposite direction of
reality. It's probably best to change it to NED and FRD. Currenty it is
ENU and RFU.
|
|
|
|
Sensor inputs can be read in from stdin if compiled without USE_ROS.
Now "callback" functions can be written.
|