raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added tiny tum

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-25 12:03:08 +02:00
parent c565033d7f
commit 6f7f8b7892
4 changed files with 39 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
src/msckf_vio.cpp
View File

@ -1664,10 +1664,29 @@ void MsckfVio::featureJacobian(
{
ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
myfile.open("/home/raphael/dev/octave/org2octave");
myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
<< "# name: Hx\n"
<< "# type: matrix\n"
<< "# rows: " << H_xj.rows() << "\n"
<< "# columns: " << H_xj.cols() << "\n"
<< H_xj << endl;
myfile << "# name: Hy\n"
<< "# type: matrix\n"
<< "# rows: " << H_fj.rows() << "\n"
<< "# columns: " << H_fj.cols() << "\n"
<< H_fj << endl;
myfile << "# name: r\n"
<< "# type: matrix\n"
<< "# rows: " << r_j.rows() << "\n"
<< "# columns: " << 1 << "\n"
<< r_j << endl;
myfile.close();
cout << "---------- LOGGED -------- " << endl;
cout << "---------- LOGGED ORG -------- " << endl;
}
return;
@ -1716,8 +1735,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
MatrixXd K = K_transpose.transpose();
// Compute the error of the state.
VectorXd delta_x = K * r;
cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
// cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// Update the IMU state.
if(PHOTOMETRIC) return;
@ -1821,8 +1840,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
// Compute the error of the state.
VectorXd delta_x = K * r;
cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
// cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// Update the IMU state.
if (not PHOTOMETRIC) return;