added some debug output, added qr decomp
This commit is contained in:
parent
dcbc69a1c4
commit
2d045660c2
@ -236,6 +236,7 @@ class MsckfVio {
|
||||
bool PHOTOMETRIC;
|
||||
|
||||
// debug flag
|
||||
bool STREAMPAUSE;
|
||||
bool PRINTIMAGES;
|
||||
bool GROUNDTRUTH;
|
||||
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user