Rolling Shutter Camera calibration

[othlu]

A module to fully calibrate the intrinsics, distortion coefficients and line delay of rolling shutter cameras.
Besides the python implementation it includes the following changes and fixes/improvements:
_ disable obsolete CameraGeometryDesignVariableContainer in favor of CameraDesignVariable
_ remove duplicate definitions of ReprojectionError from aslam_cv_backend
_ CameraDesignVariable exposes a ScalarExpression representing the time at which a keypoint was observed
_ exports ScalarExpressionNodeKeypointTime to python
_ restore CovarainceReprojectionError which enables state-depending error term standardization
_ CovarianceReprojectionError::observationTime() returned frame_time + 2* time offset
_ Fix an issue in the DogLeg- and Base-TrustRegionPolicy that prevented the correct calculation of the trust region size and resulted in bad/no convergence

This PR repeats the one onto zurich-eye/Kalibr: zurich-eye#6

[v0n0]

Taking a stab at merging this on my local. It's currently not in sync with master.

+1 for having an admin look at it

[v0n0]

Observation: from what I can see, the Boost changes can be dropped from the PR making it much simpler, because Boost was already fixed in master.

[othlu]

@v0n0 i merged upstream to fix the conflicts

[v0n0]

Thanks @othlu!

[v0n0]

I got a compilation error when trying to compile after merging this PR:

Errors     << aslam_cv_backend_python:make /home/xxx/kalibr_workspace/logs/aslam_cv_backend_python/build.make.008.log                        
In file included from /home/xxx/kalibr_workspace/src/Kalibr/aslam_cv/aslam_cv_error_terms/include/aslam/backend/CovarianceReprojectionError.hpp:74:0,
                 from /home/xxx/kalibr_workspace/src/Kalibr/aslam_cv/aslam_cv_backend_python/include/aslam/ExportReprojectionError.hpp:6,
                 from /home/xxx/kalibr_workspace/src/Kalibr/aslam_cv/aslam_cv_backend_python/src/module.cpp:4:
/home/xxx/kalibr_workspace/src/Kalibr/aslam_cv/aslam_cv_error_terms/include/aslam/backend/implementation/CovarianceReprojectionError.hpp:47:70: error: no ‘Eigen::MatrixXd aslam::backend::CovarianceReprojectionError<FRAME_T>::covarianceMatrix()’ member function declared in class ‘aslam::backend::CovarianceReprojectionError<FRAME_T>’
     Eigen::MatrixXd CovarianceReprojectionError<F>::covarianceMatrix() {
                                                                      ^
In file included from /home/xxx/kalibr_workspace/src/Kalibr/aslam_cv/aslam_cv_backend_python/src/module.cpp:4:0:
/home/xxx/kalibr_workspace/src/Kalibr/aslam_cv/aslam_cv_backend_python/include/aslam/ExportReprojectionError.hpp: In instantiation of ‘void aslam::python::exportCovarianceReprojectionError(const string&) [with CAMERA_GEOMETRY_T = aslam::cameras::CameraGeometry<aslam::cameras::PinholeProjection<aslam::cameras::NoDistortion>, aslam::cameras::RollingShutter, aslam::cameras::NoMask>; std::string = std::basic_string<char>]’:
/home/xxx/kalibr_workspace/src/Kalibr/aslam_cv/aslam_cv_backend_python/src/module.cpp:27:88:   required from here
/home/xxx/kalibr_workspace/src/Kalibr/aslam_cv/aslam_cv_backend_python/include/aslam/ExportReprojectionError.hpp:92:29: error: ‘covarianceMatrix’ is not a member of ‘aslam::backend::CovarianceReprojectionError<aslam::Frame<aslam::cameras::CameraGeometry<aslam::cameras::PinholeProjection<aslam::cameras::NoDistortion>, aslam::cameras::RollingShutter, aslam::cameras::NoMask> > >’
   .def("covarianceMatrix",  &CovarianceReprojectionError<frame_t>::covarianceMatrix)

[v0n0]

That was fixed when I declared:

Eigen::MatrixXd covarianceMatrix();

at line 46 in the file aslam_cv/aslam_cv_error_terms/include/aslam/backend/CovarianceReprojectionError.hpp

Although now I cannot find the new command after sourcing the workspace again.

[othlu]

We already fixed those issues when merging into another repo - they didn't make it into my branch. Sorry about that.
We renamed the covarianceMatrix to covarianceMap, as, strictly speaking, it is not a covariance matrix but a linear map for covariance matrices. The fix you did, isn't correct but should not break anything as the exported function isn't used in the calibration code.

[v0n0]

Got it, recompiled successfully with your last commit (also fixed the ETH build I think).

The command kalibr_calibrate_rs_cameras is not available though, would you mind adding it to the scripts section in setup.py? (not sure that's the only thing you need to do, but seemed to work for me)

Also the example command when using -h uses wrong options: models instead of model, topics instead of topic and doesn't show that also framerate and feature variance are required.

[v0n0]

Another update: I ran the RS calibration code with a dataset of mine and I got:

Initializing a pose spline with 1199 knots (100.000000 knots per second over 179.842239 seconds)
[ERROR] [1471592926.854017]: Exception: list index out of range

Unfortunately the verbose mode doesn't give me anything more specific than this.

[v0n0]

I tried with a smaller dataset and I got a different exception, where the Jacobian is set to NaN:

Initializing a pose spline with 505 knots (100.000000 knots per second over 75.757683 seconds)
run new optimisation with initial values:

LineDelay:
0.000104166666667
Intrinsics:
( 1075.03533525 ,  1075.03533525 ) ( 319.5 ,  239.5 )
Distortion:
( 0.0 ,  0.0 ) ( 0.0 ,  0.0 )
Using the block_cholesky linear system solver
Using the dog_leg trust region policy
Using the block_cholesky linear system solver
Using the dog_leg trust region policy
Initializing
Optimization problem initialized with 511 design variables and 30316 error terms
The Jacobian matrix is 60631 x 3057
[0.0]: J: 2.99137e+08
[1]: J: -nan, dJ: -nan, deltaX: 1091.75, DL - delta:2238.92, DL, beta:0.5
run new optimisation with initial values:

LineDelay:
-1.30617603895e-05
Intrinsics:
( 2166.78206173 ,  1820.14490916 ) ( -28.5076679138 ,  693.127382555 )
Distortion:
( 0.0952322946841 ,  -0.00373471577463 ) ( -1.03498060465 ,  10.3682618394 )
Using the block_cholesky linear system solver
Using the dog_leg trust region policy
Using the block_cholesky linear system solver
Using the dog_leg trust region policy
Initializing
Optimization problem initialized with 770 design variables and 30316 error terms
The Jacobian matrix is 60631 x 4611
[0.0]: J: -nan
Exception in thread block: Time is out of dual 32-bit range
Exception in thread block: Time is out of dual 32-bit range
Exception in thread block: Time is out of dual 32-bit range
Exception in thread block: Time is out of dual 32-bit range
[ERROR] [1471621981.351852]: Exception: std::exception

[v0n0]

Oh OK... I think that might have been a wrong feature variance value passed at the command line. With a smaller feature variance I get:

Optimization problem initialized with 511 design variables and 30316 error terms
The Jacobian matrix is 60631 x 3057
[0.0]: J: 1.49569e+09
Exception in thread block: [aslam::Exception] /home/xxx/kalibr_workspace/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.46993e+09 <= 1.46993e+09 < 1.46993e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!

[rehderj]

And have you tried setting larger margins with the timeoffset-padding option? Often, this failure hints to a larger underlying problem, but sometimes it is just that your margins are too small.

[v0n0]

@rehderj timeoffset-padding is not supported by this command:

[ERROR] [1471623770.637736]: unrecognized arguments: --timeoffset-padding 0.05

[rehderj]

you could change it manually in the code then on any occurrence of transformationAtTime.

[v0n0]

Seems like a quite high time threshold is set: https://github.com/ethz-asl/kalibr/pull/65/files#diff-37939fff95d750be5e8d14441ff77d4eR168

[rehderj]

That is true! I understood that you integrated the rs error terms into the IccSensor.py. Is that the case? How did you transfer the section then?

The issue of runaway time delays is often symptomatic for other underlying problems. So rather than just tuning the padding, it might be worth investigating where things go wrong.

[v0n0]

@rehderj oh not yet, first I was making sure this code can run without problems. If I integrated and the original code doesn't work, I would wind up debugging for an issue I cannot find in my own change.

[rehderj]

I finally got around to test the functionality and it works perfectly fine for me on two different datasets with rather default settings (please note that I picked an arbitrary feature variance):
kalibr_calibrate_rs_cameras --bag ../synchronized-24193.6Hz.bag --bag-from-to 10 20 --topic /camera_node/GUID/image_raw --target ../checkerboard_7x6_70x70cm.yaml --model pinhole-radtan-rs --framerate 20 --feature-variance 1
The datasets exhibit different line delays and while the results on the first one were a little off (by about 10%), the second results were spot on (> 1% error in the estimated line delay). The bags also contain IMU messages and have been verified to work for camera/IMU calibration using a slightly different approach. So in case you are interested in using the data for validating your approach, please reach out to me via email and I will be happy to try to provide them to you. They are about 10 GB in total.

[rehderj]

@v0n0 the time offset padding you will want to change is this one https://github.com/othlu/kalibr/blob/feature/rs-calibration/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py#L44-L48, nit the one that you identified. Sorry for not checking this earlier!

[v0n0]

Ha that's interesting; are you also on Ubuntu 14.04 and ROS Indigo? It's great that it works for you. I contacted you via email for the dataset. I will also try to change the other padding. Thanks!

[rehderj]

lgtm. Please see https://github.com/ethz-asl/kalibr/tree/fix/rs-model-support-and-option-naming for small fixes.
@schneith should we merge this branch?

[schneith]

cool. yes, that looks awesome. lets merge it!