Accurate Translational Velocity

Yes, everything you said makes a lot of sense I think. I will try using IMU+Optical Flow, and at a lower frequency.
Funny enough, I was also thinking this would probably be the way to go, so it's great to get a confirmation on this.
Thank you very much, I will try this idea

Hey! Can you please give my code a quick review?
It definitely has something going for it, but I feel like it could probably be better.

jakaskerl Thank you very much! And thanks for reminding me about statistical filtering. I completely forgot to do that. Well, I'll let you get back to answering other people's questions, but just know that you have been extremely helpful here and I really value it.

The speed of the movement is usually dependent on the use case.
Usually things, even people, move at a fairly constant speed given a specific use case.
So as long as I can get the direction of movement,
I can multiply it by the constant speed to get the velocity.
The direction can be determined using OpenCV's SolvePnP function,
And the 3D corners of a camera model
(In my case I'm literally getting this from a game engine by fitting a plane to the camera)
where once the 3D corners are projected into 2D space, they are the final 2D corners of the view.
These 3D corners of the image frame can be considered the world points and
are constant for a given camera model, near the origin.
The function also requires an intrinsic matrix and distortion coefficients.
Ultimately, I only need a single monocular camera and IMU for this so I just
use the left or right one and either take the data from CalibrationReader or
I could easily estimate it since the direction vector doesn't require a perfect calibration.
The intrinsic matrix is:
fx 0 cx
0 fy cy
0 0 1
where fx is the width and fy is the height
and cx is the center x and cy is the center y. (dimensions divided by two)
The distortion coeffecients can probably just be five zeros in one row.
Given a homography, you can warp the 2D image corners mathematically and put that into SolvePnP.
The correct homography can be estimated using the OpenCV image stitcher.
It's the R matrix from the camera parameters in stitcher.cameras().
If you do it that way you will need C++ for that part as OpenCV doesn't expose that functionality
to the bindings unfortunately.
SolvePnP can be used with DLT as the method (Direct Least Squares)
Which will return only a projected translation.
You can then remove a certain amount of translation based on the detected rotation angles
because the rotation affects this translation and it really shouldn't.
That is a function which you actually will need to calibrate and fit.
Then, you can simply normalize the translation vector and multiply by the constant speed.
I know this isn't code, but I have been working for a long time on this
and my code is proprietary.
However, combined with some smooth linear interpolation (known as Slerp),
You can accumulate a very nice trajectory.
Combine it with pose graph optimization, and you have a good pose for the sensor.
That part is explained in a MATLAB video on YouTube and it's really just soft body physics.

Sorry for the complicated explanation.
I promise it's not rocket science once you get into it xD

I do however have a slightly suboptimal public mapping software I wrote last year
which you can totally use for free, but it was highly experimental.
Maybe you can contribute any improvements?
karrson/OdoToy

OK… I made the realization that my previous post was a SERIOUS SIMPLIFICATION and was honestly TERRIBLE…

Here is what I added to my engineering notebook just now to remind me (This is actually correct):

Matrix Blunder

Karrson: I just realized...

Matrices are surprisingly like vectors...

You can't take an NxN matrix, put it in an N+1xN+1 matrix and expect it to magically work...

It will not modify the N+1 dimension if that part is identity...

Data doesn't just magically appear but somehow after learning homogenous coords I was under the impression that it can xD

Emily: kinda the other way around - vectors are just 1xN matrices

Karrson: I know, but they are similar in that if you multiply a square matrix by a square matrix the extra dimension stays the same if it was originally identity

so dimenionality-wise they are similar

like multiplying 3, 5, 0 by 6, 2, 0 for example

the 0 part stays the same, same if its a 1

ofc in vectors its element-wise so that parts different

Karrson: Oh I think I get what you mean - yeah, you don't have to think of vectors differently at all if you know matrices

(other than extra work you might have to do)

Karrson: I guess what I'm saying is that a combination of dimensionality, points, vectors, homogenous coordinates, affine, projective, are all like apples and oranges where a matrix gets you from one frame to another. Apples don't multiply with oranges. They are different units.

and really, while homogenous coordinates may be a hack that allows you to multiply, it's not that hacky

so a 4x4 homogenous matrix really is a 3x3 in disguise, just that it has to be 4x4 to actually work.

So yes, I believe I have proven quite well that if you want a 3D pose that actually makes use of the world’s Z coordinates - which a 3x3 projection matrix or homography does NOT do btw because it’s really just homogenous 2D - then you will need a set of 3D affine points, so you really do NOT want to use projections at all for this part. That was my mistake. Actually, this WILL require a real calibration (intrinsics and extrinsics) rather than just using the image size to estimate that. Ofc, to go from raw stereo projected images to 3D affine pointclouds, you will need to work with projections, but after that the entire SLAM pipeline should be affine. 

To quote stackoverflow (https://stackoverflow.com/a/56228667),

“First of all, 3 points are too little to recover affine transformation -- you need 4 points. For N-dimensional space there is a simple rule: to unambiguously recover affine transformation you should know images of N+1 points that form a simplex --- triangle for 2D, pyramid for 3D, etc. With 3 points you could only retrieve 2D affine transformation. A good explanation of why this is the case you may find in "Beginner's guide to mapping simplexes affinely".

Regarding some retrieval algorithm. I'm afraid, I don't know Matlab to provide you with appropriate code, but I worked with Python a little bit, maybe this code can help (sorry for bad codestyle -- I'm mathematician, not programmer)

import numpy as np

# input data

ins = [[1, 1, 2], [2, 3, 0], [3, 2, -2], [-2, 2, 3]]  # <- points

out = [[0, 2, 1], [1, 2, 2], [-2, -1, 6], [4, 1, -3]] # <- mapped to

# calculations

l = len(ins)

B = np.vstack([np.transpose(ins), np.ones(l)])

D = 1.0 / np.linalg.det(B)

entry = lambda r,d: np.linalg.det(np.delete(np.vstack([r, B]), (d+1), axis=0))

M = [[(-1)**i * D * entry(R, i) for i in range(l)] for R in np.transpose(out)]

A, t = np.hsplit(np.array(M), [l-1])

t = np.transpose(t)[0]

# output

print("Affine transformation matrix:\n", A)

print("Affine transformation translation vector:\n", t)

# unittests

print("TESTING:")

for p, P in zip(np.array(ins), np.array(out)):

  image_p = np.dot(A, p) + t

  result = "[OK]" if np.allclose(image_p, P) else "[ERROR]"

  print(p, " mapped to: ", image_p, " ; expected: ", P, result)

This code demonstrates how to recover affine transformation as matrix and vector and tests that initial points are mapped to where they should. It is based on equation presented in "Beginner's guide to mapping simplexes affinely", matrix recovery is described in section "Recovery of canonical notation". The same authors published "Workbook on mapping simplexes affinely" that contains many practical examples of this kind.“

Once you have a correct and verifiable affine transformation matrix, you should be able to decompose it into the correct rotation and translation of the camera quite easily.