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

minor calcualtion changes - works at 7-8 fps bagfiles speed

Browse Source
This commit is contained in:
Raphael Maenle 2019-05-28 14:12:22 +02:00
parent cab56d9494
commit 8cfbe06945
2 changed files with 6 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
launch/msckf_vio_tum.launch
View File

@ -18,7 +18,7 @@
output="screen">
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="true"/>
<param name="PHOTOMETRIC" value="false"/>
<!-- Debugging Flaggs -->
<param name="PrintImages" value="false"/>

14
src/msckf_vio.cpp
View File

@ -1321,10 +1321,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
r_i[0] = z[0] - und_p_in_c0.x;
r_i[1] = z[1] - und_p_in_c0.y;
//cout << "comp\n" << p_c0(0)/p_c0(2) << ":" << und_p_in_c0.x << endl;
//cout << p_c0(1)/p_c0(2) << ":" << und_p_in_c0.y << endl;
MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
// set anchor Jakobi
@ -1448,6 +1444,8 @@ void MsckfVio::PhotometricFeatureJacobian(
H_x = A_null_space.transpose() * H_xi;
r = A_null_space.transpose() * r_i;
if(PRINTIMAGES)
{
std::cout << "resume playback" << std::endl;
@ -1581,16 +1579,14 @@ void MsckfVio::featureJacobian(
FullPivLU<MatrixXd> lu(H_fj.transpose());
MatrixXd A = lu.kernel();
H_x = A.transpose() * H_xj;
r = A.transpose() * r_j;
H_x = A.transpose() * H_xj;
r = A.transpose() * r_j;
/*
ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
myfile.close();
cout << "---------- LOGGED -------- " << endl;
*/
nh.setParam("/play_bag", false);
return;