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

added larger sobel filter in calculation - converges sometimes for a few seconds

Browse Source
This commit is contained in:
Raphael Maenle
2019-07-05 09:43:35 +02:00
parent 3873c978dd
commit 1a07ba3d3c
3 changed files with 73 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
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.01;
double Feature::observation_noise = 0.05;
Feature::OptimizationConfig Feature::optimization_config;
map<int, double> MsckfVio::chi_squared_test_table;
@@ -312,7 +312,7 @@ bool MsckfVio::initialize() {
for (int i = 1; i < 1000; ++i) {
boost::math::chi_squared chi_squared_dist(i);
chi_squared_test_table[i] =
boost::math::quantile(chi_squared_dist, 0.05);
boost::math::quantile(chi_squared_dist, 0.1);
}
if (!createRosIO()) return false;
@@ -555,19 +555,21 @@ void MsckfVio::manageMovingWindow(
cv::Mat deeper_frame;
cam0_img_ptr->image.convertTo(deeper_frame,CV_16S);
cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
xder/=8.;
yder/=8.;
cv::Sobel(deeper_frame, xder, -1, 1, 0, 5);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 5);
xder/=72.;
yder/=72.;
cam0.moving_window[state_server.imu_state.id].dximage = xder.clone();
cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone();
cam1_img_ptr->image.convertTo(deeper_frame,CV_16S);
cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
xder/=8.;
yder/=8.;
cv::Sobel(deeper_frame, xder, -1, 1, 0, 5);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 5);
xder/=72.;
yder/=72.;
cvWaitKey(0);
cam1.moving_window[state_server.imu_state.id].dximage = xder.clone();
cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone();
@@ -1679,7 +1681,7 @@ bool MsckfVio::PhotometricPatchPointJacobian(
dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0];
dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1];
cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl;
//cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl;
// add jacobian
@@ -1718,6 +1720,10 @@ bool MsckfVio::PhotometricPatchPointJacobian(
H_plj = dI_dhj * dh_dC0pij * dC0pij_dXplj + dI_dhj * dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 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
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)
return false;
// check if point nullspaceable
if (H_rhoj(0, 0) != 0)
return true;
@@ -1751,9 +1757,11 @@ bool MsckfVio::PhotometricMeasurementJacobian(
Eigen::MatrixXd H_pA(2 * N*N, 6);
// calcualte residual of patch
PhotometricPatchPointResidual(cam_state_id, feature, r_photo);
if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo))
return false;
cout << "r\n" << r_photo << endl;
//cout << "r\n" << r_photo << endl;
// calculate jacobian for patch
int count = 0;
bool valid = false;
@@ -2446,7 +2454,6 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
r_thin = r;
}
// Compute the Kalman gain.
const MatrixXd& P = state_server.state_cov;
MatrixXd S = H_thin*P*H_thin.transpose() +
@@ -2536,12 +2543,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
MatrixXd state_cov_fixed = (state_server.state_cov +
state_server.state_cov.transpose()) / 2.0;
state_server.state_cov = state_cov_fixed;
return;
}
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
return true;
MatrixXd P1 = H * state_server.state_cov * H.transpose();
MatrixXd P2 = Feature::observation_noise *
@@ -2549,12 +2557,40 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
//cout << "gate: " << dof << " " << gamma << " " <<
//chi_squared_test_table[dof] << endl;
// cout << "r" << r << endl;
// cout << "procov" << P1+P2 << endl;
cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
if(gamma > 1000000)
{
cout << " logging " << endl;
ofstream myfile;
myfile.open("/home/raphael/dev/octave/log2octave");
myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
<< "# name: H\n"
<< "# type: matrix\n"
<< "# rows: " << H.rows() << "\n"
<< "# columns: " << H.cols() << "\n"
<< H << endl;
myfile << "# name: r\n"
<< "# type: matrix\n"
<< "# rows: " << r.rows() << "\n"
<< "# columns: " << 1 << "\n"
<< r << endl;
myfile << "# name: C\n"
<< "# type: matrix\n"
<< "# rows: " << state_server.state_cov.rows() << "\n"
<< "# columns: " << state_server.state_cov.cols() << "\n"
<< state_server.state_cov << endl;
myfile.close();
}
if (chi_squared_test_table[dof] == 0)
return false;
if (gamma < chi_squared_test_table[dof]) {
if (gamma < chi_squared_test_table[dof]*10) {
// cout << "passed" << endl;
return true;
} else {
@@ -2661,13 +2697,14 @@ void MsckfVio::removeLostFeatures() {
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)) {
//cout << "passed" << endl;
cout << "passed" << endl;
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows();
}
}
/*
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
@@ -2681,6 +2718,7 @@ void MsckfVio::removeLostFeatures() {
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
}
*/
// Put an upper bound on the row size of measurement Jacobian,
// which helps guarantee the executation time.
@@ -2695,7 +2733,7 @@ void MsckfVio::removeLostFeatures() {
photometricMeasurementUpdate(pH_x, pr);
}
/*
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@@ -2705,7 +2743,7 @@ void MsckfVio::removeLostFeatures() {
// Perform the measurement update step.
measurementUpdate(H_x, r);
twoMeasurementUpdate(twoH_x, twor);
*/
// Remove all processed features from the map.
for (const auto& feature_id : processed_feature_ids)
map_server.erase(feature_id);
@@ -2845,13 +2883,13 @@ void MsckfVio::pruneLastCamStateBuffer()
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)) {
//cout << "passed" << endl;
cout << "passed" << endl;
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows();
}
}
/*
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
@@ -2867,7 +2905,7 @@ void MsckfVio::pruneLastCamStateBuffer()
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
}
*/
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
}
@@ -2881,7 +2919,7 @@ void MsckfVio::pruneLastCamStateBuffer()
photometricMeasurementUpdate(pH_x, pr);
}
/*
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@@ -2891,7 +2929,7 @@ void MsckfVio::pruneLastCamStateBuffer()
// Perform the measurement update step.
measurementUpdate(H_x, r);
twoMeasurementUpdate(twoH_x, twor);
*/
//reduction
int cam_sequence = std::distance(state_server.cam_states.begin(),
state_server.cam_states.find(rm_cam_state_id));
@@ -3024,17 +3062,17 @@ void MsckfVio::pruneCamStateBuffer() {
if (involved_cam_state_ids.size() == 0) continue;
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())) {
//cout << "passed" << endl;
cout << "passed" << endl;
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows();
}
}
/*
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
@@ -3049,7 +3087,7 @@ void MsckfVio::pruneCamStateBuffer() {
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
}
*/
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
}
@@ -3063,6 +3101,7 @@ void MsckfVio::pruneCamStateBuffer() {
photometricMeasurementUpdate(pH_x, pr);
}
/*
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@@ -3072,7 +3111,7 @@ void MsckfVio::pruneCamStateBuffer() {
// Perform the measurement update step.
measurementUpdate(H_x, r);
twoMeasurementUpdate(twoH_x, twor);
*/
//reduction
for (const auto& cam_id : rm_cam_state_ids) {
int cam_sequence = std::distance(state_server.cam_states.begin(),