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

added live toggle for photometric

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-25 19:05:53 +02:00
parent 6f7f8b7892
commit 1d6100ed13
5 changed files with 55 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
src/msckf_vio.cpp
View File

@@ -412,6 +412,8 @@ void MsckfVio::imageCallback(
if(STREAMPAUSE)
nh.setParam("/play_bag", false);
nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
// Start the system if the first image is received.
// The frame where the first image is received will be
// the origin.
@@ -448,7 +450,7 @@ void MsckfVio::imageCallback(
double imu_processing_time = (
ros::Time::now()-start_time).toSec();
//cout << "1" << endl;
// cout << "1" << endl;
// Augment the state vector.
start_time = ros::Time::now();
//truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
@@ -457,7 +459,7 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec();
//cout << "2" << endl;
// cout << "2" << endl;
// Add new observations for existing features or new
// features in the map server.
start_time = ros::Time::now();
@@ -466,7 +468,7 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec();
//cout << "3" << endl;
// cout << "3" << endl;
// Add new images to moving window
start_time = ros::Time::now();
manageMovingWindow(cam0_img, cam1_img, feature_msg);
@@ -474,20 +476,20 @@ void MsckfVio::imageCallback(
ros::Time::now()-start_time).toSec();
//cout << "4" << endl;
// cout << "4" << endl;
// Perform measurement update if necessary.
start_time = ros::Time::now();
removeLostFeatures();
double remove_lost_features_time = (
ros::Time::now()-start_time).toSec();
//cout << "5" << endl;
// cout << "5" << endl;
start_time = ros::Time::now();
pruneLastCamStateBuffer();
pruneCamStateBuffer();
double prune_cam_states_time = (
ros::Time::now()-start_time).toSec();
//cout << "6" << endl;
// cout << "6" << endl;
// Publish the odometry.
start_time = ros::Time::now();
publish(feature_msg->header.stamp);
@@ -1368,7 +1370,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
}
MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
//MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1);
// set anchor Jakobi
// get position of anchor in cam states
@@ -1389,13 +1393,17 @@ void MsckfVio::PhotometricMeasurementJacobian(
H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N);
// set irradiance error Block
H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N);
//H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N);
// TODO make this calculation more fluent
/*
for(int i = 0; i< N*N; i++)
H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho;
*/
H_yl.block(0, 0, N*N, N*N) = Eigen::MatrixXd::Identity(N*N, N*N);
H_yl.block(0, N*N, N*N, 1) = -H_rho;
H_x = H_xl;
H_y = H_yl;
@@ -1439,7 +1447,10 @@ void MsckfVio::PhotometricFeatureJacobian(
MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
21+state_server.cam_states.size()*7);
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1);
// MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1);
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+1);
VectorXd r_i = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
@@ -1460,6 +1471,8 @@ void MsckfVio::PhotometricFeatureJacobian(
H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
r_i.segment(stack_cntr, N*N) = r_l;
stack_cntr += N*N;
}
// Project the residual and Jacobians onto the nullspace
@@ -1798,8 +1811,10 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
if (H.rows() == 0 || r.rows() == 0)
if (H.rows() == 0 || r.rows() == 0)
{
return;
}
// Decompose the final Jacobian matrix to reduce computational
// complexity as in Equation (28), (29).
MatrixXd H_thin;
@@ -1840,10 +1855,13 @@ 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 (not PHOTOMETRIC) return;
cout << "msc veloci: " << delta_x[6] << ", " << delta_x[7] << ", " << delta_x[8] << endl;
cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
const VectorXd& delta_x_imu = delta_x.head<21>();
@@ -1900,6 +1918,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
return 1;
MatrixXd P1 = H * state_server.state_cov * H.transpose();
@@ -1908,8 +1927,8 @@ 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 << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
if (chi_squared_test_table[dof] == 0)
return false;
@@ -2008,12 +2027,12 @@ void MsckfVio::removeLostFeatures() {
PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
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;
stack_cntr += H_xj.rows();
}
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;
@@ -2180,7 +2199,6 @@ void MsckfVio::pruneLastCamStateBuffer()
feature.observations.erase(cam_id);
}
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@@ -2188,7 +2206,6 @@ void MsckfVio::pruneLastCamStateBuffer()
pr.conservativeResize(pstack_cntr);
// Perform measurement update.
measurementUpdate(H_x, r);
photometricMeasurementUpdate(pH_x, pr);