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

added direct rho estimation

Browse Source
This commit is contained in:
Raphael Maenle 2019-06-14 12:37:06 +02:00
parent b3df525060
commit 49374a4323
1 changed files with 27 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/msckf_vio.cpp
View File

@ -1410,6 +1410,30 @@ void MsckfVio::PhotometricFeatureJacobian(
if(PRINTIMAGES) if(PRINTIMAGES)
{ {
ofstream myfile;
myfile.open("/home/raphael/dev/octave/log2octave");
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_xi.rows() << "\n"
<< "# columns: " << H_xi.cols() << "\n"
<< H_xi << endl;
myfile << "# name: Hy\n"
<< "# type: matrix\n"
<< "# rows: " << H_yi.rows() << "\n"
<< "# columns: " << H_yi.cols() << "\n"
<< H_yi << endl;
myfile << "# name: r\n"
<< "# type: matrix\n"
<< "# rows: " << r_i.rows() << "\n"
<< "# columns: " << 1 << "\n"
<< r_i << endl;
myfile.close();
std::cout << "resume playback" << std::endl; std::cout << "resume playback" << std::endl;
nh.setParam("/play_bag", true); nh.setParam("/play_bag", true);
} }
@ -1548,6 +1572,7 @@ void MsckfVio::featureJacobian(
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.colPivHouseholderQr().solve(r) << endl; myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
myfile.close(); myfile.close();
cout << "---------- LOGGED -------- " << endl; cout << "---------- LOGGED -------- " << endl;
nh.setParam("/play_bag", false); nh.setParam("/play_bag", false);
@ -1711,7 +1736,7 @@ void MsckfVio::removeLostFeatures() {
invalid_feature_ids.push_back(feature.id); invalid_feature_ids.push_back(feature.id);
continue; continue;
} else { } else {
if(!feature.initializePosition(state_server.cam_states)) { if(!feature.initializeRho(state_server.cam_states)) {
invalid_feature_ids.push_back(feature.id); invalid_feature_ids.push_back(feature.id);
continue; continue;
} }
@ -1881,7 +1906,7 @@ void MsckfVio::pruneCamStateBuffer() {
feature.observations.erase(cam_id); feature.observations.erase(cam_id);
continue; continue;
} else { } else {
if(!feature.initializePosition(state_server.cam_states)) { if(!feature.initializeRho(state_server.cam_states)) {
for (const auto& cam_id : involved_cam_state_ids) for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id); feature.observations.erase(cam_id);
continue; continue;