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

added some debug output, added qr decomp

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-19 09:13:46 +02:00
parent dcbc69a1c4
commit 2d045660c2
3 changed files with 55 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/msckf_vio/msckf_vio.h
View File

@@ -236,6 +236,7 @@ class MsckfVio {
bool PHOTOMETRIC;
// debug flag
bool STREAMPAUSE;
bool PRINTIMAGES;
bool GROUNDTRUTH;

7
launch/msckf_vio_tum.launch
View File

@@ -18,13 +18,14 @@
output="screen">
<!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="true"/>
<param name="PHOTOMETRIC" value="false"/>
<!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/>
<param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="5"/>
<param name="patch_size_n" value="3"/>
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>
@@ -71,4 +72,6 @@
</node>
</group>
<node name="player" pkg="bagcontrol" type="control.py" />
</launch>

114
src/msckf_vio.cpp
View File

@@ -69,6 +69,7 @@ bool MsckfVio::loadParameters() {
//Photometry Flag
nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
nh.param<bool>("PrintImages", PRINTIMAGES, false);
nh.param<bool>("StreamPause", STREAMPAUSE, false);
nh.param<bool>("GroundTruth", GROUNDTRUTH, false);
// Frame id
@@ -304,10 +305,10 @@ bool MsckfVio::initialize() {
// Initialize the chi squared test table with confidence
// level 0.95.
for (int i = 1; i < 100; ++i) {
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.2);
boost::math::quantile(chi_squared_dist, 0.05);
}
if (!createRosIO()) return false;
@@ -407,6 +408,10 @@ void MsckfVio::imageCallback(
// Return if the gravity vector has not been set.
if (!is_gravity_set) return;
// stop playing bagfile if printing images
if(STREAMPAUSE)
nh.setParam("/play_bag", false);
// Start the system if the first image is received.
// The frame where the first image is received will be
// the origin.
@@ -443,6 +448,7 @@ void MsckfVio::imageCallback(
double imu_processing_time = (
ros::Time::now()-start_time).toSec();
//cout << "1" << endl;
// Augment the state vector.
start_time = ros::Time::now();
if(PHOTOMETRIC)
@@ -455,6 +461,8 @@ void MsckfVio::imageCallback(
double state_augmentation_time = (
ros::Time::now()-start_time).toSec();
//cout << "2" << endl;
// Add new observations for existing features or new
// features in the map server.
start_time = ros::Time::now();
@@ -462,23 +470,29 @@ void MsckfVio::imageCallback(
double add_observations_time = (
ros::Time::now()-start_time).toSec();
//cout << "3" << endl;
// Add new images to moving window
start_time = ros::Time::now();
manageMovingWindow(cam0_img, cam1_img, feature_msg);
double manage_moving_window_time = (
ros::Time::now()-start_time).toSec();
//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;
start_time = ros::Time::now();
pruneLastCamStateBuffer();
pruneCamStateBuffer();
double prune_cam_states_time = (
ros::Time::now()-start_time).toSec();
//cout << "6" << endl;
// Publish the odometry.
start_time = ros::Time::now();
publish(feature_msg->header.stamp);
@@ -509,6 +523,8 @@ void MsckfVio::imageCallback(
publish_time, publish_time/processing_time);
}
if(STREAMPAUSE)
nh.setParam("/play_bag", true);
return;
}
@@ -1428,16 +1444,8 @@ void MsckfVio::PhotometricFeatureJacobian(
MatrixXd& H_x, VectorXd& r)
{
// stop playing bagfile if printing images
if(PRINTIMAGES)
{
std::cout << "stopped playpack" << std::endl;
nh.setParam("/play_bag", false);
}
const auto& feature = map_server[feature_id];
// Check how many camera states in the provided camera
// id camera has actually seen this feature.
vector<StateIDType> valid_cam_state_ids(0);
@@ -1538,13 +1546,8 @@ void MsckfVio::PhotometricFeatureJacobian(
myfile.close();
cout << "---------- LOGGED -------- " << endl;
cvWaitKey(0);
}
}
if(PRINTIMAGES)
{
std::cout << "resume playback" << std::endl;
nh.setParam("/play_bag", true);
}
return;
}
@@ -1619,12 +1622,6 @@ void MsckfVio::featureJacobian(
MatrixXd& H_x, VectorXd& r)
{
// stop playing bagfile if printing images
if(PRINTIMAGES)
{
std::cout << "stopped playpack" << std::endl;
nh.setParam("/play_bag", false);
}
const auto& feature = map_server[feature_id];
@@ -1686,9 +1683,6 @@ void MsckfVio::featureJacobian(
H_x = A.transpose() * H_xj;
r = A.transpose() * r_j;
cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl;
// stop playing bagfile if printing images
if(PRINTIMAGES)
{
@@ -1698,8 +1692,6 @@ void MsckfVio::featureJacobian(
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
myfile.close();
cout << "---------- LOGGED -------- " << endl;
std::cout << "stopped playpack" << std::endl;
nh.setParam("/play_bag", true);
}
return;
@@ -1708,8 +1700,8 @@ void MsckfVio::featureJacobian(
void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
if (H.rows() == 0 || r.rows() == 0) return;
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;
@@ -1718,7 +1710,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
if(PHOTOMETRIC)
augmentationSize = 7;
/*
// QR decomposition to make stuff faster
if (H.rows() > H.cols()) {
// Convert H to a sparse matrix.
SparseMatrix<double> H_sparse = H.sparseView();
@@ -1736,29 +1728,24 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
H_thin = H_temp.topRows(21+state_server.cam_states.size()*augmentationSize);
r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize);
//HouseholderQR<MatrixXd> qr_helper(H);
//MatrixXd Q = qr_helper.householderQ();
//MatrixXd Q1 = Q.leftCols(21+state_server.cam_states.size()*6);
//H_thin = Q1.transpose() * H;
//r_thin = Q1.transpose() * r;
} else {
H_thin = H;
r_thin = r;
}
*/
// Compute the Kalman gain.
const MatrixXd& P = state_server.state_cov;
MatrixXd S = H*P*H.transpose() +
MatrixXd S = H_thin*P*H_thin.transpose() +
Feature::observation_noise*MatrixXd::Identity(
H.rows(), H.rows());
H_thin.rows(), H_thin.rows());
//MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P);
MatrixXd K_transpose = S.ldlt().solve(H*P);
cout << "inverting: " << S.rows() << ":" << S.cols() << endl;
MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
MatrixXd K = K_transpose.transpose();
// Compute the error of the state.
VectorXd delta_x = K * r;
cout << "update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// Update the IMU state.
const VectorXd& delta_x_imu = delta_x.head<21>();
@@ -1800,7 +1787,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
}
// Update state covariance.
MatrixXd I_KH = MatrixXd::Identity(K.rows(), H.cols()) - K*H;
MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin;
//state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() +
// K*K.transpose()*Feature::observation_noise;
state_server.state_cov = I_KH*state_server.state_cov;
@@ -1814,7 +1801,6 @@ void MsckfVio::measurementUpdate(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();
@@ -1827,7 +1813,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
chi_squared_test_table[dof] << endl;
if (chi_squared_test_table[dof] == 0)
return true;
return false;
if (gamma < chi_squared_test_table[dof]) {
//cout << "passed" << endl;
return true;
@@ -1929,11 +1915,6 @@ 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 << "passed" << endl;
}
else
{
cout << "rejected" << endl;
}
// Put an upper bound on the row size of measurement Jacobian,
@@ -1941,6 +1922,8 @@ void MsckfVio::removeLostFeatures() {
if (stack_cntr > 1500) break;
}
cout << "processed features: " << processed_feature_ids.size() << endl;
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
@@ -2000,6 +1983,7 @@ void MsckfVio::findRedundantCamStates(vector<StateIDType>& rm_cam_state_ids) {
void MsckfVio::pruneLastCamStateBuffer()
{
if (state_server.cam_states.size() < max_cam_state_size)
return;
@@ -2058,7 +2042,7 @@ void MsckfVio::pruneLastCamStateBuffer()
MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size());
VectorXd r = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
int pruned_cntr = 0;
for (auto& item : map_server) {
auto& feature = item.second;
@@ -2077,24 +2061,26 @@ void MsckfVio::pruneLastCamStateBuffer()
else
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
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;
stack_cntr += H_xj.rows();
cout << "passed" << endl;
}
else
{
cout << "rejected" << endl;
pruned_cntr++;
}
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
}
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
// Perform measurement update.
measurementUpdate(H_x, r);
if(pruned_cntr != 0)
{
cout << "pruned features: " << pruned_cntr << endl;
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
// Perform measurement update.
measurementUpdate(H_x, r);
}
//reduction
int cam_sequence = std::distance(state_server.cam_states.begin(),
@@ -2137,6 +2123,7 @@ void MsckfVio::pruneLastCamStateBuffer()
void MsckfVio::pruneCamStateBuffer() {
if (state_server.cam_states.size() < max_cam_state_size)
return;
@@ -2228,11 +2215,6 @@ void MsckfVio::pruneCamStateBuffer() {
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 << "passed" << endl;
}
else
{
cout << "rejected" << endl;
}
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);