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

changed some parameters

Browse Source
This commit is contained in:
Raphael Maenle
2019-07-12 11:37:36 +02:00
parent 3b6d2c918d
commit 5ba25f8f95
4 changed files with 18 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/msckf_vio/feature.hpp
View File

@@ -935,7 +935,7 @@ bool Feature::VisualizePatch(
//cv::imwrite(loc.str(), cam0.featureVisu);
cv::imshow("patch", cam0.featureVisu);
cvWaitKey(1);
cvWaitKey(0);
}
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const

10
launch/image_processor_tinytum.launch
View File

@@ -14,8 +14,8 @@
<rosparam command="load" file="$(arg calibration_file)"/>
<param name="grid_row" value="4"/>
<param name="grid_col" value="4"/>
<param name="grid_min_feature_num" value="5"/>
<param name="grid_max_feature_num" value="10"/>
<param name="grid_min_feature_num" value="3"/>
<param name="grid_max_feature_num" value="4"/>
<param name="pyramid_levels" value="3"/>
<param name="patch_size" value="15"/>
<param name="fast_threshold" value="10"/>
@@ -24,9 +24,9 @@
<param name="ransac_threshold" value="3"/>
<param name="stereo_threshold" value="5"/>
<remap from="~imu" to="/imu0"/>
<remap from="~cam0_image" to="/cam0/image_raw"/>
<remap from="~cam1_image" to="/cam1/image_raw"/>
<remap from="~imu" to="/imu02"/>
<remap from="~cam0_image" to="/cam0/image_raw2"/>
<remap from="~cam1_image" to="/cam1/image_raw2"/>
</node>
</group>

6
launch/msckf_vio_tinytum.launch
View File

@@ -22,10 +22,10 @@
<!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/>
<param name="PrintImages" value="false"/>
<param name="PrintImages" value="true"/>
<param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="3"/>
<param name="patch_size_n" value="5"/>
<!-- 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="12"/>
<param name="max_cam_state_size" value="20"/>
<param name="position_std_threshold" value="8.0"/>
<param name="rotation_threshold" value="0.2618"/>

17
src/msckf_vio.cpp
View File

@@ -50,7 +50,7 @@ Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity();
// Static member variables in Feature class.
FeatureIDType Feature::next_id = 0;
double Feature::observation_noise = 0.05;
double Feature::observation_noise = 0.2;
Feature::OptimizationConfig Feature::optimization_config;
map<int, double> MsckfVio::chi_squared_test_table;
@@ -1623,7 +1623,7 @@ bool MsckfVio::PhotometricPatchPointResidual(
new_anchorPatch_3d = feature.anchorPatch_3d;
}
for(auto point : new_anchorPatch_3d)
for(auto point : feature.anchorPatch_3d)
{
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
@@ -1869,6 +1869,7 @@ bool MsckfVio::PhotometricMeasurementJacobian(
// visualizing functions
feature.MarkerGeneration(marker_pub, state_server.cam_states);
feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
cout << "r\n" << r_photo << endl;
//feature.VisualizeKernel(cam_state, cam_state_id, cam0);
}
@@ -2782,7 +2783,6 @@ void MsckfVio::removeLostFeatures() {
MatrixXd p2H_xj;
VectorXd p2r_j;
cout << "5: " << endl;
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j, N))
{
if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
@@ -2792,7 +2792,7 @@ void MsckfVio::removeLostFeatures() {
}
}
/*
cout << "3: " << endl;
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, p2H_xj, p2r_j, 3))
{
@@ -2803,7 +2803,7 @@ void MsckfVio::removeLostFeatures() {
p2stack_cntr += p2H_xj.rows();
}
}
*/
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
@@ -2981,7 +2981,6 @@ void MsckfVio::pruneLastCamStateBuffer()
for (const auto& cam_state : state_server.cam_states)
involved_cam_state_ids.push_back(cam_state.first);
cout << "5: " << endl;
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j, N))
{
if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
@@ -2991,6 +2990,7 @@ void MsckfVio::pruneLastCamStateBuffer()
}
}
/*
cout << "3: " << endl;
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
{
@@ -3000,6 +3000,7 @@ 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);
@@ -3179,7 +3180,6 @@ void MsckfVio::pruneCamStateBuffer() {
if (involved_cam_state_ids.size() == 0) continue;
cout << "5: " << endl;
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j, N))
{
if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// involved_cam_state_ids.size())) {
@@ -3189,6 +3189,7 @@ void MsckfVio::pruneCamStateBuffer() {
}
}
/*
cout << "3: " << endl;
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
{
@@ -3198,7 +3199,7 @@ void MsckfVio::pruneCamStateBuffer() {
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);