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);
}
bool Feature::VisualizePatch(
const CAMState& cam_state,
const StateIDType& cam_state_id,
@ -934,7 +935,7 @@ bool Feature::VisualizePatch(
//cv::imwrite(loc.str(), cam0.featureVisu);
cv::imshow("patch", cam0.featureVisu);
cvWaitKey(0);
cvWaitKey(1);
}
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);
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 findRedundantCamStates(
std::vector<StateIDType>& rm_cam_state_ids);

4
launch/msckf_vio_euroc.launch
View File

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

68
src/msckf_vio.cpp
View File

@ -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.1);
boost::math::quantile(chi_squared_dist, 0.4);
}
if (!createRosIO()) return false;
@ -554,23 +554,34 @@ void MsckfVio::manageMovingWindow(
cv::Mat yder;
cv::Mat deeper_frame;
// generate derivative matrix for cam0
cam0_img_ptr->image.convertTo(deeper_frame,CV_16S);
cv::Sobel(deeper_frame, xder, -1, 1, 0, 5);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 5);
xder/=72.;
yder/=72.;
xder/=96.;
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].dyimage = yder.clone();
// generate derivative matrix for cam 1
cam1_img_ptr->image.convertTo(deeper_frame,CV_16S);
cv::Sobel(deeper_frame, xder, -1, 1, 0, 5);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 5);
xder/=72.;
yder/=72.;
xder/=96.;
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);
*/
// save into moving window
cam1.moving_window[state_server.imu_state.id].dximage = xder.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
// 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;
*/
// check if point nullspaceable
if (H_rhoj(0, 0) != 0)
return true;
@ -1766,11 +1777,15 @@ bool MsckfVio::PhotometricMeasurementJacobian(
int count = 0;
bool valid = false;
Matrix<double, 2, 4> dI_dhj;// = Matrix<double, 1, 2>::Zero();
int valid_count = 0;
for (auto point : feature.anchorPatch_3d)
{
// 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))
{
valid_count++;
valid = true;
}
// stack point into entire jacobi
H_rho.block<2, 1>(count*2, 0) = H_rhoj;
@ -1781,7 +1796,7 @@ bool MsckfVio::PhotometricMeasurementJacobian(
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;
//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();
@ -2559,6 +2574,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
// cout << "r" << r << endl;
// cout << "procov" << P1+P2 << endl;
if(filter == 1)
cout << "gate: " << dof << " " << gamma << " " <<
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)
return false;
if (gamma < chi_squared_test_table[dof]*10) {
if (gamma < chi_squared_test_table[dof]) {
// cout << "passed" << endl;
return true;
} else {
@ -2696,7 +2712,7 @@ 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)) {
if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
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;
@ -2704,7 +2720,7 @@ void MsckfVio::removeLostFeatures() {
}
}
/*
featureJacobian(feature.id, cam_state_ids, H_xj, r_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;
twostack_cntr += twoH_xj.rows();
}
*/
// Put an upper bound on the row size of measurement Jacobian,
// which helps guarantee the executation time.
@ -2733,7 +2749,7 @@ void MsckfVio::removeLostFeatures() {
photometricMeasurementUpdate(pH_x, pr);
}
/*
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@ -2743,7 +2759,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);
@ -2882,14 +2898,14 @@ 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)) {
if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
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);
@ -2905,7 +2921,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);
}
@ -2919,7 +2935,7 @@ void MsckfVio::pruneLastCamStateBuffer()
photometricMeasurementUpdate(pH_x, pr);
}
/*
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@ -2929,7 +2945,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));
@ -3064,7 +3080,7 @@ void MsckfVio::pruneCamStateBuffer() {
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;
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
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);
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;
twostack_cntr += twoH_xj.rows();
}
*/
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
}
@ -3101,7 +3117,7 @@ void MsckfVio::pruneCamStateBuffer() {
photometricMeasurementUpdate(pH_x, pr);
}
/*
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@ -3111,7 +3127,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(),