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

restructured calcualtion of patches in code

Browse Source
This commit is contained in:
Raphael Maenle
2019-07-02 08:32:56 +02:00
parent 58fe991647
commit 737c23f32a
4 changed files with 365 additions and 238 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
include/msckf_vio/msckf_vio.h
View File

@ -202,32 +202,56 @@ class MsckfVio {
Eigen::Vector4d& r);
// This function computes the Jacobian of all measurements viewed
// in the given camera states of this feature.
void featureJacobian(const FeatureIDType& feature_id,
void featureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
void twodotMeasurementJacobian(
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
bool ConstructJacobians(
Eigen::MatrixXd& H_rho,
Eigen::MatrixXd& H_pl,
Eigen::MatrixXd& H_pA,
const Feature& feature,
const StateIDType& cam_state_id,
Eigen::MatrixXd& H_xl,
Eigen::MatrixXd& H_yl);
bool PhotometricPatchPointResidual(
const StateIDType& cam_state_id,
const Feature& feature,
Eigen::VectorXd& r);
bool PhotometricPatchPointJacobian(
const CAMState& cam_state,
const Feature& feature,
Eigen::Vector3d point,
int count,
Eigen::Matrix<double, 1, 1>& H_rhoj,
Eigen::Matrix<double, 1, 6>& H_plj,
Eigen::Matrix<double, 1, 6>& H_pAj);
bool PhotometricMeasurementJacobian(
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
Eigen::MatrixXd& H_x,
Eigen::MatrixXd& H_y,
Eigen::VectorXd& r);
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
Eigen::MatrixXd& H_x,
Eigen::MatrixXd& H_y,
Eigen::VectorXd& r);
void twodotFeatureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
bool PhotometricFeatureJacobian(
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids,
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
void measurementUpdate(const Eigen::MatrixXd& H,
@ -263,6 +287,13 @@ class MsckfVio {
// Chi squared test table.
static std::map<int, double> chi_squared_test_table;
double eval_time;
IMUState timed_old_imu_state;
IMUState timed_old_true_state;
IMUState old_imu_state;
IMUState old_true_state;
// change in position
Eigen::Vector3d delta_position;