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

minor print changes

Browse Source
This commit is contained in:
Raphael Maenle 2019-05-16 13:56:37 +02:00
parent 2ee7c248c1
commit 9c7f67d2fd
2 changed files with 23 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
launch/msckf_vio_tum.launch
View File

@ -21,7 +21,7 @@
<param name="PHOTOMETRIC" value="true"/> <param name="PHOTOMETRIC" value="true"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="PrintImages" value="false"/> <param name="PrintImages" value="true"/>
<param name="GroundTruth" value="false"/> <param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="7"/> <param name="patch_size_n" value="7"/>

38
src/msckf_vio.cpp
View File

@ -428,21 +428,18 @@ void MsckfVio::imageCallback(
// save true state in seperate state vector // save true state in seperate state vector
//if(ErrState) batchTruthProcessing(feature_msg->header.stamp.toSec());
//{
batchTruthProcessing(feature_msg->header.stamp.toSec());
if(GROUNDTRUTH) if(GROUNDTRUTH)
{ {
state_server.imu_state.position = true_state_server.imu_state.position; state_server.imu_state.position = true_state_server.imu_state.position;
state_server.imu_state.orientation = true_state_server.imu_state.orientation; state_server.imu_state.orientation = true_state_server.imu_state.orientation;
state_server.imu_state.position_null = true_state_server.imu_state.position_null; state_server.imu_state.position_null = true_state_server.imu_state.position_null;
state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null; state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null;
state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0; state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0;
state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu; state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu;
} }
//}
double imu_processing_time = ( double imu_processing_time = (
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
@ -1484,6 +1481,14 @@ 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;
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.close();
cout << "---------- LOGGED -------- " << endl;
if(PRINTIMAGES) if(PRINTIMAGES)
{ {
std::cout << "resume playback" << std::endl; std::cout << "resume playback" << std::endl;
@ -1537,10 +1542,7 @@ void MsckfVio::measurementJacobian(
Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero(); Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
// original jacobi // original jacobi
//dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
// my version of calculation
dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
//dpc0_dxc.leftCols(3) = - skewSymmetric(R_w_c0.transpose() * (t_c0_w - p_w)) * R_w_c0;
dpc0_dxc.rightCols(3) = -R_w_c0; dpc0_dxc.rightCols(3) = -R_w_c0;
Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero(); Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
@ -1626,13 +1628,13 @@ void MsckfVio::featureJacobian(
H_x = A.transpose() * H_xj; H_x = A.transpose() * H_xj;
r = A.transpose() * r_j; r = A.transpose() * r_j;
/*
ofstream myfile; ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); 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 << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
myfile.close(); myfile.close();
cout << "---------- LOGGED -------- " << endl; cout << "---------- LOGGED -------- " << endl;
*/
nh.setParam("/play_bag", false); nh.setParam("/play_bag", false);
return; return;