Error occurs when calibrate IMU and camera with recover covariance

[chengfzy]

I see the covariance of designed variable could be calculated in IMU-Camera calibration from the command help. I use the sample datasets with the below command
kalibr_calibrate_imu_camera --bag ./example/dynamic.bag --cam ./example/camchain-.dynamic.yaml --imu ./example/imu_adis16448.yaml --target ./example/april_6x6.yaml --bag-from-to 5 55 --recover-covariance

However, the error occurs, below is output

...
....
....
Optimizing...
Using the block_cholesky linear system solver
Using the levenberg_marquardt trust region policy
Using the block_cholesky linear system solver
Using the levenberg_marquardt trust region policy
Initializing
Optimization problem initialized with 1043 design variables and 15662 error terms
The Jacobian matrix is 33324 x 4677
[0.0]: J: 264190
[1]: J: 880.243, dJ: 263310, deltaX: 0.0566446, LM - lambda:10 mu:2
[2]: J: 716.301, dJ: 163.942, deltaX: 0.0302133, LM - lambda:3.33333 mu:2
[3]: J: 712.107, dJ: 4.19349, deltaX: 0.0298072, LM - lambda:1.11111 mu:2
[4]: J: 712.011, dJ: 0.0959996, deltaX: 0.0747766, LM - lambda:0.37037 mu:2
[5]: J: 712.002, dJ: 0.00902875, deltaX: 0.253795, LM - lambda:0.150616 mu:2
Recovering covariance...
terminate called after throwing an instance of 'aslam::Exception'
what(): [Exception] /home/jeffery/Documents/Code/ROS/kalibr_ws/src/kalibr/aslam_nonparametric_estimation/aslam_splines/include/aslam/backend/implementation/BSplineMotionError.hpp:86: evaluateJacobiansImplementation() This is currently unsupported

In the output, the see the code in BSplineMotionError is not implemented. I saw the code but could not understand the inside theory completely. Could anyone know how to modify it, or any paper/book could be referred?

[jaekelj]

I'm also having the same issue. Is anyone planning on implementing this, or are there any other suggestions for extracting an uncertainty of the IMU-Camera calibration?

[chengfzy]

Here I list my solution to get the covariance of estimated result.

First of all, the kalibr don't calculate the covariance of the IMU-Camera calibration. If we modify the code to calculate the covariance, it seems much effort and cannot make sure the modification is right. Therefore, I bypass the direct calculation method by calculating the inverse the Hessian block.

In the linear system solver, kalibr will build a problem like H * x = b, where the H is the Hessian block, and the last 8 parameters in x are the quaternion (N=4), translation(N=3) and time offset(N=1). We could add log to print the last 8 parameters and the calculated IMU-Camera transformation to check whether they are the same value. I checked they are the same. Then we extract the corresponding Hessian block by the index. Because Hessian is the sparse matrix, the inverse of the block is the covariance what we needed.

Although I obtain the covariance of T_BC(transformation from camera to IMU), but this value is about 1e-12, which don't seem any sense for application.

[jaekelj]

Thanks for the insight! If you can upload the changes to made to extract the relevant block from the Hessian I can look through it and see if I can figure out why the values are so extreme.

[chengfzy]

Extract Hessian in below file,

kalibr/aslam_optimizer/aslam_backend/src/BlockCholeskyLinearSystemSolver.cpp

Lines 74 to 106 in 8fbb27d

	bool BlockCholeskyLinearSystemSolver::solveSystem(Eigen::VectorXd& outDx)
	{
	if (_useDiagonalConditioner) {
	Eigen::VectorXd d = _diagonalConditioner.cwiseProduct(_diagonalConditioner);
	// Augment the diagonal
	int rowBase = 0;
	for (int i = 0; i < _H._M.bRows(); ++i) {
	Eigen::MatrixXd& block = *_H._M.block(i, i, true);
	SM_ASSERT_EQ_DBG(Exception, block.rows(), block.cols(), "Diagonal blocks are square...right?");
	block.diagonal() += d.segment(rowBase, block.rows());
	rowBase += block.rows();
	}
	}
	// Solve the system
	outDx.resize(_H._M.rows());
	bool solutionSuccess = _solver->solve(_H._M, &outDx[0], &_rhs[0]);
	if (_useDiagonalConditioner) {
	// Un-augment the diagonal
	int rowBase = 0;
	for (int i = 0; i < _H._M.bRows(); ++i) {
	Eigen::MatrixXd& block = *_H._M.block(i, i, true);
	block.diagonal() -= _diagonalConditioner.segment(rowBase, block.rows());
	rowBase += block.rows();
	}
	}
	if( ! solutionSuccess ) {
	//std::cout << "Solution failed...creating a new solver\n";
	// This seems to help when the CHOLMOD stuff gets into a bad state
	initSolver();
	}
	return solutionSuccess;
	}

add below code before the final return

if (_H._M.rows() > 100) {
    printInfo("BlockCholeskyLinearSystemSolver::solveSystem()");
    cout << "Hessian size = " << _H._M.rows() << " X " << _H._M.cols() << ", block size = " << _H._M.bRows()
         << " X " << _H._M.bCols();
    if (_H._M.rows() > 3000 || _H._M.cols() > 3000) {
        cout << endl;
    } else {
        cout << "|H| = " << _H._M.toDense().norm() << endl;
    }
    cout << "rhs size = " << _rhs.size() << ", |rhs| = " << _rhs.norm() << endl;
    cout << "dx size = " << outDx.size() << ", |dx| = " << outDx.norm() << endl;
    // last 3-2 block
    int startBlockRow = _H._M.bRows() - 3;
    int startBlockCol = _H._M.bCols() - 3;
    int r0 = _H._M.rowBaseOfBlock(startBlockRow);
    int r1 = _H._M.rowBaseOfBlock(startBlockRow + 2);
    int c0 = _H._M.colBaseOfBlock(startBlockCol);
    int c1 = _H._M.colBaseOfBlock(startBlockCol + 2);
    Eigen::MatrixXd hBlock =
        _H._M.slice(startBlockRow, startBlockRow + 2, startBlockCol, startBlockCol + 2)->toDense();
    cout << "H[" << r0 << ":" << r1 << ", " << c0 << ":" << c1 << "] = " << endl << hBlock << endl;
    cout << "H[" << r0 << ":" << r1 << ", " << c0 << ":" << c1 << "]^-1 = " << endl << hBlock.inverse() << endl;
}

[chengfzy]

Here list my repository I modified for reference.
kalibr-JefferyModification

Please enable some macro definition to show the final result.

[gussitc]

Have you looked into this bug yet? It would be really helpful to be able to extract the variance of the time offset in particular.

[JzHuai0108]

I tried to recover covariances of the estimated parameters here a long while ago. You have get some info therein.