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

activated gating test

Browse Source
This commit is contained in:
Raphael Maenle 2019-05-23 09:55:37 +02:00
parent 2aef2089aa
commit e788854fe8
1 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/msckf_vio.cpp
View File

@ -1493,13 +1493,13 @@ void MsckfVio::PhotometricFeatureJacobian(
*/ */
H_x = A_null_space.transpose() * H_xi; H_x = A_null_space.transpose() * H_xi;
r = A_null_space.transpose() * r_i; r = A_null_space.transpose() * r_i;
/*
ofstream myfile; ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl; myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl;
myfile.close(); myfile.close();
cout << "---------- LOGGED -------- " << endl; cout << "---------- LOGGED -------- " << endl;
*/
if(PRINTIMAGES) if(PRINTIMAGES)
{ {
std::cout << "resume playback" << std::endl; std::cout << "resume playback" << std::endl;
@ -1758,7 +1758,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
} }
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
return true;
MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P1 = H * state_server.state_cov * H.transpose();