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

fixed minor error when not enough samples, edited feature ammount and patch size to make irradiance msckf more stable

Browse Source
This commit is contained in:
Raphael Maenle 2019-07-09 11:24:25 +02:00
parent a7c296ca3d
commit 1380ec426f
6 changed files with 79 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
config/camchain-imucam-tum-scaled.yaml
View File

@ -7,7 +7,7 @@ cam0:
camera_model: pinhole
distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202,
0.00020293673591811182]
distortion_model: pre-equidistant
distortion_model: equidistant
intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]
resolution: [512, 512]
rostopic: /cam0/image_raw
@ -25,7 +25,7 @@ cam1:
camera_model: pinhole
distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606,
0.0003299517423931039]
distortion_model: pre-equidistant
distortion_model: equidistant
intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983]
resolution: [512, 512]
rostopic: /cam1/image_raw

10
include/msckf_vio/feature.hpp
View File

@ -208,7 +208,7 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
bool estimate_FrameIrradiance(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
CameraCalibration& cam,
std::vector<double>& anchorPatch_estimate,
IlluminationParameter& estimatedIllumination) const;
@ -549,7 +549,7 @@ return delta;
bool Feature::estimate_FrameIrradiance(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
CameraCalibration& cam,
std::vector<double>& anchorPatch_estimate,
IlluminationParameter& estimated_illumination) const
{
@ -558,11 +558,11 @@ bool Feature::estimate_FrameIrradiance(
// muliply by a and add b of this frame
auto anchor = observations.begin();
if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end())
if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
return false;
double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms;
double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms;
double anchorExposureTime_ms = cam.moving_window.find(anchor->first)->second.exposureTime_ms;
double frameExposureTime_ms = cam.moving_window.find(cam_state_id)->second.exposureTime_ms;
double a_A = anchorExposureTime_ms;

4
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="3"/>
<param name="grid_max_feature_num" value="4"/>
<param name="grid_min_feature_num" value="5"/>
<param name="grid_max_feature_num" value="10"/>
<param name="pyramid_levels" value="3"/>
<param name="patch_size" value="15"/>
<param name="fast_threshold" value="10"/>

2
launch/msckf_vio_euroc.launch
View File

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

4
launch/msckf_vio_tinytum.launch
View File

@ -25,7 +25,7 @@
<param name="PrintImages" value="false"/>
<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="10"/>
<param name="max_cam_state_size" value="20"/>
<param name="position_std_threshold" value="8.0"/>
<param name="rotation_threshold" value="0.2618"/>

132
src/msckf_vio.cpp
View File

@ -277,9 +277,9 @@ bool MsckfVio::createRosIO() {
truth_sub = nh.subscribe("ground_truth", 100,
&MsckfVio::truthCallback, this);
cam0_img_sub.subscribe(nh, "cam0_image", 10);
cam1_img_sub.subscribe(nh, "cam1_image", 10);
feature_sub.subscribe(nh, "features", 10);
cam0_img_sub.subscribe(nh, "cam0_image", 100);
cam1_img_sub.subscribe(nh, "cam1_image", 100);
feature_sub.subscribe(nh, "features", 100);
image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub);
image_sub.registerCallback(&MsckfVio::imageCallback, this);
@ -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.4);
boost::math::quantile(chi_squared_dist, 0.05);
}
if (!createRosIO()) return false;
@ -703,7 +703,7 @@ bool MsckfVio::resetCallback(
is_first_img = true;
// Restart the subscribers.
imu_sub = nh.subscribe("imu", 100,
imu_sub = nh.subscribe("imu", 1000,
&MsckfVio::imuCallback, this);
truth_sub = nh.subscribe("ground_truth", 100,
@ -1563,25 +1563,28 @@ bool MsckfVio::PhotometricPatchPointResidual(
const Feature& feature,
VectorXd& r)
{
VectorXd r_photo = VectorXd::Zero(2*N*N);
int count = 0;
const CAMState& cam_state = state_server.cam_states[cam_state_id];
//estimate photometric measurement
std::vector<double> estimate_irradiance;
std::vector<double> estimate_photo_z;
IlluminationParameter estimated_illumination;
std::vector<double> estimate_photo_z_c0, estimate_photo_z_c1;
std::vector<double> photo_z_c0, photo_z_c1;
// estimate irradiance based on anchor frame
/*
IlluminationParameter estimated_illumination;
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
for (auto& estimate_irradiance_j : estimate_irradiance)
estimate_photo_z.push_back (estimate_irradiance_j);// *
estimate_photo_z_c0.push_back (estimate_irradiance_j);// *
//estimated_illumination.frame_gain * estimated_illumination.feature_gain +
//estimated_illumination.frame_bias + estimated_illumination.feature_bias);
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam1, estimate_irradiance, estimated_illumination);
for (auto& estimate_irradiance_j : estimate_irradiance)
estimate_photo_z_c1.push_back (estimate_irradiance_j);// *
//estimated_illumination.frame_gain * estimated_illumination.feature_gain +
//estimated_illumination.frame_bias + estimated_illumination.feature_bias);
*/
// irradiance measurement around feature point in c0 and c1
std::vector<double> true_irradiance_c0, true_irradiance_c1;
@ -1615,9 +1618,11 @@ bool MsckfVio::PhotometricPatchPointResidual(
// add observation
photo_z_c0.push_back(feature.PixelIrradiance(p_in_c0, current_image_c0));
photo_z_c1.push_back(feature.PixelIrradiance(p_in_c1, current_image_c1));
// calculate photom. residual acc. to paper
// r_photo(count) = photo_z[count] - estimate_photo_z[count];
//r_photo(count*2) = photo_z_c0[count] - estimate_photo_z_c0[count];
//r_photo(count*2+1) = photo_z_c1[count] - estimate_photo_z_c1[count];
// calculate photom. residual alternating between frames
r_photo(count*2) = true_irradiance_c0[count] - photo_z_c0[count];
r_photo(count*2+1) = true_irradiance_c1[count] - photo_z_c1[count];
@ -1747,13 +1752,12 @@ bool MsckfVio::PhotometricMeasurementJacobian(
const FeatureIDType& feature_id,
MatrixXd& H_x, MatrixXd& H_y, VectorXd& r)
{
// Prepare all the required data.
const CAMState& cam_state = state_server.cam_states[cam_state_id];
const Feature& feature = map_server[feature_id];
//photometric observation
VectorXd r_photo = VectorXd::Zero(N*N);
VectorXd r_photo;
// one line of the NxN Jacobians
Eigen::Matrix<double, 2, 1> H_rhoj;
@ -1771,7 +1775,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo))
return false;
//cout << "r\n" << r_photo << endl;
// calculate jacobian for patch
int count = 0;
@ -1796,13 +1799,13 @@ 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;
// 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;
// construct the jacobian structure needed for nullspacing
MatrixXd H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7);
MatrixXd H_yl = MatrixXd::Zero(2*N*N, 1);
MatrixXd H_xl;
MatrixXd H_yl;
ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl);
@ -1844,6 +1847,9 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
Eigen::MatrixXd& H_xl,
Eigen::MatrixXd& H_yl)
{
H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7);
H_yl = MatrixXd::Zero(2*N*N, 1);
// get position of anchor in cam states
auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
@ -1867,9 +1873,9 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
/*for(int i = 0; i< N*N; i++)
H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
*/
*/
H_yl = H_rho;
H_yl.block(0, 0, H_rho.rows(), H_rho.cols()) = H_rho;
}
@ -1894,23 +1900,35 @@ bool MsckfVio::PhotometricFeatureJacobian(
}
int jacobian_row_size = 0;
jacobian_row_size = 2 * N * N * valid_cam_state_ids.size();
// stacked feature jacobians
MatrixXd H_xi;
MatrixXd H_yi;
VectorXd r_i;
MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
21+state_server.cam_states.size()*7);
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); // CHANGED N*N+1 to 1
VectorXd r_i = VectorXd::Zero(jacobian_row_size);
// temporary measurement jacobians
MatrixXd H_xl;
MatrixXd H_yl;
Eigen::VectorXd r_l;
int stack_cntr = 0;
bool first = true;
// go through every valid state the feature was observed in
for (const auto& cam_id : valid_cam_state_ids) {
MatrixXd H_xl;
MatrixXd H_yl;
Eigen::VectorXd r_l = VectorXd::Zero(2*N*N);
// skip observation if measurement is not valid
if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l))
continue;
// set size of stacking jacobians, once the returned jacobians are known
if(first)
{
first = not first;
jacobian_row_size = r_l.size() * valid_cam_state_ids.size();
H_xi = MatrixXd::Zero(jacobian_row_size, H_xl.cols());
H_yi = MatrixXd::Zero(jacobian_row_size, H_yl.cols()); // CHANGED N*N+1 to 1
r_i = VectorXd::Zero(jacobian_row_size);
}
auto cam_state_iter = state_server.cam_states.find(cam_id);
int cam_state_cntr = std::distance(
state_server.cam_states.begin(), cam_state_iter);
@ -1918,12 +1936,12 @@ bool MsckfVio::PhotometricFeatureJacobian(
// Stack the Jacobians.
H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl;
H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
r_i.segment(stack_cntr, 2*N*N) = r_l;
stack_cntr += 2*N*N;
r_i.segment(stack_cntr, r_l.size()) = r_l;
stack_cntr += r_l.size();
}
// if not enough to propper nullspace (in paper implementation)
if(stack_cntr < 2*N*N)
if(stack_cntr < r_l.size())
return false;
// Project the residual and Jacobians onto the nullspace
@ -1936,6 +1954,7 @@ bool MsckfVio::PhotometricFeatureJacobian(
if(H_yi(i,0) != 0)
valid = true;
FullPivLU<MatrixXd> lu(H_yi.transpose());
MatrixXd A_null_space = lu.kernel();
@ -2004,7 +2023,9 @@ bool MsckfVio::PhotometricFeatureJacobian(
cout << "---------- LOGGED -------- " << endl;
}
return true;
if(valid)
return true;
return false;
}
void MsckfVio::measurementJacobian(
@ -2435,10 +2456,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
if (H.rows() == 0 || r.rows() == 0)
{
cout << "zero" << endl;
return;
}
return;
// Decompose the final Jacobian matrix to reduce computational
// complexity as in Equation (28), (29).
MatrixXd H_thin;
@ -2572,12 +2590,6 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
// cout << "r" << r << endl;
// cout << "procov" << P1+P2 << endl;
if(filter == 1)
cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
if(gamma > 1000000)
{
cout << " logging " << endl;
@ -2608,6 +2620,9 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
return false;
if (gamma < chi_squared_test_table[dof]) {
// cout << "passed" << endl;
if(filter == 1)
cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
return true;
} else {
// cout << "failed" << endl;
@ -2713,13 +2728,11 @@ void MsckfVio::removeLostFeatures() {
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j))
{
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, cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
@ -2735,7 +2748,6 @@ void MsckfVio::removeLostFeatures() {
twostack_cntr += twoH_xj.rows();
}
// Put an upper bound on the row size of measurement Jacobian,
// which helps guarantee the executation time.
//if (stack_cntr > 1500) break;
@ -2745,11 +2757,8 @@ void MsckfVio::removeLostFeatures() {
{
pH_x.conservativeResize(pstack_cntr, pH_x.cols());
pr.conservativeResize(pstack_cntr);
photometricMeasurementUpdate(pH_x, pr);
}
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@ -2898,8 +2907,8 @@ 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(), 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();
@ -2921,27 +2930,24 @@ 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);
}
if(pstack_cntr)
{
pH_x.conservativeResize(pstack_cntr, pH_x.cols());
pr.conservativeResize(pstack_cntr);
photometricMeasurementUpdate(pH_x, pr);
}
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
twor.conservativeResize(twostack_cntr);
// Perform the measurement update step.
measurementUpdate(H_x, r);
twoMeasurementUpdate(twoH_x, twor);
@ -3081,17 +3087,14 @@ 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(), 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;
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);
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;
r.segment(stack_cntr, r_j.rows()) = r_j;
@ -3104,11 +3107,11 @@ void MsckfVio::pruneCamStateBuffer() {
twostack_cntr += twoH_xj.rows();
}
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
}
if(pstack_cntr)
{
@ -3117,7 +3120,6 @@ void MsckfVio::pruneCamStateBuffer() {
photometricMeasurementUpdate(pH_x, pr);
}
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);