Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Error occurs when calibrate IMU and camera with recover covariance #291

Open
chengfzy opened this issue Jul 9, 2019 · 7 comments
Open
Labels
bug Something isn't working

Comments

@chengfzy
Copy link

chengfzy commented Jul 9, 2019

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
Copy link

jaekelj commented Aug 26, 2019

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
Copy link
Author

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
Copy link

jaekelj commented Aug 28, 2019

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
Copy link
Author

chengfzy commented Aug 29, 2019

Extract Hessian in below file,

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
Copy link
Author

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

Please enable some macro definition to show the final result.

@goldbattle goldbattle added the bug Something isn't working label Apr 22, 2022
@goldbattle goldbattle pinned this issue Jun 2, 2022
@gussitc
Copy link

gussitc commented Dec 4, 2022

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
Copy link

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bug Something isn't working
Projects
None yet
Development

No branches or pull requests

5 participants