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

added debug launch file, added state augmentation, added jakobi concat; resulting jakobis do not pass gating test

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-24 19:36:38 +02:00
parent 1ffc4fb37f
commit 821d9d6f71
5 changed files with 215 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/msckf_vio/feature.hpp
View File

@ -496,7 +496,7 @@ bool Feature::initializeAnchor(
//initialize patch Size
//TODO make N size a ros parameter
int N = 3;
int N = 13;
int n = (int)(N-1)/2;
auto anchor = observations.begin();

11
include/msckf_vio/msckf_vio.h
View File

@ -162,6 +162,7 @@ class MsckfVio {
// Measurement update
void stateAugmentation(const double& time);
void PhotometricStateAugmentation(const double& time);
void addFeatureObservations(const CameraMeasurementConstPtr& msg);
// This function is used to compute the measurement Jacobian
// for a single feature observed at a single camera frame.
@ -180,9 +181,9 @@ class MsckfVio {
void PhotometricMeasurementJacobian(
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
Eigen::Matrix<double, 4, 6>& H_x,
Eigen::Matrix<double, 4, 3>& H_f,
Eigen::Vector4d& r);
Eigen::MatrixXd& H_x,
Eigen::MatrixXd& H_y,
Eigen::VectorXd& r);
void PhotometricFeatureJacobian(
const FeatureIDType& feature_id,
@ -207,7 +208,7 @@ class MsckfVio {
StateServer state_server;
// Maximum number of camera states
int max_cam_state_size;
// Features used
MapServer map_server;
@ -224,6 +225,8 @@ class MsckfVio {
CameraCalibration cam0;
CameraCalibration cam1;
// covariance data
double irradiance_frame_bias;
ros::Time image_save_time;