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

removed dx filter, corrected jacobi calculation with bigger sobel (and correct division), removed scale for mahalanobis

Browse Source
This commit is contained in:
Raphael Maenle 2019-07-05 13:51:58 +02:00
parent 1a07ba3d3c
commit a7c296ca3d
4 changed files with 48 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/msckf_vio/feature.hpp
View File

@ -736,6 +736,7 @@ bool Feature::VisualizeKernel(
cvWaitKey(0); cvWaitKey(0);
} }
bool Feature::VisualizePatch( bool Feature::VisualizePatch(
const CAMState& cam_state, const CAMState& cam_state,
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
@ -934,7 +935,7 @@ bool Feature::VisualizePatch(
//cv::imwrite(loc.str(), cam0.featureVisu); //cv::imwrite(loc.str(), cam0.featureVisu);
cv::imshow("patch", cam0.featureVisu); cv::imshow("patch", cam0.featureVisu);
cvWaitKey(0); cvWaitKey(1);
} }
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const

2
include/msckf_vio/msckf_vio.h
View File

@ -261,7 +261,7 @@ class MsckfVio {
void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
bool gatingTest(const Eigen::MatrixXd& H, bool gatingTest(const Eigen::MatrixXd& H,
const Eigen::VectorXd&r, const int& dof); const Eigen::VectorXd&r, const int& dof, int filter=0);
void removeLostFeatures(); void removeLostFeatures();
void findRedundantCamStates( void findRedundantCamStates(
std::vector<StateIDType>& rm_cam_state_ids); std::vector<StateIDType>& rm_cam_state_ids);

4
launch/msckf_vio_euroc.launch
View File

@ -18,14 +18,14 @@
output="screen"> output="screen">
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two --> <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
<param name="FILTER" value="2"/> <param name="FILTER" value="1"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/> <param name="StreamPause" value="true"/>
<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)"/>

68
src/msckf_vio.cpp
View File

@ -312,7 +312,7 @@ bool MsckfVio::initialize() {
for (int i = 1; i < 1000; ++i) { for (int i = 1; i < 1000; ++i) {
boost::math::chi_squared chi_squared_dist(i); boost::math::chi_squared chi_squared_dist(i);
chi_squared_test_table[i] = chi_squared_test_table[i] =
boost::math::quantile(chi_squared_dist, 0.1); boost::math::quantile(chi_squared_dist, 0.4);
} }
if (!createRosIO()) return false; if (!createRosIO()) return false;
@ -554,23 +554,34 @@ void MsckfVio::manageMovingWindow(
cv::Mat yder; cv::Mat yder;
cv::Mat deeper_frame; cv::Mat deeper_frame;
// generate derivative matrix for cam0
cam0_img_ptr->image.convertTo(deeper_frame,CV_16S); cam0_img_ptr->image.convertTo(deeper_frame,CV_16S);
cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); cv::Sobel(deeper_frame, xder, -1, 1, 0, 5);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); cv::Sobel(deeper_frame, yder, -1, 0, 1, 5);
xder/=72.; xder/=96.;
yder/=72.; yder/=96.;
// save into moving window
cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam0.moving_window[state_server.imu_state.id].dximage = xder.clone();
cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone();
// generate derivative matrix for cam 1
cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); cam1_img_ptr->image.convertTo(deeper_frame,CV_16S);
cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); cv::Sobel(deeper_frame, xder, -1, 1, 0, 5);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); cv::Sobel(deeper_frame, yder, -1, 0, 1, 5);
xder/=72.; xder/=96.;
yder/=72.; yder/=96.;
/*
cv::Mat norm_abs_xderImage;
cv::convertScaleAbs(xder, norm_abs_xderImage);
cv::normalize(norm_abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
cv::imshow("xder", norm_abs_xderImage);
cvWaitKey(0); cvWaitKey(0);
*/
// save into moving window
cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam1.moving_window[state_server.imu_state.id].dximage = xder.clone();
cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone();
@ -1721,9 +1732,9 @@ bool MsckfVio::PhotometricPatchPointJacobian(
H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6
// check if any direction not large enough for eval // check if any direction not large enough for eval
if(dI_dhj(0, 0) < 0.01 or dI_dhj(0, 1) < 0.01 or dI_dhj(1, 2) < 0.01 or dI_dhj(1, 3) < 0.01) /*if((dI_dhj(0, 0) < 0.1 or dI_dhj(0, 1) < 0.1) and (dI_dhj(1, 2) < 0.1 or dI_dhj(1, 3) < 0.1))
return false; return false;
*/
// check if point nullspaceable // check if point nullspaceable
if (H_rhoj(0, 0) != 0) if (H_rhoj(0, 0) != 0)
return true; return true;
@ -1766,11 +1777,15 @@ bool MsckfVio::PhotometricMeasurementJacobian(
int count = 0; int count = 0;
bool valid = false; bool valid = false;
Matrix<double, 2, 4> dI_dhj;// = Matrix<double, 1, 2>::Zero(); Matrix<double, 2, 4> dI_dhj;// = Matrix<double, 1, 2>::Zero();
int valid_count = 0;
for (auto point : feature.anchorPatch_3d) for (auto point : feature.anchorPatch_3d)
{ {
// get jacobi of single point in patch // get jacobi of single point in patch
if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj)) if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj))
{
valid_count++;
valid = true; valid = true;
}
// stack point into entire jacobi // stack point into entire jacobi
H_rho.block<2, 1>(count*2, 0) = H_rhoj; H_rho.block<2, 1>(count*2, 0) = H_rhoj;
@ -1781,7 +1796,7 @@ bool MsckfVio::PhotometricMeasurementJacobian(
count++; count++;
} }
cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl;
//Eigen::Matrix<double, 2, 1> h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; //Eigen::Matrix<double, 2, 1> h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo;
//cout << "h photo: \n" << h_photo << endl; //cout << "h photo: \n" << h_photo << endl;
@ -2548,7 +2563,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
} }
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, int filter) {
MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P1 = H * state_server.state_cov * H.transpose();
@ -2559,6 +2574,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
// cout << "r" << r << endl; // cout << "r" << r << endl;
// cout << "procov" << P1+P2 << endl; // cout << "procov" << P1+P2 << endl;
if(filter == 1)
cout << "gate: " << dof << " " << gamma << " " << cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl; chi_squared_test_table[dof] << endl;
@ -2590,7 +2606,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
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]*10) { if (gamma < chi_squared_test_table[dof]) {
// cout << "passed" << endl; // cout << "passed" << endl;
return true; return true;
} else { } else {
@ -2696,7 +2712,7 @@ void MsckfVio::removeLostFeatures() {
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j))
{ {
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
cout << "passed" << endl; cout << "passed" << endl;
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
@ -2704,7 +2720,7 @@ void MsckfVio::removeLostFeatures() {
} }
} }
/*
featureJacobian(feature.id, cam_state_ids, H_xj, r_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
@ -2718,7 +2734,7 @@ void MsckfVio::removeLostFeatures() {
twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows(); twostack_cntr += twoH_xj.rows();
} }
*/
// Put an upper bound on the row size of measurement Jacobian, // Put an upper bound on the row size of measurement Jacobian,
// which helps guarantee the executation time. // which helps guarantee the executation time.
@ -2733,7 +2749,7 @@ void MsckfVio::removeLostFeatures() {
photometricMeasurementUpdate(pH_x, pr); photometricMeasurementUpdate(pH_x, pr);
} }
/*
H_x.conservativeResize(stack_cntr, H_x.cols()); H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr); r.conservativeResize(stack_cntr);
@ -2743,7 +2759,7 @@ void MsckfVio::removeLostFeatures() {
// Perform the measurement update step. // Perform the measurement update step.
measurementUpdate(H_x, r); measurementUpdate(H_x, r);
twoMeasurementUpdate(twoH_x, twor); twoMeasurementUpdate(twoH_x, twor);
*/
// Remove all processed features from the map. // Remove all processed features from the map.
for (const auto& feature_id : processed_feature_ids) for (const auto& feature_id : processed_feature_ids)
map_server.erase(feature_id); map_server.erase(feature_id);
@ -2882,14 +2898,14 @@ void MsckfVio::pruneLastCamStateBuffer()
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
{ {
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
cout << "passed" << endl; cout << "passed" << endl;
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows(); pstack_cntr += pH_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);
@ -2905,7 +2921,7 @@ void MsckfVio::pruneLastCamStateBuffer()
twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows(); twostack_cntr += twoH_xj.rows();
} }
*/
for (const auto& cam_id : involved_cam_state_ids) for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id); feature.observations.erase(cam_id);
} }
@ -2919,7 +2935,7 @@ void MsckfVio::pruneLastCamStateBuffer()
photometricMeasurementUpdate(pH_x, pr); photometricMeasurementUpdate(pH_x, pr);
} }
/*
H_x.conservativeResize(stack_cntr, H_x.cols()); H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr); r.conservativeResize(stack_cntr);
@ -2929,7 +2945,7 @@ void MsckfVio::pruneLastCamStateBuffer()
// Perform the measurement update step. // Perform the measurement update step.
measurementUpdate(H_x, r); measurementUpdate(H_x, r);
twoMeasurementUpdate(twoH_x, twor); twoMeasurementUpdate(twoH_x, twor);
*/
//reduction //reduction
int cam_sequence = std::distance(state_server.cam_states.begin(), int cam_sequence = std::distance(state_server.cam_states.begin(),
state_server.cam_states.find(rm_cam_state_id)); state_server.cam_states.find(rm_cam_state_id));
@ -3064,7 +3080,7 @@ void MsckfVio::pruneCamStateBuffer() {
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
{ {
if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// involved_cam_state_ids.size())) {
cout << "passed" << endl; cout << "passed" << endl;
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
@ -3072,7 +3088,7 @@ void MsckfVio::pruneCamStateBuffer() {
} }
} }
/*
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);
@ -3087,7 +3103,7 @@ void MsckfVio::pruneCamStateBuffer() {
twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows(); twostack_cntr += twoH_xj.rows();
} }
*/
for (const auto& cam_id : involved_cam_state_ids) for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id); feature.observations.erase(cam_id);
} }
@ -3101,7 +3117,7 @@ void MsckfVio::pruneCamStateBuffer() {
photometricMeasurementUpdate(pH_x, pr); photometricMeasurementUpdate(pH_x, pr);
} }
/*
H_x.conservativeResize(stack_cntr, H_x.cols()); H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr); r.conservativeResize(stack_cntr);
@ -3111,7 +3127,7 @@ void MsckfVio::pruneCamStateBuffer() {
// Perform the measurement update step. // Perform the measurement update step.
measurementUpdate(H_x, r); measurementUpdate(H_x, r);
twoMeasurementUpdate(twoH_x, twor); twoMeasurementUpdate(twoH_x, twor);
*/
//reduction //reduction
for (const auto& cam_id : rm_cam_state_ids) { for (const auto& cam_id : rm_cam_state_ids) {
int cam_sequence = std::distance(state_server.cam_states.begin(), int cam_sequence = std::distance(state_server.cam_states.begin(),