added scaling for images

2019-06-19 18:32:44 +02:00
parent c79fc173b3
commit 02b9e0bc78
7 changed files with 439 additions and 115 deletions
--- a/include/msckf_vio/msckf_vio.h
+++ b/include/msckf_vio/msckf_vio.h
@ -219,6 +219,7 @@ class MsckfVio {
    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,
        const Eigen::VectorXd& r);
    bool gatingTest(const Eigen::MatrixXd& H,
@ -252,6 +253,7 @@ class MsckfVio {

    // State vector
    StateServer state_server;
+    StateServer photometric_state_server;

    // Ground truth state vector 
    StateServer true_state_server;