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

minor edits

Browse Source
This commit is contained in:
Raphael Maenle 2019-07-11 16:16:25 +02:00
parent 4a604e6dca
commit 55669ea2c9
2 changed files with 11 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
launch/msckf_vio_tinytum.launch
View File

@ -25,7 +25,7 @@
<param name="PrintImages" value="false"/> <param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/> <param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="5"/> <param name="patch_size_n" value="3"/>
<!-- Calibration parameters --> <!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/> <rosparam command="load" file="$(arg calibration_file)"/>
@ -33,7 +33,7 @@
<param name="frame_rate" value="20"/> <param name="frame_rate" value="20"/>
<param name="fixed_frame_id" value="$(arg fixed_frame_id)"/> <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
<param name="child_frame_id" value="odom"/> <param name="child_frame_id" value="odom"/>
<param name="max_cam_state_size" value="20"/> <param name="max_cam_state_size" value="12"/>
<param name="position_std_threshold" value="8.0"/> <param name="position_std_threshold" value="8.0"/>
<param name="rotation_threshold" value="0.2618"/> <param name="rotation_threshold" value="0.2618"/>

26
src/msckf_vio.cpp
View File

@ -449,7 +449,7 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
cout << "2" << endl; // cout << "2" << endl;
// Add new observations for existing features or new // Add new observations for existing features or new
// features in the map server. // features in the map server.
start_time = ros::Time::now(); start_time = ros::Time::now();
@ -458,7 +458,7 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
cout << "3" << endl; // cout << "3" << endl;
// Add new images to moving window // Add new images to moving window
start_time = ros::Time::now(); start_time = ros::Time::now();
manageMovingWindow(cam0_img, cam1_img, feature_msg); manageMovingWindow(cam0_img, cam1_img, feature_msg);
@ -466,14 +466,14 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
cout << "4" << endl; // cout << "4" << endl;
// Perform measurement update if necessary. // Perform measurement update if necessary.
start_time = ros::Time::now(); start_time = ros::Time::now();
removeLostFeatures(); removeLostFeatures();
double remove_lost_features_time = ( double remove_lost_features_time = (
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
cout << "5" << endl; // cout << "5" << endl;
start_time = ros::Time::now(); start_time = ros::Time::now();
pruneLastCamStateBuffer(); pruneLastCamStateBuffer();
double prune_cam_states_time = ( double prune_cam_states_time = (
@ -483,7 +483,7 @@ void MsckfVio::imageCallback(
batchTruthProcessing(feature_msg->header.stamp.toSec()); batchTruthProcessing(feature_msg->header.stamp.toSec());
cout << "6" << endl; // cout << "6" << endl;
// Publish the odometry. // Publish the odometry.
start_time = ros::Time::now(); start_time = ros::Time::now();
publish(feature_msg->header.stamp); publish(feature_msg->header.stamp);
@ -2064,9 +2064,6 @@ bool MsckfVio::PhotometricFeatureJacobian(
cout << "---------- LOGGED -------- " << endl; cout << "---------- LOGGED -------- " << endl;
} }
cout << H_x.rows() << ":" << H_x.cols() << endl;
cout << r.rows() << ":" << r.cols() << endl;
if(valid) if(valid)
return true; return true;
return false; return false;
@ -2662,13 +2659,14 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
} }
*/ */
if(filter == 1)
cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
if (chi_squared_test_table[dof] == 0) if (chi_squared_test_table[dof] == 0)
return false; return false;
if (gamma < chi_squared_test_table[dof]) { if (gamma < chi_squared_test_table[dof]) {
if(filter == 1)
cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
// cout << "passed" << endl; // cout << "passed" << endl;
return true; return true;
} else { } else {
@ -2795,11 +2793,9 @@ void MsckfVio::removeLostFeatures() {
cout << "3: " << endl; cout << "3: " << endl;
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, p2H_xj, p2r_j, 3)) if(PhotometricFeatureJacobian(feature.id, cam_state_ids, p2H_xj, p2r_j, 3))
{ {
if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) { if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
cout << p2H_x.rows() << ":" << p2H_x.cols() << ", " << p2H_xj.rows() << ":" << p2H_xj.cols() << endl; cout << p2H_x.rows() << ":" << p2H_x.cols() << ", " << p2H_xj.rows() << ":" << p2H_xj.cols() << endl;
p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj; p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j; p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
@ -2994,9 +2990,7 @@ void MsckfVio::pruneLastCamStateBuffer()
} }
} }
/*
cout << "3: " << endl; cout << "3: " << endl;
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3)) if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
{ {
if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) { if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
@ -3005,7 +2999,6 @@ void MsckfVio::pruneLastCamStateBuffer()
p2stack_cntr += p2H_xj.rows(); p2stack_cntr += p2H_xj.rows();
} }
} }
*/
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
@ -3196,7 +3189,6 @@ void MsckfVio::pruneCamStateBuffer() {
} }
cout << "3: " << endl; cout << "3: " << endl;
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3)) if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
{ {
if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) { if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {