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

added full switch

Browse Source
This commit is contained in:
Raphael Maenle 2019-07-19 17:20:10 +02:00
parent ed2ba61828
commit a8090ca58a
4 changed files with 142 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
include/msckf_vio/msckf_vio.h
View File

@ -202,7 +202,7 @@ class MsckfVio {
Eigen::Vector4d& r);
// This function computes the Jacobian of all measurements viewed
// in the given camera states of this feature.
void featureJacobian(
bool featureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
@ -246,7 +246,7 @@ class MsckfVio {
Eigen::VectorXd& r);
void twodotFeatureJacobian(
bool twodotFeatureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
@ -283,6 +283,7 @@ class MsckfVio {
bool nan_flag;
bool play;
double last_time_bound;
double time_offset;
// Patch size for Photometry
int N;

8
launch/image_processor_tinytum.launch
View File

@ -18,7 +18,7 @@
<param name="grid_row" value="4"/>
<param name="grid_col" value="4"/>
<param name="grid_min_feature_num" value="3"/>
<param name="grid_max_feature_num" value="4"/>
<param name="grid_max_feature_num" value="5"/>
<param name="pyramid_levels" value="3"/>
<param name="patch_size" value="15"/>
<param name="fast_threshold" value="10"/>
@ -27,9 +27,9 @@
<param name="ransac_threshold" value="3"/>
<param name="stereo_threshold" value="5"/>
<remap from="~imu" to="/imu02"/>
<remap from="~cam0_image" to="/cam0/image_raw2"/>
<remap from="~cam1_image" to="/cam1/image_raw2"/>
<remap from="~imu" to="/imu0"/>
<remap from="~cam0_image" to="/cam0/image_raw"/>
<remap from="~cam1_image" to="/cam1/image_raw"/>
</node>

5
launch/msckf_vio_tinytum.launch
View File

@ -17,8 +17,7 @@
args='standalone msckf_vio/MsckfVioNodelet'
output="screen">
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
<param name="FILTER" value="1"/>
<param name="FILTER" value="0"/>
<!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/>
@ -33,7 +32,7 @@
<param name="frame_rate" value="20"/>
<param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
<param name="child_frame_id" value="odom"/>
<param name="max_cam_state_size" value="20"/>
<param name="max_cam_state_size" value="12"/>
<param name="position_std_threshold" value="8.0"/>
<param name="rotation_threshold" value="0.2618"/>

236
src/msckf_vio.cpp
View File

@ -50,7 +50,7 @@ Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity();
// Static member variables in Feature class.
FeatureIDType Feature::next_id = 0;
double Feature::observation_noise = 0.05;
double Feature::observation_noise = 0.01;
Feature::OptimizationConfig Feature::optimization_config;
map<int, double> MsckfVio::chi_squared_test_table;
@ -425,6 +425,7 @@ void MsckfVio::imageCallback(
// the origin.
if (is_first_img) {
is_first_img = false;
time_offset = feature_msg->header.stamp.toSec();
state_server.imu_state.time = feature_msg->header.stamp.toSec();
}
static double max_processing_time = 0.0;
@ -786,7 +787,7 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) {
void MsckfVio::calcErrorState()
{
// imu error
// imu "error"
err_state_server.imu_state.id = state_server.imu_state.id;
err_state_server.imu_state.time = state_server.imu_state.time;
@ -832,9 +833,9 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
// Counter how many IMU msgs in the buffer are used.
int used_truth_msg_cntr = 0;
double truth_time;
for (const auto& truth_msg : truth_msg_buffer) {
double truth_time = truth_msg.header.stamp.toSec();
truth_time = truth_msg.header.stamp.toSec();
if (truth_time < true_state_server.imu_state.time) {
++used_truth_msg_cntr;
continue;
@ -854,6 +855,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
// Execute process model.
processTruthtoIMU(truth_time, m_rotation, m_translation);
++used_truth_msg_cntr;
}
last_time_bound = time_bound;
@ -864,6 +866,31 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
truth_msg_buffer.erase(truth_msg_buffer.begin(),
truth_msg_buffer.begin()+used_truth_msg_cntr);
std::ofstream logfile;
int FileHandler;
char FileBuffer[1024];
float load = 0;
FileHandler = open("/proc/loadavg", O_RDONLY);
if( FileHandler >= 0) {
auto file = read(FileHandler, FileBuffer, sizeof(FileBuffer) - 1);
sscanf(FileBuffer, "%f", &load);
close(FileHandler);
}
auto gt = true_state_server.imu_state.position;
auto gr = true_state_server.imu_state.orientation;
logfile.open("/home/raphael/dev/MSCKF_ws/bag/log.txt", std::ios_base::app);
//ros time, cpu load , ground truth, estimation, ros time
logfile << true_state_server.imu_state.time - time_offset << "; " << load << "; ";
logfile << gt[0] << "; " << gt[1] << "; " << gt[2] << "; " << gr[0] << "; " << gr[1] << "; " << gr[2] << "; " << gr[3] << ";";
// estimation
auto et = state_server.imu_state.position;
auto er = state_server.imu_state.orientation;
logfile << et[0] << "; " << et[1] << "; " << et[2] << "; " << er[0] << "; " << er[1] << "; " << er[2] << "; " << er[3] << "; \n" << endl;;
logfile.close();
/*
// calculate change
delta_position = state_server.imu_state.position - old_imu_state.position;
@ -1459,12 +1486,14 @@ void MsckfVio::twodotMeasurementJacobian(
return;
}
void MsckfVio::twodotFeatureJacobian(
bool MsckfVio::twodotFeatureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
MatrixXd& H_x, VectorXd& r)
{
if(FILTER != 2)
return false;
const auto& feature = map_server[feature_id];
@ -1561,7 +1590,7 @@ void MsckfVio::twodotFeatureJacobian(
std::cout << "resume playback" << std::endl;
nh.setParam("/play_bag", true);
}
return;
return true;
}
@ -1891,6 +1920,8 @@ bool MsckfVio::PhotometricFeatureJacobian(
const std::vector<StateIDType>& cam_state_ids,
MatrixXd& H_x, VectorXd& r)
{
if(FILTER != 1)
return false;
const auto& feature = map_server[feature_id];
@ -2101,12 +2132,15 @@ void MsckfVio::measurementJacobian(
return;
}
void MsckfVio::featureJacobian(
bool MsckfVio::featureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
MatrixXd& H_x, VectorXd& r)
{
// stop playing bagfile if printing images
if(FILTER != 0)
return false;
const auto& feature = map_server[feature_id];
@ -2239,7 +2273,7 @@ void MsckfVio::featureJacobian(
myfile.close();
}
return;
return true;
}
@ -2625,37 +2659,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
if(gamma > 1000000)
{
cout << " logging " << endl;
ofstream myfile;
myfile.open("/home/raphael/dev/octave/log2octave");
myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
<< "# name: H\n"
<< "# type: matrix\n"
<< "# rows: " << H.rows() << "\n"
<< "# columns: " << H.cols() << "\n"
<< H << endl;
myfile << "# name: r\n"
<< "# type: matrix\n"
<< "# rows: " << r.rows() << "\n"
<< "# columns: " << 1 << "\n"
<< r << endl;
myfile << "# name: C\n"
<< "# type: matrix\n"
<< "# rows: " << state_server.state_cov.rows() << "\n"
<< "# columns: " << state_server.state_cov.cols() << "\n"
<< state_server.state_cov << endl;
myfile.close();
}
if (chi_squared_test_table[dof] == 0)
return false;
if (gamma < chi_squared_test_table[dof]) {
// cout << "passed" << endl;
if(filter == 1)
cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
return true;
@ -2769,20 +2776,25 @@ void MsckfVio::removeLostFeatures() {
}
}
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_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(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
if(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(twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j))
{
if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
}
}
// Put an upper bound on the row size of measurement Jacobian,
// which helps guarantee the executation time.
@ -2795,16 +2807,22 @@ void MsckfVio::removeLostFeatures() {
pr.conservativeResize(pstack_cntr);
photometricMeasurementUpdate(pH_x, pr);
}
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
if(stack_cntr)
{
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
measurementUpdate(H_x, r);
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
twor.conservativeResize(twostack_cntr);
// Perform the measurement update step.
measurementUpdate(H_x, r);
twoMeasurementUpdate(twoH_x, twor);
}
if(twostack_cntr)
{
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
twor.conservativeResize(twostack_cntr);
twoMeasurementUpdate(twoH_x, twor);
}
// Remove all processed features from the map.
for (const auto& feature_id : processed_feature_ids)
map_server.erase(feature_id);
@ -2951,20 +2969,23 @@ void MsckfVio::pruneLastCamStateBuffer()
}
}
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
if(featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j))
{
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();
pruned_cntr++;
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();
pruned_cntr++;
}
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
}
if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j))
{
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
}
}
for (const auto& cam_id : involved_cam_state_ids)
@ -2978,16 +2999,20 @@ void MsckfVio::pruneLastCamStateBuffer()
photometricMeasurementUpdate(pH_x, pr);
}
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
twor.conservativeResize(twostack_cntr);
// Perform the measurement update step.
measurementUpdate(H_x, r);
twoMeasurementUpdate(twoH_x, twor);
if(stack_cntr)
{
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
measurementUpdate(H_x, r);
}
if(twostack_cntr)
{
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
twor.conservativeResize(twostack_cntr);
twoMeasurementUpdate(twoH_x, twor);
}
//reduction
int cam_sequence = std::distance(state_server.cam_states.begin(),
state_server.cam_states.find(rm_cam_state_id));
@ -3128,21 +3153,22 @@ void MsckfVio::pruneCamStateBuffer() {
pstack_cntr += pH_xj.rows();
}
}
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
if (gatingTest(H_xj, r_j, involved_cam_state_ids.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();
if(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())) {
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(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j))
{
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
}
}
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
@ -3156,16 +3182,20 @@ void MsckfVio::pruneCamStateBuffer() {
photometricMeasurementUpdate(pH_x, pr);
}
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
twor.conservativeResize(twostack_cntr);
// Perform the measurement update step.
measurementUpdate(H_x, r);
twoMeasurementUpdate(twoH_x, twor);
if(stack_cntr)
{
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);
measurementUpdate(H_x, r);
}
if(stack_cntr)
{
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
twor.conservativeResize(twostack_cntr);
twoMeasurementUpdate(twoH_x, twor);
}
//reduction
for (const auto& cam_id : rm_cam_state_ids) {
int cam_sequence = std::distance(state_server.cam_states.begin(),