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

fixed non 0 filling of new state covariance

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-25 19:13:22 +02:00
parent de07296d31
commit e2e936ff01
3 changed files with 65 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
src/msckf_vio.cpp
View File

@@ -55,6 +55,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh):
is_gravity_set(false),
is_first_img(true),
image_sub(10),
nan_flag(false),
nh(pnh) {
return;
}
@@ -488,6 +489,7 @@ bool MsckfVio::resetCallback(
std_srvs::Trigger::Request& req,
std_srvs::Trigger::Response& res) {
cout << "reset" << endl;
ROS_WARN("Start resetting msckf vio...");
// Temporarily shutdown the subscribers to prevent the
// state from updating.
@@ -901,8 +903,10 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) {
size_t old_rows = state_server.state_cov.rows();
size_t old_cols = state_server.state_cov.cols();
MatrixXd temp_cov = state_server.state_cov;
// add 7 for camera state + irradiance bias eta = b_l
state_server.state_cov.conservativeResize(old_rows+7, old_cols+7);
state_server.state_cov.conservativeResizeLike(Eigen::MatrixXd::Zero(old_rows+7, old_cols+7));
// Rename some matrix blocks for convenience.
const Matrix<double, 21, 21>& P11 =
@@ -918,13 +922,13 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) {
J * P11 * J.transpose();
// Add photometry P_eta and surrounding Zeros
state_server.state_cov.block<1, 12>(old_rows+6, 0) = Matrix<double, 1, 12>::Zero();
state_server.state_cov.block<12, 1>(0, old_cols+6) = Matrix<double, 12, 1>::Zero();
state_server.state_cov(old_rows+6, old_cols+6) = irradiance_frame_bias;
// Fix the covariance to be symmetric
MatrixXd state_cov_fixed = (state_server.state_cov +
state_server.state_cov.transpose()) / 2.0;
state_server.state_cov = state_cov_fixed;
return;
}
@@ -976,7 +980,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
const Vector3d& t_c0_w = cam_state.position;
//temp N
const int N = 9;
const int N = 3;
// Cam1 pose.
Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
@@ -1112,7 +1116,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
//TODO make this more fluent as well
count = 0;
for(auto data : photo_r)
r[count++] = data;
r[count++] = data;
return;
}
@@ -1124,7 +1128,7 @@ void MsckfVio::PhotometricFeatureJacobian(
const auto& feature = map_server[feature_id];
int N = 9;
int N = 3;
// Check how many camera states in the provided camera
// id camera has actually seen this feature.
vector<StateIDType> valid_cam_state_ids(0);
@@ -1194,26 +1198,24 @@ void MsckfVio::PhotometricFeatureJacobian(
int sv_size = 0;
Eigen::VectorXd singularValues = svd_helper.singularValues();
for(int i = 0; i < singularValues.size(); i++)
if(singularValues[i] < 1e-3)
if(singularValues[i] > 1e-5)
sv_size++;
int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size();
MatrixXd A = svd_helper.matrixU().rightCols(
jacobian_row_size-null_space_size);
jacobian_row_size-sv_size);
H_x = A.transpose() * H_xi;
r = A.transpose() * r_i;
cout << "r\n" << r << endl;
cout << "Hx\n" << H_x << endl;
return;
}
void MsckfVio::measurementJacobian(
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r) {
Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r)
{
// Prepare all the required data.
const CAMState& cam_state = state_server.cam_states[cam_state_id];
@@ -1275,7 +1277,8 @@ void MsckfVio::measurementJacobian(
void MsckfVio::featureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
MatrixXd& H_x, VectorXd& r) {
MatrixXd& H_x, VectorXd& r)
{
const auto& feature = map_server[feature_id];
@@ -1322,29 +1325,28 @@ void MsckfVio::featureJacobian(
int sv_size = 0;
Eigen::VectorXd singularValues = svd_helper.singularValues();
for(int i = 0; i < singularValues.size(); i++)
if(singularValues[i] < 1e-3)
if(singularValues[i] > 1e-5)
sv_size++;
int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size();
//cout << "singular values: \n" << svd_helper.singularValues();
cout << "null_space: " << null_space_size << endl;
int null_space_size = svd_helper.matrixU().cols() - sv_size;
MatrixXd A = svd_helper.matrixU().rightCols(
jacobian_row_size - 3);
jacobian_row_size - sv_size);
H_x = A.transpose() * H_xj;
r = A.transpose() * r_j;
cout << "A: \n" << A.transpose() << endl;
return;
}
void MsckfVio::measurementUpdate(
const MatrixXd& H, const VectorXd& r) {
void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
if (H.rows() == 0 || r.rows() == 0) return;
cout << "decomposition...";
// Decompose the final Jacobian matrix to reduce computational
// complexity as in Equation (28), (29).
MatrixXd H_thin;
@@ -1377,7 +1379,8 @@ void MsckfVio::measurementUpdate(
H_thin = H;
r_thin = r;
}
cout << "done" << endl;
cout << "computing K...";
// Compute the Kalman gain.
const MatrixXd& P = state_server.state_cov;
MatrixXd S = H_thin*P*H_thin.transpose() +
@@ -1387,6 +1390,8 @@ void MsckfVio::measurementUpdate(
MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
MatrixXd K = K_transpose.transpose();
cout << "done" << endl;
// Compute the error of the state.
VectorXd delta_x = K * r_thin;
@@ -1444,12 +1449,19 @@ void MsckfVio::measurementUpdate(
return;
}
bool MsckfVio::gatingTest(
const MatrixXd& H, const VectorXd& r, const int& dof) {
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
MatrixXd P1 = H * state_server.state_cov * H.transpose();
MatrixXd P2 = Feature::observation_noise *
MatrixXd::Identity(H.rows(), H.rows());
//cout << "H: \n" << H << endl;
//cout << "cov: \n" << state_server.state_cov << endl;
//cout << "P1: \n" << P1 << endl;
//cout << "solv: \n" << (P1+P2).ldlt().solve(r) << endl;
double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
cout << dof << " " << gamma << " " <<
@@ -1465,13 +1477,14 @@ bool MsckfVio::gatingTest(
}
void MsckfVio::removeLostFeatures() {
// Remove the features that lost track.
// BTW, find the size the final Jacobian matrix and residual vector.
int jacobian_row_size = 0;
vector<FeatureIDType> invalid_feature_ids(0);
vector<FeatureIDType> processed_feature_ids(0);
int N = 3;
for (auto iter = map_server.begin();
iter != map_server.end(); ++iter) {
// Rename the feature to be checked.
@@ -1507,7 +1520,11 @@ void MsckfVio::removeLostFeatures() {
}
}
jacobian_row_size += 4*feature.observations.size() - 3;
if(PHOTOMETRIC)
//just use max. size, as gets shrunken down after anyway
jacobian_row_size += N*N*feature.observations.size();
else
jacobian_row_size += 4*feature.observations.size() - 3;
processed_feature_ids.push_back(feature.id);
}
@@ -1523,8 +1540,12 @@ void MsckfVio::removeLostFeatures() {
// Return if there is no lost feature to be processed.
if (processed_feature_ids.size() == 0) return;
int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
MatrixXd H_x = MatrixXd::Zero(jacobian_row_size,
21+6*state_server.cam_states.size());
21+augmentationSize*state_server.cam_states.size());
VectorXd r = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
@@ -1548,11 +1569,11 @@ void MsckfVio::removeLostFeatures() {
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();
cout << "made gating test" << endl;
cout << "approved chi" << endl;
}
else
{
cout << "failed gating test" << endl;
cout << "rejected chi" << endl;
}
// Put an upper bound on the row size of measurement Jacobian,
@@ -1573,8 +1594,7 @@ void MsckfVio::removeLostFeatures() {
return;
}
void MsckfVio::findRedundantCamStates(
vector<StateIDType>& rm_cam_state_ids) {
void MsckfVio::findRedundantCamStates(vector<StateIDType>& rm_cam_state_ids) {
// Move the iterator to the key position.
auto key_cam_state_iter = state_server.cam_states.end();
@@ -1627,6 +1647,8 @@ void MsckfVio::pruneCamStateBuffer() {
vector<StateIDType> rm_cam_state_ids(0);
findRedundantCamStates(rm_cam_state_ids);
int N = 3;
// Find the size of the Jacobian matrix.
int jacobian_row_size = 0;
for (auto& item : map_server) {
@@ -1671,7 +1693,10 @@ void MsckfVio::pruneCamStateBuffer() {
continue;
}
}
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
if(PHOTOMETRIC)
jacobian_row_size += N*N*involved_cam_state_ids.size();
else
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
}
//cout << "jacobian row #: " << jacobian_row_size << endl;
@@ -1716,9 +1741,12 @@ void MsckfVio::pruneCamStateBuffer() {
feature.observations.erase(cam_id);
}
cout << "resize" << endl;
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
cout << "done" << endl;
// Perform measurement update.
measurementUpdate(H_x, r);