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

removed comments, reg. msckf not working currently

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-28 18:21:36 +02:00
parent d013a1b080
commit 58fe991647
3 changed files with 10 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
src/msckf_vio.cpp
View File

@@ -1547,7 +1547,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
int count = 0;
double dx, dy;
cout << "patching" << endl;
for (auto point : feature.anchorPatch_3d)
{
//cout << "____feature-measurement_____\n" << endl;
@@ -1666,7 +1665,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
//feature.VisualizeKernel(cam_state, cam_state_id, cam0);
}
cout << "returning" << endl;
return true;
}
@@ -1797,8 +1795,6 @@ bool MsckfVio::PhotometricFeatureJacobian(
cout << "---------- LOGGED -------- " << endl;
}
cout << "donefeature" << endl;
return true;
}
@@ -2166,8 +2162,6 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
// Compute the error of the state.
VectorXd delta_x = K * r;
cout << "two rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
cout << "two update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// Update the IMU state.
if (FILTER != 2) return;
@@ -2272,13 +2266,9 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
// Compute the error of the state.
VectorXd delta_x = K * r;
cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// Update the IMU state.
if (FILTER != 1) return;
cout << "here" << endl;
if(PRINTIMAGES)
{
//octave
@@ -2438,9 +2428,6 @@ void MsckfVio::removeLostFeatures() {
// processed_feature_ids.size() << endl;
//cout << "jacobian row #: " << jacobian_row_size << endl;
cout << "sizing" << endl;
// Remove the features that do not have enough measurements.
for (const auto& feature_id : invalid_feature_ids)
map_server.erase(feature_id);
@@ -2479,27 +2466,19 @@ void MsckfVio::removeLostFeatures() {
MatrixXd twoH_xj;
VectorXd twor_j;
cout << "measuring" << endl;
/*
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true);
{
cout << "p gating" << endl;
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
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();
}
}
}*/
cout << "donephoto" << endl;
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
cout << "two" << endl;
twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
cout << "gating" << endl;
if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j;
@@ -2524,7 +2503,6 @@ void MsckfVio::removeLostFeatures() {
photometricMeasurementUpdate(pH_x, pr);
}
cout << "resizing" << endl;
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@@ -2670,26 +2648,21 @@ void MsckfVio::pruneLastCamStateBuffer()
for (const auto& cam_state : state_server.cam_states)
involved_cam_state_ids.push_back(cam_state.first);
cout << "measuring" << endl;
/*
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true);
{
cout << "p gating" << endl;
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
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();
}
}
}*/
cout << "norm" << endl;
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
cout << "two" << endl;
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
cout << "gating" << endl;
if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j;
@@ -2857,23 +2830,18 @@ void MsckfVio::pruneCamStateBuffer() {
}
if (involved_cam_state_ids.size() == 0) continue;
cout << "measuring" << endl;
/*
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
{
cout << "p gating" << endl;
if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
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();
}
}
cout << "norm" << endl;
}*/
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
cout << "two" << endl;
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
cout << "gating" << endl;
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;