Feature/rolling shutter smart factors

[lucacarlone]

• added jacobians for interpolate (@shteren1)
• added projection factor for rolling shutter camera (@shteren1)
• added tests for projection factor
• added Smart projection factor with rolling shutter
• added tests for Smart projection factor with rolling shutter

[lucacarlone]

@shteren1 : I added the tests and applied minor changes to your implementation (which already looked great and worked out of the box!). The ProjectionFactor part is pretty much ready. Now I will move to developing the smart factor version. Let me know if you have comments.

[shteren1]

Thanks Luca! I know the factor works, I've used it extensively to create stunning vision only rolling shutter SLAM's with loop closures and all.

It did compile on the original version I developed it on - 4.0.3 stable, not sure why it failed to compile on the PR, thanks for taking care of it.

@dellaert @lucacarlone I also have a version of this factor that accepts body-to-sensor pose3 matrix and optimizes it, incase of a multi camera setup with unknown calibration between cameras. I thought it was very nitche'y so I didn't add it, but if you see value it in I can add it also.

[lucacarlone]

Thanks Luca! I know the factor works, I've used it extensively to create stunning vision only rolling shutter SLAM's with loop closures and all.

It did compile on the original version I developed it on - 4.0.3 stable, not sure why it failed to compile on the PR, thanks for taking care of it.

@dellaert @lucacarlone I also have a version of this factor that accepts body-to-sensor pose3 matrix and optimizes it, incase of a multi camera setup with unknown calibration between cameras. I thought it was very nitche'y so I didn't add it, but if you see value it in I can add it also.

cool! out of curiosity: what tool did you use to calibrate the row readout time? also: did you still use a RANSAC-like preprocessing to filter out outliers before the optimization?

I will leave it to @dellaert to decide if the other body-to-sensor factor is of interest. I'll be happy to help with the tests if needed.

[shteren1]

I work with a specific camera for which I know all the intrinsic parameters and read times so I didn't require any calibration for that.

As far as the SLAM goes, I used a simple essential matrix estimation between consecutive frames to initialize the SLAM, the essential matrix part used the RANSAC + essential matrix calculation from opencv. I know its possible to build a similar optimizer with gtsam but opencv's one has a complete simple usage solution that works fast. I also used SIFT extraction and matching from opencv to build the SLAM graph.

[lucacarlone]

I work with a specific camera for which I know all the intrinsic parameters and read times so I didn't require any calibration for that.

As far as the SLAM goes, I used a simple essential matrix estimation between consecutive frames to initialize the SLAM, the essential matrix part used the RANSAC + essential matrix calculation from opencv. I know its possible to build a similar optimizer with gtsam but opencv's one has a complete simple usage solution that works fast. I also used SIFT extraction and matching from opencv to build the SLAM graph.

great - thanks for sharing!

[shteren1]

@lucacarlone it looks good I went over the code, very impressive work with the jacobians there. looking forward to using it once its merged.

[dellaert]

Very cool. Would like to take another look so marking Request Changes, but I think it's close.

[lucacarlone]

@dellaert @shteren1 : thanks for the comments. I'll implement them in the next few days and ping you when ready.

[shteren1]

@dellaert regarding alpha greater than 1, it is required for the edge case of the last pose in the graph.
since you don't have the next pose to interpolate with, you must do a small extrapolation using the previous pose.
I tried using a generic projection factor just for the last pose in the graph and It's position got slightly distorted. I think using a small extrapolation is a better solution.

[lucacarlone]

@dellaert regarding alpha greater than 1, it is required for the edge case of the last pose in the graph.
since you don't have the next pose to interpolate with, you must do a small extrapolation using the previous pose.
I tried using a generic projection factor just for the last pose in the graph and It's position got slightly distorted. I think using a small extrapolation is a better solution.

I agree with @shteren1 that having the option to extrapolate would be useful. I plan to use this in Kimera, and if we can extrapolate the integration in our code will be straightforward.

[dellaert]

Yeah, no worries: just document: "typically \in [0,1] but can also be used to extrapolate before pose A or after pose B." ?

[dellaert]

I fixed the template issue and formatted new files with clang-format, Google style. Will merge after CI is complete.

[dellaert]

@shteren1 could you add unit tests for Lie::interpolate though? It's a fairly complex Jacobian calculation which was added but has no direct unit test. See #864

[lucacarlone]

I fixed the template issue and formatted new files with clang-format, Google style. Will merge after CI is complete.

great - thanks!

[lucacarlone]

@shteren1 could you add unit tests for Lie::interpolate though? It's a fairly complex Jacobian calculation which was added but has no direct unit test. See #864

I think I added unit tests for that in testPose3. should I move to testLie?

[dellaert]

PS @lucacarlone @shteren1 part of the issue was some arguments were not const&, just const. Somehow that prevented template instantiation. But changing to const& also helps with performance, as only a pointer is passed.

[lucacarlone]

PS @lucacarlone @shteren1 part of the issue was some arguments were not const&, just const. Somehow that prevented template instantiation. But changing to const& also helps with performance, as only a pointer is passed.

thanks for figuring that out! (btw: I also figured out the clang formatting issue - thanks for mentioning the style was off)

[dellaert]

Ok, @lucacarlone . Merged, so you can now merge develop into your other PR branches. Hopefully not too many conflicts due to formatting.

[lucacarlone]

the merge was not too bad so far - thanks @dellaert !