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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/msckf_vio/feature.hpp
View File

@ -441,7 +441,7 @@ bool Feature::VisualizePatch(
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
{ {
return (float)(image.at<uint8_t>(pose.x, pose.y)); return ((float)image.at<uint8_t>(pose.x, pose.y))/256;
} }
cv::Point2f Feature::projectPositionToCamera( cv::Point2f Feature::projectPositionToCamera(
@ -496,7 +496,7 @@ bool Feature::initializeAnchor(
//initialize patch Size //initialize patch Size
//TODO make N size a ros parameter //TODO make N size a ros parameter
int N = 9; int N = 3;
int n = (int)(N-1)/2; int n = (int)(N-1)/2;
auto anchor = observations.begin(); auto anchor = observations.begin();
@ -551,7 +551,7 @@ bool Feature::initializeAnchor(
return false; return false;
} }
for(auto point : vec) for(auto point : vec)
anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)point.x,(int)point.y)); anchorPatch.push_back(PixelIrradiance(point, anchorImage));
// project patch pixel to 3D space // project patch pixel to 3D space
for(auto point : und_v) for(auto point : und_v)

2
include/msckf_vio/msckf_vio.h
View File

@ -204,6 +204,8 @@ class MsckfVio {
// Photometry flag // Photometry flag
bool PHOTOMETRIC; bool PHOTOMETRIC;
bool nan_flag;
// Chi squared test table. // Chi squared test table.
static std::map<int, double> chi_squared_test_table; static std::map<int, double> chi_squared_test_table;

92
src/msckf_vio.cpp
View File

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