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
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="GroundTruth" value="false"/>
<param name="patch_size_n" value="5"/>
<param name="patch_size_n" value="3"/>
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>
@@ -33,7 +33,7 @@
<param name="frame_rate" value="20"/>
<param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
<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="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();
cout << "2" << endl;
// cout << "2" << endl;
// Add new observations for existing features or new
// features in the map server.
start_time = ros::Time::now();
@@ -458,7 +458,7 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec();
cout << "3" << endl;
// cout << "3" << endl;
// Add new images to moving window
start_time = ros::Time::now();
manageMovingWindow(cam0_img, cam1_img, feature_msg);
@@ -466,14 +466,14 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec();
cout << "4" << endl;
// cout << "4" << endl;
// Perform measurement update if necessary.
start_time = ros::Time::now();
removeLostFeatures();
double remove_lost_features_time = (
ros::Time::now()-start_time).toSec();
cout << "5" << endl;
// cout << "5" << endl;
start_time = ros::Time::now();
pruneLastCamStateBuffer();
double prune_cam_states_time = (
@@ -483,7 +483,7 @@ void MsckfVio::imageCallback(
batchTruthProcessing(feature_msg->header.stamp.toSec());
cout << "6" << endl;
// cout << "6" << endl;
// Publish the odometry.
start_time = ros::Time::now();
publish(feature_msg->header.stamp);
@@ -2064,9 +2064,6 @@ bool MsckfVio::PhotometricFeatureJacobian(
cout << "---------- LOGGED -------- " << endl;
}
cout << H_x.rows() << ":" << H_x.cols() << endl;
cout << r.rows() << ":" << r.cols() << endl;
if(valid)
return true;
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)
return false;
if (gamma < chi_squared_test_table[dof]) {
if(filter == 1)
cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
// cout << "passed" << endl;
return true;
} else {
@@ -2795,11 +2793,9 @@ void MsckfVio::removeLostFeatures() {
cout << "3: " << endl;
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)) {
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;
p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
@@ -2994,9 +2990,7 @@ void MsckfVio::pruneLastCamStateBuffer()
}
}
/*
cout << "3: " << endl;
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)) {
@@ -3005,7 +2999,6 @@ void MsckfVio::pruneLastCamStateBuffer()
p2stack_cntr += p2H_xj.rows();
}
}
*/
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
@@ -3196,7 +3189,6 @@ void MsckfVio::pruneCamStateBuffer() {
}
cout << "3: " << endl;
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)) {