From 2a8fb83dadbb12e836fd644c248b6af03b77d6b8 Mon Sep 17 00:00:00 2001 From: Jeff Delaune Date: Thu, 15 Oct 2020 18:45:29 -0700 Subject: [PATCH] Initial commit --- .gitignore | 9 + CMakeLists.txt | 145 ++++ LICENSE | 13 + include/x/common/eigen_matrix_base_plugin.h | 65 ++ include/x/common/types.h | 63 ++ include/x/ekf/ekf.h | 205 +++++ include/x/ekf/propagator.h | 166 ++++ include/x/ekf/state.h | 351 ++++++++ include/x/ekf/state_buffer.h | 141 ++++ include/x/ekf/updater.h | 127 +++ include/x/vio/msckf_slam_update.h | 130 +++ include/x/vio/msckf_update.h | 108 +++ include/x/vio/range_update.h | 119 +++ include/x/vio/slam_update.h | 124 +++ include/x/vio/solar_update.h | 61 ++ include/x/vio/state_manager.h | 223 +++++ include/x/vio/tools.h | 86 ++ include/x/vio/track_manager.h | 127 +++ include/x/vio/types.h | 274 ++++++ include/x/vio/update.h | 81 ++ include/x/vio/vio.h | 141 ++++ include/x/vio/vio_updater.h | 214 +++++ include/x/vision/camera.h | 67 ++ include/x/vision/feature.h | 133 +++ include/x/vision/tiled_image.h | 80 ++ include/x/vision/timing.h | 120 +++ include/x/vision/tracker.h | 141 ++++ include/x/vision/triangulation.h | 128 +++ include/x/vision/types.h | 80 ++ package.xml | 14 + src/x/ekf/ekf.cpp | 219 +++++ src/x/ekf/propagator.cpp | 872 ++++++++++++++++++++ src/x/ekf/state.cpp | 293 +++++++ src/x/ekf/state_buffer.cpp | 99 +++ src/x/ekf/updater.cpp | 74 ++ src/x/vio/msckf_slam_update.cpp | 272 ++++++ src/x/vio/msckf_update.cpp | 210 +++++ src/x/vio/range_update.cpp | 407 +++++++++ src/x/vio/slam_update.cpp | 251 ++++++ src/x/vio/solar_update.cpp | 94 +++ src/x/vio/state_manager.cpp | 634 ++++++++++++++ src/x/vio/track_manager.cpp | 633 ++++++++++++++ src/x/vio/vio.cpp | 386 +++++++++ src/x/vio/vio_updater.cpp | 360 ++++++++ src/x/vision/camera.cpp | 158 ++++ src/x/vision/feature.cpp | 58 ++ src/x/vision/tiled_image.cpp | 218 +++++ src/x/vision/timing.cpp | 107 +++ src/x/vision/tracker.cpp | 644 +++++++++++++++ src/x/vision/triangulation.cpp | 234 ++++++ 50 files changed, 9959 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 LICENSE create mode 100644 include/x/common/eigen_matrix_base_plugin.h create mode 100644 include/x/common/types.h create mode 100644 include/x/ekf/ekf.h create mode 100644 include/x/ekf/propagator.h create mode 100644 include/x/ekf/state.h create mode 100644 include/x/ekf/state_buffer.h create mode 100644 include/x/ekf/updater.h create mode 100644 include/x/vio/msckf_slam_update.h create mode 100644 include/x/vio/msckf_update.h create mode 100644 include/x/vio/range_update.h create mode 100644 include/x/vio/slam_update.h create mode 100644 include/x/vio/solar_update.h create mode 100644 include/x/vio/state_manager.h create mode 100644 include/x/vio/tools.h create mode 100644 include/x/vio/track_manager.h create mode 100644 include/x/vio/types.h create mode 100644 include/x/vio/update.h create mode 100644 include/x/vio/vio.h create mode 100644 include/x/vio/vio_updater.h create mode 100644 include/x/vision/camera.h create mode 100644 include/x/vision/feature.h create mode 100644 include/x/vision/tiled_image.h create mode 100644 include/x/vision/timing.h create mode 100644 include/x/vision/tracker.h create mode 100644 include/x/vision/triangulation.h create mode 100644 include/x/vision/types.h create mode 100644 package.xml create mode 100644 src/x/ekf/ekf.cpp create mode 100644 src/x/ekf/propagator.cpp create mode 100644 src/x/ekf/state.cpp create mode 100644 src/x/ekf/state_buffer.cpp create mode 100644 src/x/ekf/updater.cpp create mode 100644 src/x/vio/msckf_slam_update.cpp create mode 100644 src/x/vio/msckf_update.cpp create mode 100644 src/x/vio/range_update.cpp create mode 100644 src/x/vio/slam_update.cpp create mode 100644 src/x/vio/solar_update.cpp create mode 100644 src/x/vio/state_manager.cpp create mode 100644 src/x/vio/track_manager.cpp create mode 100644 src/x/vio/vio.cpp create mode 100644 src/x/vio/vio_updater.cpp create mode 100644 src/x/vision/camera.cpp create mode 100644 src/x/vision/feature.cpp create mode 100644 src/x/vision/tiled_image.cpp create mode 100644 src/x/vision/timing.cpp create mode 100644 src/x/vision/tracker.cpp create mode 100644 src/x/vision/triangulation.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..70bb441 --- /dev/null +++ b/.gitignore @@ -0,0 +1,9 @@ +# Temporary/backup files +*~ +*.sw* + +# Compile flag database for YouCompleteMe +*.json + +# Formatting +.clang-format diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..4661507 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,145 @@ +################################################################################# +# User build settings + +set(DUAL_THREAD true) # Set true to process image and inertial data on different + # threads +set(VERBOSE true) # Set false to disable all publishing and standard output + # stream, except pose at update rate. That will improve runtime. +set(TIMING false) # Set true to enable timers +set(PROFILING false) # Set true to disable compiler flags which are not + # compatible with Callgrind profiling tool. +set(UNIT_TESTS false) # Set true to enable unit tests + +################################################################################# + +cmake_minimum_required(VERSION 2.8.3) +project(x) +set (CMAKE_BUILD_TYPE Release) + +# Set definitions +if(DUAL_THREAD) + add_definitions(-DDUAL_THREAD) +endif() +if(VERBOSE) + add_definitions(-DVERBOSE) +endif() +if(TIMING) + add_definitions(-DTIMING) +endif() +if(UNIT_TESTS) + add_definitions(-DRUN_UNIT_TESTS) +endif() +if (CMAKE_BUILD_TYPE MATCHES Debug) + add_definitions(-DDEBUG -DDEBUGMSF) +elseif (CMAKE_BUILD_TYPE MATCHES RelWithDebInfo) + # Enable asserts + add_definitions(-UNDEBUG) +endif() +add_definitions(-D_LINUX -D_REENTRANT) + +# Eigen plugin +add_definitions(-DEIGEN_MATRIXBASE_PLUGIN=) + +# OpenCV: catkin package or system install +# first try locate the opencv3_catkin ros package, +# else try OpenCV 3 and 4 system installs. +find_package(opencv3_catkin QUIET) +find_package(OpenCV 3 QUIET) +find_package(OpenCV 4 QUIET) + +if(opencv3_catkin_FOUND) + message("OpenCV3_catkin detected") + set(OPENCV_PACKAGE "opencv3_catkin") + set(OpenCV_INCLUDE_DIRS "") + set(OpenCV_LIBRARIES "") +elseif(${OpenCV_FOUND}) + message("OpenCV system install detected") + set(OPENCV_PACKAGE "") +else() + message(FATAL_ERROR "No OpenCV 3 or 4 detected.") +endif() + +find_package(catkin REQUIRED COMPONENTS + ${OPENCV_PACKAGE} + cmake_modules +) + +find_package(Eigen3 REQUIRED) + +# Set build flags, depending on the architecture +set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall") + +if (CMAKE_BUILD_TYPE MATCHES Debug) + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0") +endif() + +if (CMAKE_BUILD_TYPE MATCHES Release) + + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") + + if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") # tested on Jetson TX2 + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv8-a+crypto -mcpu=cortex-a57+crypto -flto -ffast-math -fvect-cost-model=unlimited") + #elseif(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch32") # uncomment with correct check for Snapdragon Flight Pro + # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv7-a -mfpu=neon-vfpv4 -mfloat-abi=softfp -flto -ffast-math -fvect-cost-model=unlimited") + endif() + + if (${PROFILING} MATCHES false) + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -funsafe-loop-optimizations -fsee -funroll-loops -fno-math-errno -funsafe-math-optimizations -ffinite-math-only -fno-signed-zeros") + endif() + +endif() + +# For downstream packages in catkin +set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +set(EIGEN3_LIBRARIES ${EIGEN3_LIBRARIES}) + +# Configure this package +catkin_package( + DEPENDS EIGEN3 + INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIR} + LIBRARIES x +) + +## Package internal and additional locations of header files +## Separating the projects include directory from {catkin_INCLUDE_DIRS} +## allows to tag that with SYSTEM, which disables GCC warnings for +## these (all the ros header, opencv, ..) +include_directories (include) + +include_directories (SYSTEM + ${OpenCV_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +set (SOURCE + src/x/ekf/ekf.cpp + src/x/ekf/propagator.cpp + src/x/ekf/state.cpp + src/x/ekf/state_buffer.cpp + src/x/ekf/updater.cpp + src/x/vio/vio.cpp + src/x/vio/vio_updater.cpp + src/x/vio/state_manager.cpp + src/x/vio/track_manager.cpp + src/x/vio/msckf_update.cpp + src/x/vio/msckf_slam_update.cpp + src/x/vio/slam_update.cpp + src/x/vio/range_update.cpp + src/x/vio/solar_update.cpp + src/x/vision/camera.cpp + src/x/vision/feature.cpp + src/x/vision/tiled_image.cpp + src/x/vision/timing.cpp + src/x/vision/tracker.cpp + src/x/vision/triangulation.cpp + +) + +add_library (x ${SOURCE}) + +# Additional libraries to link against +target_link_libraries(x + ${OpenCV_LIBRARIES} + ${catkin_LIBRARIES} +) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..f51261c --- /dev/null +++ b/LICENSE @@ -0,0 +1,13 @@ +Copyright 2020 California Institute of Technology (“Caltech”) + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/include/x/common/eigen_matrix_base_plugin.h b/include/x/common/eigen_matrix_base_plugin.h new file mode 100644 index 0000000..a4c2ff9 --- /dev/null +++ b/include/x/common/eigen_matrix_base_plugin.h @@ -0,0 +1,65 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_COMMON_EIGEN_MATRIX_BASE_PLUGIN_H_ +#define X_COMMON_EIGEN_MATRIX_BASE_PLUGIN_H_ + +/******************************************************************************* + * This header defines custom extensions of the Eigen MatrixBase class for xEKF. + ******************************************************************************/ + +/** + * Converts a 3-vector to the associated 3x3 cross-product matrix. + * + * The matrix must be a vector of size 3. The cross-product matrix is a 3x3 + * skew-symmetric matrix. + * + * @return The 3x3 cross-product matrix + */ +inline Matrix< Scalar, 3, 3 > +toCrossMatrix() const { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); + Matrix< Scalar, 3, 3 > mat; + mat << 0.0, -derived().z(), derived().y(), + derived().z(), 0.0, -derived().x(), + -derived().y(), derived().x(), 0.0; + return mat; +} + +/** + * Converts a 3-vector to the associated 4x4 quaternion differentiation matrix. + * + * The matrix must be a vector of size 3 and represent angular rate. + * + * This is the transform from Eq. (108) in "Indirect Kalman Filter for 3D + * Attitude Estimation" by Nik Trawny and Stergios Roumeliotis (Univ. of + * Minnesota). This is equivalent to Eq. (2.17) of xVIO tech report, + * adapted to the (x,y,z,w) quaternion coefficient order from Eigen. + * + * @return The 4x4 quaternion differentiation matrix. + */ +inline Matrix< Scalar, 4, 4 > +toOmegaMatrix() const { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); + Matrix< Scalar, 4, 4 > mat; + mat << 0.0, derived().z(), -derived().y(), derived().x(), + -derived().z(), 0.0, derived().x(), derived().y(), + derived().y(), -derived().x(), 0.0, derived().z(), + -derived().x(), -derived().y(), -derived().z(), 0.0; + return mat; +} + +#endif // X_COMMON_EIGEN_MATRIX_BASE_PLUGIN_H_ diff --git a/include/x/common/types.h b/include/x/common/types.h new file mode 100644 index 0000000..3db2d60 --- /dev/null +++ b/include/x/common/types.h @@ -0,0 +1,63 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_COMMON_TYPES_H +#define X_COMMON_TYPES_H + +#include + +namespace x { + + using Vector3 = Eigen::Vector3d; + using Quaternion = Eigen::Quaterniond; + using Matrix = Eigen::MatrixXd; + + /** + * A structure to pass IMU noise parameters. + * + * Default values: ADXRS610 gyros, MXR9500G/M accels (Astec). + */ + struct ImuNoise { + /** + * Gyro noise spectral density [rad/s/sqrt(Hz)] + */ + double n_w = 0.0083; + + /** + * Gyro bias random walk [rad/s^2/sqrt(Hz)] + */ + double n_bw = 0.00083; + + /** + * Accel noise spectral density [m/s^2/sqrt(Hz)] + */ + double n_a = 0.0013; + + /** + * Accel bias random walk [m/s^3/sqrt(Hz)] + */ + double n_ba = 00013; + }; + + /** + * Denotes an invalid object through a -1 timestamp. + */ + constexpr double kInvalid = -1.0; + + +} // namespace x + +#endif // #ifndef X_COMMON_TYPES_H diff --git a/include/x/ekf/ekf.h b/include/x/ekf/ekf.h new file mode 100644 index 0000000..6c21d95 --- /dev/null +++ b/include/x/ekf/ekf.h @@ -0,0 +1,205 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_EKF_EKF_H_ +#define X_EKF_EKF_H_ + +#include +#include +#include +#include + +namespace x { + /** + * Filter initialization status. + */ + enum InitStatus { + /** + * Filter is not initialized and doesn't process IMU messages. + */ + kNotInitialized, + /** + * Filter received the init command and is standing by for the first IMU + * measurement. + */ + kStandBy, + /** + * Filter is initialized and processes IMU and update measurements. + */ + kInitialized, + }; + + /** + * A Extended Kalman filter class. + * + * Carries propagation and update of the states contained in a buffer. + */ + class Ekf + { + public: + /** + * Minimum constructor. + */ + Ekf(Updater& updater); + + /** + * Copy constructor + */ + Ekf(const Ekf& ekf); + + /** + * Sets all EKF parameters before filter initialization. + * + * @param[in] updater Updater object, specific to measurement type. + * @param[in] g Gravity vector, expressed in world frame. + * @param[in] imu_noise IMU noise parameters (continuous time). + * @param[in] state_buffer_sz Size to allocate for the state buffer. + * @param[in] default_state Default state instance to initialize buffer + * elements at. + * @param[in] a_m_max Max specific force norm threshold. + * @param[in] delta_seq_imu Expected difference between successive IMU + * sequence IDs. + * @param[in] time_margin_bfr Time margin, in seconds, around the buffer + * time range. + */ + void set(const Updater& updater, + Vector3& g, + const ImuNoise& imu_noise, + const size_t state_buffer_sz, + const State& default_state, + const double a_m_max, + const unsigned int delta_seq_imu, + const double time_margin_bfr); + + /** + * Get a reference to the updater. + */ + Updater& getUpdaterRef(); + + /** + * (Re-)Initialize the filter from input state. + * + * @param[in] init_state State to initialize from. This state is required + * have the same dynamic state size that have been + * allocated by the set function in the buffer. + */ + void initializeFromState(const State& init_state); + + /** + * Process IMU measurements. + * + * Propagates state and covariance in the buffer. + * + * @param[in] timestamp + * @param[in] seq IMU sequence ID. + * @param[in] w_m Angular velocity (gyroscope). + * @param[in] a_m Specific force (accelerometer). + * @return The propagated state. + */ + State processImu(const double timestamp, + const unsigned int seq, + const Vector3& w_m, + const Vector3& a_m); + + /** + * Process update measurement. + * + * Retrieve the closest state prior to the measurement and call updater's + * EKF update function. + */ + State processUpdateMeasurement(); + + /** + * Locks mutex (thread safety). + */ + void lock(); + + /** + * Unlock mutex (thread safety). + */ + void unlock(); + + private: + /** + * State and covariance propagator. + */ + Propagator propagator_; + + /** + * A reference to an EKF updater concrete class. + * + * Constructs and applies an EKF from a measurement. Updater is the + * abstract class. The reference should point to a concrete updater + * that is specific to a measurement type (e.g. VioUpdater) + */ + Updater& updater_; + + /** + * Time-sorted ring state buffer. + * + * Each state corresponds to an IMU message. + */ + StateBuffer state_buffer_; + + /** + * Initialization status. + */ + InitStatus init_status_ { kNotInitialized }; + + /** + * Max specific force norm threshold. + * + * Used to detect accelerometer spikes. Default value 50 m/s^2. + */ + double a_m_max_ { 50.0 }; + + /** + * Expected difference between successive IMU sequence IDs. + * + * Used to detects missing IMU messages. Default value 1. + */ + unsigned int delta_seq_imu_ { 1 }; + + /** + * Thread safety. + */ + boost::mutex mutex_; + + /** + * Repropagate state buffer from input state. + * + * Repropagates state estimates and covariance from the input state copied + * at input index, until last state in buffer. Returns false if the state + * at that index in buffer doesn't have the same timestamp, i.e. if the + * buffer was overwritten while the update was being computed (thread + * safe). + * + * @param[in] state State to repropagate from. + * @param[in] idx Index this state should copied at. + * @return True if the repropagation was successful + */ + bool repropagateFromStateAtIdx(const State& state, const int idx); + }; + + /** + * Bad argument exception type when the size of the dynamic arrays (poses, + * features, covariance matrix) in the initialization state doesn't match the + * size allocated in the buffer. + */ + class init_bfr_mismatch {}; +} // namespace x + +#endif // X_EKF_EKF_H_ diff --git a/include/x/ekf/propagator.h b/include/x/ekf/propagator.h new file mode 100644 index 0000000..f081af6 --- /dev/null +++ b/include/x/ekf/propagator.h @@ -0,0 +1,166 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_EKF_PROPAGATOR_H_ +#define X_EKF_PROPAGATOR_H_ + +#include + +namespace x { + using Matrix4 = Eigen::Matrix4d; + + /** + * Fixed-size core error state covariance matrix + */ + using CoreCovMatrix = Eigen::Matrix; + + /** + * A class to carry inertial propagation of state and covariance . + */ + class Propagator + { + public: + /** + * Default constructor. + */ + Propagator() {}; + + /** + * A constructor. + * + * @param[in] g Gravity vector, expressed in world frame. + * @param[in] imu_noise IMU noise parameters (continuous time). + */ + Propagator(const Vector3& g, + const ImuNoise& imu_noise); + + /** + * Sets all member variables. + * + * @param[in] g Gravity vector, expressed in world frame. + * @param[in] imu_noise IMU noise parameters (continuous time). + */ + void set(const Vector3& g, + const ImuNoise& imu_noise); + + /** + * Propagates state estimates between two time instants. + * + * @param[in] state_0 Start state. + * @param[in,out] state_1 End state. + */ + void propagateState(const State& state_0, State& state_1); + + /** + * Propagates covariance estimates between two time instants. + * + * Implementation of covariance propagation described in Section III.C + * of "Real-Time Metric State Estimation for Modular Vision-Inertial + * Systems" by Stephan Weiss and Roland Siegwart (IROS, 2011). + * + * @param[in] state_0 Start state. + * @param[in,out] state_1 End state. + */ + void propagateCovariance(const State& state_0, State& state_1); + + private: + /** + * Gravity vector, expressed in world frame. + */ + Vector3 g_ { Vector3(0.0, 0.0, -9.81) }; + + /** + * IMU noise parameters (continuous time). + */ + ImuNoise imu_noise_; + + /** + * First order quaternion integrator. + * + * Implementation of section 1.6.2 of "Indirect Kalman Filter for 3D + * Attitude Estimation" by Nik Trawny and Stergios Roumeliotis (Univ. of + * Minnesota) + * + * @param[in] e_w_0 Angular velocity at time t0 + * @param[in] e_w_1 Angular velocity at time t1 + * @param[in] dt t1-t0 + * @return Matrix transforming quaternion at time t0 into quaternion at + * time t1. + */ + Matrix4 quaternionIntegrator(const Vector3& e_w_0, + const Vector3& e_w_1, + const double dt) const; + + /** + * Computes the discrete state transition matrix. + * + * @param[in] dt Time difference of the propagation t1-t0. + * @param[in] e_w Bias-free gyro measurement at t1. + * @param[in] e_a Bias-free accel measurement at t1. + * @param[in] q Orientation quaternion at t1. + * @return The kSizeCoreErr x kSizeCoreErr discrete state transition + * matrix + */ + CoreCovMatrix discreteStateTransition(const double dt, + const Vector3& e_w, + const Vector3& e_a, + const Quaternion& q) const; + + /** + * Propagate error state covariance matrix given discrete state transition + * and process noise covariance matrices. + * + * @param[in] cov_0 Covariance matrix at time t0. + * @param[in] f_d Discrete state transition matrix between t0 and t1. + * @param[in] q_d Discrete process noise covariance matrix. + * @param[out] cov_1 Covariance matrix at time t1. + */ + void propagateCovarianceMatrices(const Eigen::MatrixXd& cov_0, + const CoreCovMatrix& f_d, + const CoreCovMatrix& q_d, + Eigen::MatrixXd& cov_1); + + /** + * Computes the discrete process noise covariance matrix. + * + * The implementation of the function was adapted from MSF, which itself + * was auto-generated by MATLAB symbolic toolbox based on Eq. (21) of + * "Real-Time Metric State Estimation for Modular Vision-Inertial Systems" + * by Stephan Weiss and Roland Siegwart (IROS, 2011). + * + * @param[in] dt Time difference of the propagation t1-t0. + * @param[in] q Orientation quaternion at t1. + * @param[in] e_w Bias-free gyro measurement at t1. + * @param[in] e_a Bias-free accel measurement at t1. + * @param[in] n_w Gyro noise spectral density. + * @param[in] n_bw Gyro bias random walk. + * @param[in] n_a Accel noise spectral density. + * @param[in] n_ba Accel bias random walk. + * @return The kSizeCoreErr x kSizeCoreErr discrete process noise + * covariance matrix + */ + CoreCovMatrix discreteProcessNoiseCov(const double dt, + const Quaternion& q, + const Vector3& e_w, + const Vector3& e_a, + const double n_w, + const double n_bw, + const double n_a, + const double n_ba) const; + }; +} // namespace x + +#endif // X_EKF_PROPAGATOR_H_ diff --git a/include/x/ekf/state.h b/include/x/ekf/state.h new file mode 100644 index 0000000..a1edd24 --- /dev/null +++ b/include/x/ekf/state.h @@ -0,0 +1,351 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_EKF_STATE_H_ +#define X_EKF_STATE_H_ + +#include +#include + +namespace x +{ + /** + * Start indices and total size of the core (inertial) error states. + * + * Refer to the State class for the definition of each state. + */ + enum { + kIdxP = 0, + kIdxV = 3, + kIdxQ = 6, + kIdxBw = 9, + kIdxBa = 12, + kSizeCoreErr = 21 + }; + + /** + * The xEKF inertial state class. + * + * This class stores the inertial, or core, states propagated by xEKF, the + * error covariance matrix, and the IMU measurements at a given time. This is + * an abstract class. Vision states are defined as optional additional dynamic + * states. The errors states are in the following order in the covariance + * matrix: p, v, q, b_w, b_a, p_array, q_array, f_array. + * {W} world frame + * {I} IMU frame + */ + class State + { + public: + /////////////////////////////// CONSTRUCTORS ///////////////////////////// + /** + * Default constructor (zero state). + * + * Zero states. The dynamic vision state arrays p_array_, q_array_ and + * f_arrays_ are size zero. + */ + State() {}; + + /** + * Zero-state constructor, allocating vision states. + * + * Zero states. This constructor allocates the input size for the vision + * state arrays p_array_, q_array_ and f_arrays_. + * + * @param[in] n_poses Number of poses in the sliding window (i.e. number + * of positions in p_array_ / quaternions in q_array_). + * @param[in] n_features Number of features in f_array_. + */ + State(const size_t n_poses, const size_t n_features); + + /** + * Full state constructor. + * + * This constructor sets all the state vector and covariance matrix + * entries, i.e. all the class member variables. Refer to the description + * of the member for a description of the inputs. + */ + State(const double time, + const unsigned int seq, + const Vector3& p, + const Vector3& v, + const Quaternion& q, + const Vector3& b_w, + const Vector3& b_a, + const Matrix& p_array, + const Matrix& q_array, + const Matrix& f_array, + const Matrix& cov, + const Quaternion& q_ic, + const Vector3& p_ic, + const Vector3& w_m, + const Vector3& a_m); + + //////////////////////////// GETTERS & SETTERS /////////////////////////// + + double getTime() const; + + unsigned int getSeq() const; + + Vector3 getPosition() const; + + Quaternion getOrientation() const; + + Matrix getPositionArray() const; + + Matrix getOrientationArray() const; + + Matrix getFeatureArray() const; + + Quaternion getOrientationExtrinsics() const; + + Vector3 getPositionExtrinsics() const; + + Matrix getCovariance() const; + + /** + * Assembles and returns 6x6 covariance of the pose error (dp,dq). + * + * @return The pose covariance matrix. + */ + Matrix getPoseCovariance() const; + + Matrix& getCovarianceRef(); + + void setTime(const double time); + + void setPositionArray(const Matrix& p_array); + + void setOrientationArray(const Matrix& q_array); + + void setFeatureArray(const Matrix& f_array); + + void setCovariance(const Matrix& cov); + + /** + * Sets the IMU members of the state. + * + * This is used before propagating that state when a new IMU message came + * in. + * + * @param[in] time IMU timestamp. + * @param[in] seq IMU sequence ID. + * @param[in] w_m Gyroscopes' angular rate measurements. + * @param[in] a_m Accelerometers' specific force measurements. + */ + void setImu(const double time, + const unsigned int seq, + const Vector3& w_m, + const Vector3& a_m); + + /** + * Set static members from the input state. + * + * "Static" state members are those for which the estimate does not vary + * at IMU rate, e.g. inertial biases, or vision states. + */ + void setStaticStatesFrom(const State& state); + + ////////////////////////////////////////////////////////////////////////// + /** + * Reset state. + * + * Flags the state as invalid. Its members will have to be set again for + * use in the EKF. + */ + void reset(); + + /** + * Compute the maximum number of poses allowed in the sliding window. + */ + int nPosesMax() const; + + /** + * Compute the maximum number of features allowed in the state. + * + */ + int nFeaturesMax() const; + + /** + * Compute number of error states corresponding to the current state object. + * + * @return The number of errors states. + */ + int nErrorStates() const; + + /** + * Compute unbiased IMU measurements. + * + * Substracts biases to actual measurements. + * + * @param[out] e_w Unbiased gyros measurement. + * @param[out] e_a Unbiased accel measurement. + */ + void computeUnbiasedImuMeasurements(Vector3& e_w, Vector3& e_a) const; + + /** + * Compute camera orientation from that of the IMU and extrinsics. + * + * @return A unit quaternion modeling the rotation changing the axes of + * {W} into {C}. + */ + Attitude computeCameraAttitude() const; + + /** + * Compute camera position from that of the IMU state and extrinsics. + * + * @return A 3-vector for the camera position in world frame. + */ + Vector3 computeCameraPosition() const; + + /** + * Compute camera orientation from that of the IMU and extrinsics. + * + * @return A unit quaternion modeling the rotation changing the axes of + * {W} into {C}. + */ + Quaternion computeCameraOrientation() const; + + /** + * Correct state based on input. + * + * @param[in] correction State correction vector. + */ + void correct(const Eigen::VectorXd& correction); + + /** + * Create a string that prints all state information. + */ + std::string toString() const; + + private: + /** + * Timestamp + * + * Also acts as a flag that the state is valid and ready fo use in the + * EKF. + */ + double time_ { kInvalid }; + + /** + * Sequence ID. + * + * This sequence ID is used to identify the sensor measurement (e.g. IMU) + * that corresponds to the current state. It should match the data's + * sequence ID. + */ + unsigned int seq_ { 0 }; + + /** + * Position of {I} wrt {W}, expressed in the axes of {W}. + */ + Vector3 p_ { Vector3::Zero() }; + + /** + * Velocity of {I} wrt {W}, expressed in the axes of {W}. + */ + Vector3 v_ { Vector3::Zero() }; + + /** + * Unit quaternion modeling the rotation changing the axes of {W} into + * {I}. + */ + Quaternion q_ { Quaternion(1.0, 0.0, 0.0, 0.0) }; + + /** + * Gyroscope bias. + */ + Vector3 b_w_ { Vector3::Zero() }; + + /** + * Accelerometer bias. + */ + Vector3 b_a_ { Vector3::Zero() }; + + /** + * Array of positions of {C} wrt {W}, expressed in the axes of {W}. + * + * These static position states are used for MSCKF updates. + */ + //Vector3Array p_array_; + Matrix p_array_; + + /** + * Array of quaternions modeling the rotation changing the axes of {W} + * into {C}. + * + * These static orientation states are used for MSCKF update. + */ + //QuaternionArray q_array_; + Matrix q_array_; + + /** + * Array of 3D feature states. + * + * These feature states are expressed in inverse-depth coordinates and + * used for SLAM. + */ + //Vector3Array f_array_; + Matrix f_array_; + + /** + * Error covariance matrix. + */ + Matrix cov_; + + ////////////////////////// DEPRECATED STATES ///////////////////////////// + // TODO: Remove these states and modify measurement jacobians after xEKF + // is implemented + + /** + * Unit quaternion modeling the rotation changing the axes of {I} into + * {C}. + */ + Quaternion q_ic_ { Quaternion(1.0, 0.0, 0.0, 0.0) }; + + /** + * Position of {C} wrt {I}, expressed in the axes of {I}. + */ + Vector3 p_ic_ { Vector3::Zero() }; + + /////////////////////////// IMU MEASUREMENT ///////////////////////////// + + /** + * Angular rate measurement from gyroscopes [rad/s] + */ + Vector3 w_m_ { Vector3::Zero() }; + + /** + * Specific force [m/s^2] + */ + Vector3 a_m_ { Vector3::Zero() }; + + ////////////////////////////////////////////////////////////////////////// + + /** + * Computes the error quaternion associated to a set of 3 small angles. + * + * @param[in] delta_theta 3-vector of small angles (x,y,z). + * @return The error quaternion. + */ + Quaternion errorQuatFromSmallAngles(const Vector3& delta_theta) const; + + friend class Propagator; + friend class Ekf; + }; +} // namespace x + +#endif // X_EKF_STATE_H_ diff --git a/include/x/ekf/state_buffer.h b/include/x/ekf/state_buffer.h new file mode 100644 index 0000000..13ccea6 --- /dev/null +++ b/include/x/ekf/state_buffer.h @@ -0,0 +1,141 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_EKF_STATE_BUFFER_H_ +#define X_EKF_STATE_BUFFER_H_ + +#include + +namespace x +{ + constexpr int kInvalidIndex = -1; + + /** + * State buffer class. + * + * This class is a simple ring buffer of States, implemented through a + * std::vector. + */ + class StateBuffer : public std::vector + { + public: + /** + * Constructs a buffer of a given size, allocating states for given + * pose sliding window size and feature size. + * + * @param[in] size Buffer size (default: 250 states). + * @param[in] default_state Default state instance to initialize buffer + * elements at. + * @param[in] time_margin Time margin, in seconds, around the buffer time + * range. + */ + StateBuffer(const size_t size = 250, + const State& default_state = State(), + const double time_margin = 0.005) + : std::vector(size, default_state) + , time_margin_ { time_margin } + {} + + /** + * Get index the most recent IMU state in the buffer (tail). + */ + int getTailIdx() const { return tail_idx_; }; + + /** + * Get a reference to most recent state in buffer (tail). + * + * This is the state at index tail__idx_. + * + * @return A reference to the tail state. + */ + State& getTailStateRef(); + + /** + * Get the index of the closest state to input timestamp. + * + * Returns the index of the state in buffer which is the closest to the + * input timestamp, or invalid index if the requested is outside the + * buffer time range. + * + * @param[in] timestamp Requested time. + * @return The index of the state closest to requested time. + */ + int closestIdx(const double timestamp) const; + + /** + * Get index coming after the input index in the ring buffer. + * + * @param[in] idx Input index. + * @return The next index in buffer. + */ + int nextIdx(const int idx) const; + + /** + * Get index coming before the input index in the ring buffer. + * + * @param[in] idx Input index. + * @return The previous index in buffer. + */ + int previousIdx(const int idx) const; + + /* + * Enqueue new state in the buffer, to be filled in-place by the caller. + * + * Returns a reference to the next state in buffer, which is set as new + * tail and has to be filled the caller. + * + * @return A reference to the new tail state. + */ + State& enqueueInPlace(); + + /** + * Reset buffer from input state. + * + * Resets all states in buffer, then copies input state at the start of + * the buffer (new tail and head). + * + * @param[in] init_state State to reinitialize from. + */ + void resetFromState(const State& init_state); + + private: + /** + * Index of the most recent IMU state in the buffer (tail). + */ + int tail_idx_ { kInvalidIndex }; + + /** + * Index of the oldest IMU state in the buffer (head). + */ + int head_idx_ { kInvalidIndex }; + + /** + * Number of valid states in buffer. + */ + size_t n_valid_states_ { 0 }; + + /** + * Time margin, in seconds, around the buffer time range. + * + * This sets the tolerance for how far in the future/past the closest + * state request can be without returning an invalid state. + */ + double time_margin_ { 0.005 } ; + + }; +} // namespace x + +#endif // X_EKF_STATE_BUFFER_H_ diff --git a/include/x/ekf/updater.h b/include/x/ekf/updater.h new file mode 100644 index 0000000..1172e43 --- /dev/null +++ b/include/x/ekf/updater.h @@ -0,0 +1,127 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_EKF_UPDATER_H_ +#define X_EKF_UPDATER_H_ + +#include + +namespace x +{ + /** + * The xEKF updater abstract class. + * + * An abstract class that implements the (iterated) EKF update and defines + * the pure virtual methods for pre-update work, update matrices construction + * and post-update work. These methods are to be implemented by a derived + * concrete class which is application-specific (e.g VIO or GPS update). + */ + class Updater + { + public: + /** + * A pure virtual method: to get the measurement timestamp. + */ + virtual double getTime() const = 0; + + /** + * Updates input state with current measurement member. + * + * This is the most important function. xEKF calls this function to update + * the input state based on the current measurement. + * + * @param[in] state Update state + */ + void update(State& state); + + protected: + /** + * Number of IEKF iterations. + * + * The filter simplifies to EKF when equals to 1. + */ + int iekf_iter_ { 1 }; + + /** + * Compute Kalman update and apply to state. + * + * @param[in,out] state Update state + * @param[in] H Measurement Jacobian + * @param[in] res Measurement residual + * @param[in] R Measurement covariance matrix + * @param[in,out] correction Correction vector (total correction for IEKF) + * @param[in] cov_update False if the covariance should not be updated (only + * (for IEKF iterations, default = true) + */ + void applyUpdate(State& state, + const Eigen::MatrixXd& H, + const Eigen::MatrixXd& res, + const Eigen::MatrixXd& R, + Matrix& correction_total, + const bool cov_update = true); + + /** + * A pure virtual method for measurement processing. + * + * Only happens once, and is stored if measurement later re-applied. Here this + * where image processing takes place. + * + * @param[in] state State prior. + */ + virtual void preProcess(const State& state) = 0; + + /** + * A pure virtual method for pre-update work + * + * Stuff that needs happen before the Kalman update. This function will NOT + * be called at each IEKF iteration. Here, this corresponds to state + * management. + * + * @param[in,out] state Update state + * @return True if an update should be constructed + */ + virtual bool preUpdate(State& state) = 0; + + /** + * A pure virtual method for update construction. + * + * Prepares measurement Jacobians, residual and noice matrices and applies + * (iterated) EKF update. This function WILL be called at each IEKF iteration. + * + * @param[in] state Update state + * @param[out] h Measurement Jacobian matrix + * @param[out] res Measurement residual vector + * @param[out] r Measurement noise covariance matrix + */ + virtual void constructUpdate(const State& state, + Matrix& h, + Matrix& res, + Matrix& r) = 0; + + /** + * A pure virtual method for post-update work. + * + * Stuff that need to happen after the Kalman update. Here this SLAM feature + * initialization. + * + * @param[in,out] state Update state. + * @param[in] correction Kalman state correction. + */ + virtual void postUpdate(State& state, const Matrix& correction) = 0; + }; +} // namespace x + +#endif // X_EKF_UPDATER_H_ diff --git a/include/x/vio/msckf_slam_update.h b/include/x/vio/msckf_slam_update.h new file mode 100644 index 0000000..ccb1dd8 --- /dev/null +++ b/include/x/vio/msckf_slam_update.h @@ -0,0 +1,130 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_VIO_MSCKF_SLAM_UPDATE_H_ +#define X_VIO_MSCKF_SLAM_UPDATE_H_ + +#include +#include +#include + +namespace x +{ + /** + * MSCKF-SLAM update and feature state initialization. + * + * Implementation of the Multi-State Contraint Kalman Filter update and SLAM + * features initialization in the state vector and error state covariance + * matrix. The major difference wrt the standard MSCKF update is that feature + * is expressed in inverse-depth coordinates, as opposed to cartesian, which + * affects the nullspace projection. As presented Li 2012 ICRA paper and xVIO + * tech report. + */ + class MsckfSlamUpdate : public Update + { + public: + /** + * Default constructor + */ + MsckfSlamUpdate() {}; + + /** + * Constructor. + * + * Does the full update matrix construction job. + * + * @param[in] tkrs Feature tracks in normalized coordinates + * @param[in] quats Camera quaternion states + * @param[in] poss Camera position states + * @param[in] triangulator Feature triangulator + * @param[in] cov_s Error state covariance + * @param[in] n_poses_max Maximum number of poses in sliding window. + * @param[in] sigma_img Standard deviation of feature measurement + * [in normalized coordinates] + */ + MsckfSlamUpdate(const x::TrackList& trks, + const x::AttitudeList& quats, + const x::TranslationList& poss, + const Triangulation& triangulator, + const Eigen::MatrixXd& cov_s, + const int n_poses_max, + const double sigma_img); + + /////////////////////////////// Getters ////////////////////////////////// + + /** + * Returns a constant reference to the MSCKF-SLAM initialization matrices. + */ + const x::MsckfSlamMatrices& getInitMats() const { return init_mats_; }; + + /** + * Returns a constant reference to the inliers 3D cartesian coodinates. + */ + const Vector3dArray& getInliers() const { return inliers_; }; + + /** + * Returns a constant reference to the outliers 3D cartesian coodinates. + */ + const Vector3dArray& getOutliers() const { return outliers_; }; + + private: + + /** + * MSCKF-SLAM initialization matrices + */ + x::MsckfSlamMatrices init_mats_; + + /** + * 3D cartesian coordinates prior for MSCKF feature inliers + */ + Vector3dArray inliers_; + + /** + * 3D cartesian coordinates prior for MSCKF feature outliers + */ + Vector3dArray outliers_; + + /** + * Process one feature track. + * + * @param[in] track Feature track in normalized coordinates + * @param[in] C_q_G Camera attitude state list + * @param[in] G_p_C Camera position state list + * @param[in] triangulator Feature triangulator + * @param[in] P State covariance + * @param[in] n_poses_max Maximum number of poses in sliding window. + * @param[in] var_img Variance of feature measurements + * [in normalized coordinates] + * @param[in] j Index of current track in the MSCKF track list + * @param[out] row_h Current rows in stacked Jacobian + * @param[out] row1 Current rows in stacked MSCKF-SLAM matrices + */ + void processOneTrack(const x::Track& track, + const x::AttitudeList& C_q_G, + const x::TranslationList& G_p_C, + const Triangulation& triangulator, + const Eigen::MatrixXd& P, + const int n_poses_max, + const double var_img, + const size_t& j, + size_t& row_h, + size_t& row1); + + + }; +} + +#endif // X_VIO_MSCKF__SLAM_UPDATE_H_ diff --git a/include/x/vio/msckf_update.h b/include/x/vio/msckf_update.h new file mode 100644 index 0000000..b2cd0dd --- /dev/null +++ b/include/x/vio/msckf_update.h @@ -0,0 +1,108 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_VIO_MSCKF_UPDATE_H_ +#define X_VIO_MSCKF_UPDATE_H_ + +#include +#include +#include + +namespace x +{ + /** + * MSCKF update. + * + * Implementation of the Multi-State Contraint Kalman Filter. + * (MSCKF). As presented Mourikis 2007 ICRA paper. + */ + class MsckfUpdate : public Update + { + public: + /** + * Constructor. + * + * Does the full update matrix construction job. + * + * @param[in] tkrs Feature tracks in normalized coordinates + * @param[in] quats Camera quaternion states + * @param[in] poss Camera position states + * @param[in] triangulator Feature triangulator + * @param[in] cov_s Error state covariance + * @param[in] n_poses_max Maximum number of poses in sliding window. + * @param[in] sigma_img Standard deviation of feature measurement + * [in normalized coordinates] + */ + MsckfUpdate(const x::TrackList& trks, + const x::AttitudeList& quats, + const x::TranslationList& poss, + const Triangulation& triangulator, + const Eigen::MatrixXd& cov_s, + const int n_poses_max, + const double sigma_img); + + /***************************** Getters **********************************/ + + /** + * Returns a constant reference to the inliers 3D cartesian coodinates. + */ + const Vector3dArray& getInliers() const { return inliers_; }; + + /** + * Returns a constant reference to the outliers 3D cartesian coodinates. + */ + const Vector3dArray& getOutliers() const { return outliers_; }; + + private: + /** + * 3D cartesian coordinates prior for MSCKF feature inliers + */ + Vector3dArray inliers_; + + /** + * 3D cartesian coordinates prior for MSCKF feature outliers + */ + Vector3dArray outliers_; + + /** + * Process one feature track. + * + * @param[in] track Feature track in normalized coordinates + * @param[in] C_q_G Camera attitude state list + * @param[in] G_p_C Camera position state list + * @param[in] triangulator Feature triangulator + * @param[in] P State covariance + * @param[in] n_poses_max Maximum number of poses in sliding window. + * @param[in] var_img Variance of feature measurement. + * [in normalized coordinates]. + * @param[in] j Index of current track in the MSCKF track list + * @param[out] row_h Current rows in stacked Jacobian + */ + void processOneTrack(const x::Track& track, + const x::AttitudeList& C_q_G, + const x::TranslationList& G_p_C, + const Triangulation& triangulator, + const Eigen::MatrixXd& P, + const int n_poses_max, + const double var_img, + const size_t& j, + size_t& row_h); + + + }; +} + +#endif // X_VIO_MSCKF_UPDATE_H_ diff --git a/include/x/vio/range_update.h b/include/x/vio/range_update.h new file mode 100644 index 0000000..902ffbd --- /dev/null +++ b/include/x/vio/range_update.h @@ -0,0 +1,119 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_VIO_RANGE_UPDATE_H_ +#define X_VIO_RANGE_UPDATE_H_ + +#include +#include + +namespace x +{ + /** + * Range visual update. + * + * This update requires SLAM states. As presented in xVIO tech report. + */ + class RangeUpdate : public Update + { + public: + + /** + * Constructor. + * + * Does the full update matrix construction job. + * + * @param[in] range_meas Range measurement + * @param[in] tr_feat_ids Indexes of the 3 triangular facet vertices + * in the SLAM state vector. + * @param[in] quats Camera quaternion states + * @param[in] poss Camera position states + * @param[in] feature_states Feature state vector + * @param[in] anchor_idxs Anchor pose indexes of inverse-depth SLAM + * features + * @param[in] cov_s Error state covariance + * @param[in] n_poses_max Maximum number of poses in sliding window. + * @param[in] sigma_range Standard deviation of range measurement noise + * [m]. + */ + RangeUpdate(const x::RangeMeasurement& range_meas, + const std::vector& tr_feat_ids, + const x::AttitudeList& quats, + const x::TranslationList& poss, + const Eigen::MatrixXd& feature_states, + const std::vector& anchor_idxs, + const Eigen::MatrixXd& cov_s, + const int n_poses_max, + const double sigma_range); + + private: + + using RangeJacobian = Eigen::Matrix; + + /** + * Processes a range measurement with the facet model. + * + * @param[in] range_meas Range measurement + * @param[in] tr_feat_ids Indexes of the 3 triangular facet vertices + * in the SLAM state vector. + * @param[in] C_q_G Camera attitude state list + * @param[in] G_p_C Camera position state list + * @param[in] feature_states Feature state vector + * @param[in] anchor_idxs Anchor pose indexes of inverse-depth SLAM + * features + * @param[in] P State covariance + * @param[in] n_poses_max Maximum number of poses in sliding window. + * @param[in] sigma_range Standard deviation of range measurement noise + * [m]. + */ + void processRangedFacet(const x::RangeMeasurement& range_meas, + const std::vector& tr_feat_ids, + const x::AttitudeList& C_q_G, + const x::TranslationList& G_p_C, + const Eigen::MatrixXd& feature_states, + const std::vector& anchor_idxs, + const Eigen::MatrixXd& P, + const int n_poses_max, + const double sigma_range); + + /** + * Processes a range measurement with the feature model. + * + * @param[in] range_meas Range measurement + * @param[in] C_q_G Camera attitude state list + * @param[in] G_p_C Camera position state list + * @param[in] feature_states Feature state vector + * @param[in] anchor_idxs Anchor pose indexes of inverse-depth SLAM + * features + * @param[in] P State covariance + * @param[in] n_poses_max Maximum number of poses in sliding window. + * @param[in] sigma_range Standard deviation of range measurement noise + * [m]. + * @param[in] j Index of feature in the list + */ + void processRangedFeature(const x::RangeMeasurement& range_meas, + const x::AttitudeList& C_q_G, + const x::TranslationList& G_p_C, + const Eigen::MatrixXd& feature_states, + const std::vector& anchor_idxs, + const Eigen::MatrixXd& P, + const int n_poses_max, + const double sigma_range, + const size_t& j); + }; +} + +#endif // X_VIO_SLAM_UPDATE_H_ diff --git a/include/x/vio/slam_update.h b/include/x/vio/slam_update.h new file mode 100644 index 0000000..b3274ff --- /dev/null +++ b/include/x/vio/slam_update.h @@ -0,0 +1,124 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_VIO_SLAM_UPDATE_H_ +#define X_VIO_SLAM_UPDATE_H_ + +#include +#include + +namespace x +{ + /** + * SLAM update. + * + * Inverse-depth SLAM. As presented in xVIO tech report. + */ + class SlamUpdate : public Update + { + public: + + /** + * Default constructor + */ + SlamUpdate() {}; + + /** + * Constructor. + * + * Does the full update matrix construction job. + * + * @param[in] tkrs Feature tracks in normalized coordinates. + * @param[in] quats Camera quaternion states. + * @param[in] poss Camera position states. + * @param[in] feature_states Feature state vector. + * @param[in] anchor_idxs Anchor pose indexes of inverse-depth SLAM + * features. + * @param[in] cov_s Error state covariance. + * @param[in] n_poses_max Maximum number of poses in sliding window. + * @param[in] sigma_img Standard deviation of feature measurement + * [in normalized coordinates] + */ + SlamUpdate(const x::TrackList& trks, + const x::AttitudeList& quats, + const x::TranslationList& poss, + const Eigen::MatrixXd& feature_states, + const std::vector& anchor_idxs, + const Eigen::MatrixXd& cov_s, + const int n_poses_max, + const double sigma_img); + + /** + * Compute inverse-depth 3D coordinates based for the new SLAM features. + * + * This method assumed the last pose state is the anchor. These + * coordinates will become the new SLAM feature states. + * + * @param[in] new_trks N image tracks to initialize as states. + * @param[in] rho_0 Initial inverse depth of SLAM features [1/m]. + * @param[out] ivds 3N vector {alpha, beta, rho}i of inverse-depth. + * coordinates + */ + void computeInverseDepthsNew(const x::TrackList& new_trks, + const double rho_0, + Eigen::MatrixXd& ivds) const; + + private: + + /** + * Process one feature track. + * + * @param[in] track Feature track in normalized coordinates + * @param[in] C_q_G Camera attitude state list + * @param[in] G_p_C Camera position state list + * @param[in] feature_states Feature state vector + * @param[in] anchor_idxs Anchor pose indexes of inverse-depth SLAM + * features + * @param[in] P State covariance + * @param[in] n_poses_max Maximum number of poses in sliding window. + * @param[in] var_img Variance of feature measurement + * [in normalized coordinates]. + * @param[in] j Index of current track in the MSCKF track list + * @param[out] row_h Current rows in stacked Jacobian + */ + void processOneTrack(const x::Track& track, + const x::AttitudeList& C_q_G, + const x::TranslationList& G_p_C, + const Eigen::MatrixXd& feature_states, + const std::vector& anchor_idxs, + const Eigen::MatrixXd& P, + const int n_poses_max, + const double var_img, + const size_t& j, + size_t& row_h); + + /** + * Computes inverse-depth 3D coordinates of 1 new feature. + * + * @param[in] feature New feature normalized coordinates. + * @param[in] rho_0 Initial inverse depth of SLAM features [1/m]. + * @param[in] idx Index at which the coordinates should be inserted at in ivds. + * @param[out] ivds 3N vector {alpha, beta, rho}i of inverse-depth. + * coordinates + */ + void computeOneInverseDepthNew(const x::Feature& feature, + const double rho_0, + const unsigned int& idx, + Eigen::MatrixXd& ivds) const; + }; +} + +#endif // X_VIO_SLAM_UPDATE_H_ diff --git a/include/x/vio/solar_update.h b/include/x/vio/solar_update.h new file mode 100644 index 0000000..72fa1a0 --- /dev/null +++ b/include/x/vio/solar_update.h @@ -0,0 +1,61 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_VIO_SOLAR_UPDATE_H_ +#define X_VIO_SOLAR_UPDATE_H_ + +#include +#include + +namespace x +{ + /** + * Solar update. + * + * As presented in 2020 IEEE Aerospace paper. + */ + class SolarUpdate : public Update + { + public: + + /** + * Constructor. + * + * Does the full update matrix construction job. + * + * @param[in] range_meas Range measurement. + * @param[in] quats Camera quaternion states. + * @param[in] cov_s Error state covariance matrix. + */ + SolarUpdate(const x::SunAngleMeasurement& angle, + const x::Quaternion& quat, + const Eigen::MatrixXd& cov_s); + + private: + + using SunAngleJacobian = Eigen::Matrix; + + /** @brief Processes a sun sensor measurement. + * + * @param[in] angle Sun angle measurement + * @param[in] quat IMU attitude in state vector + */ + void processSunAngle(const x::SunAngleMeasurement& angle, + const x::Quaternion& quat); + }; +} + +#endif // X_VIO_SOLAR_UPDATE_H_ diff --git a/include/x/vio/state_manager.h b/include/x/vio/state_manager.h new file mode 100644 index 0000000..0e1b2aa --- /dev/null +++ b/include/x/vio/state_manager.h @@ -0,0 +1,223 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef STATE_MANAGER_H +#define STATE_MANAGER_H + +#include +#include + +#include +#include + +namespace x { + class StateManager { + public: + StateManager(int n_poses_max = 0, int n_features_max = 0) + : n_poses_max_ { n_poses_max } + , n_features_max_ { n_features_max } + , anchor_idxs_(n_features_max , -1) + {} + + /******************************* Getters ************************************/ + + size_t getNFeatures() const { return n_features_; }; + + std::vector getAnchorIdxs() const { return anchor_idxs_; }; + + /****************************************************************************/ + + void clear(); + + /** + * SLAM and MSCKF state management: slide window, add, remove, reparam. + * + * Adds and removes necessary states from the closest state to timestamp + * in the buffer. Changes are propagated to the latest state. New states + * are the latest camera position and attitude estimates. Removed states + * are lost persistent features. This method also clears the sliding + * window when it has reached max size. + * + * @param[in,out] state State to manage. + * @param[in] del_feat_idx Indexes of features to be deleted in the state + * vector. + */ + void manage(State& state, std::vector del_feat_idx); + + /** + * Initializes MSCKF-SLAM features in state and covariance in one batch. + * + * @param[in,out] state state to be augmented. + * @param[in] init_mats MSCKF-SLAM initialization matrices. + * @param[in] correction Correction vector after feature's opportunistic + * update. + * @param[in] sigma_img Standard deviation of feature measurement + * [in normalized coordinates]. + */ + void initMsckfSlamFeatures(State& state, + const x::MsckfSlamMatrices& init_mats, + const Matrix& correction, + const double sigma_img); + + /** + * Initializes a new SLAM the standard way (see Civera). + * + * @param[in,out] state state to be augmented. + * @param[in] feature Inverse-depth feature coordinates (size 3N). + * @param[in] sigma_img Standard deviation of feature measurement + * [in normalized coordinates]. + * @param[in] sigma_rho_0 Initial standard deviation of SLAM inverse + * depth [1/m]. + */ + void initStandardSlamFeatures(State& state, + const Matrix& feature, + const double sigma_img, + const double sigma_rho_0); + + unsigned int anchorPoseIdxForFeatureAtIdx(unsigned int i) + { + assert(i < n_features_); + return anchor_idxs_[i]; + } + + std::vector computeSLAMCartesianFeaturesForState( + const State& state) const; + + /// The value of poseSize() is the number of poses stored in the most recent state. + /// \return The current number of poses. + size_t poseSize(void) const { return n_poses_; } + + /** + * Convert attitude states to an attitude list. + * + * @param[in] state State input. + * @param[in] max_size Max size of output list, cropping from the end (default 0: no cropping) + * @return The attitude list. + */ + AttitudeList convertCameraAttitudesToList(const State& state, + const int max_size = 0) const; + + /** + * Convert position states to a translation list. + * + * @param[in] state State input. + * @return The position list. + */ + x::TranslationList convertCameraPositionsToList(const State& state) const; + + /// Retrieve a specific entry from an attitudes buffer. + /// \param attitudes The buffer to retrieve the attitude from + /// \param idx The index of the attitude in attitudes + /// \return The value of the camera attitudes at position idx in state. + Eigen::Vector4d Attitude(Eigen::VectorXd const& attitudes, int idx) const + { + return x::MatrixBlock(attitudes, idx * 4, 0, 4, 1); + } + + /// Retrieve a specific entry from an positions buffer. + /// \param positions The buffer to retrieve the position from + /// \param idx The index of the position in positions + /// \return The value of the camera positions at position idx in state. + Eigen::Vector3d Position(Eigen::VectorXd const& positions, int idx) const + { + return x::MatrixBlock(positions, idx * 3, 0, 3, 1); + } + + private: + /** + * Maximum number of poses in state sliding window. + * + * Fixed for all states. + */ + int n_poses_max_; + + /** + * Maximum number of features in state. + * + * Fixed for all states. + */ + int n_features_max_; + + /** + * Current number of poses in state vector. + */ + int n_poses_ = 0; + + size_t n_features_ = 0; ///< Current number of persistent features + + /** + * Anchor pose indexes of the SLAM feature states. + * + * i-th entry is the index of the pose state which is the anchor for SLAM + * feature state i, in inverse-depth coordinates. + */ + std::vector anchor_idxs_; + + bool stateHasBeenFilledBefore_ = false; + + static unsigned constexpr jac_rows = 3; ///< Rows of the Jacobian matrix entries + static unsigned constexpr jac_cols = 3; ///< Cols of the Jacobian matrix entries + + using Jacobian = Eigen::Matrix; + + /** + * Augment state covariance matrix with the new poses. + * + * @param[in] state Input state + * @param[in] pos Current pose index in the sliding window. + * @param[in,out] Covariance matrix of input state + * TODO(jeff) Unsafe, covariance matrix is already in the state object. + */ + void augmentCovariance(const State& state, + const int pos, + Matrix& covariance); + + /// Reparametrizes persistent features which are anchored to the oldest pose + /// in the sliding window. + /// \param atts_old sliding windows of attitudes before cleanup + /// \param poss_old sliding windows of positions before cleanup + /// \param features feature states to be reparametrized + /// \param covariance covariance matrix to be reparametrized + void reparametrizeFeatures(Eigen::VectorXd const& atts_old, + Eigen::VectorXd const& poss_old, + Eigen::VectorXd& features, + Matrix& covariance); + + /// Slides the window of poses in the state vector and covariance matrix. This + /// adds the newest pose, but also removes the oldest one when the window is + /// full. + /// \param atts Sliding windows of attitudes + /// \param poss Sliding windows of positions + /// \param covariance Covariance matrix + void slideWindow(Eigen::VectorXd& atts, + Eigen::VectorXd& poss, + Matrix& covariance); + + /* \brief Append new features to the state vector and covariance + * matrix + * + * \param state_ptr + * \param new_features 3n feature states to insert + * \param cov 3n x 3n feature covariance matrix + * \param cross 3n x n cross-covariance matrix with other states + */ + void addFeatureStates(State& state_ptr, + const Matrix& new_features, + const Matrix& cov, + const Matrix& cross); + }; +} // namespace x +#endif /* STATE_MANAGER_H */ diff --git a/include/x/vio/tools.h b/include/x/vio/tools.h new file mode 100644 index 0000000..a4e8ced --- /dev/null +++ b/include/x/vio/tools.h @@ -0,0 +1,86 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef _TOOLS_H_ +#define _TOOLS_H_ + +#include + +namespace x +{ +template +Eigen::Block const MatrixBlock( + Eigen::MatrixBase const& matrix, int row, int col, int rows, int cols) { + return Eigen::Block(matrix.derived(), row, col, rows, cols); +} + +template +Eigen::Block MatrixBlock( + Eigen::MatrixBase& matrix, int row, int col, int rows, int cols) { + return Eigen::Block(matrix.derived(), row, col, rows, cols); +} + +template +Eigen::Block MatrixBlock(Eigen::MatrixBase& matrix, + int row, + int col) { + return MatrixBlock(matrix.derived(), + row, + col, + matrix.rows() - row, + matrix.cols() - col); +} +template +Eigen::Block const MatrixBlock( + Eigen::MatrixBase const& matrix, int row, int col) +{ + return MatrixBlock(matrix.derived(), + row, + col, + matrix.rows() - row, + matrix.cols() - col); +} + +struct Skew { + Eigen::Matrix3d matrix; + Eigen::Vector3d vector; + Skew(double x, double y, double z) : matrix(), vector(x, y, z) + { + matrix << 0, -vector(2), vector(1), vector(2), 0, -vector(0), -vector(1), + vector(0), 0; + } +}; + +struct Quatern { + x::Quaternion q; + x::Quaternion const& operator()(x::Attitude const& attitude) { + q.x() = attitude.ax; + q.y() = attitude.ay; + q.z() = attitude.az; + q.w() = attitude.aw; + return q; + } + + x::Quaternion const& operator()(Eigen::Vector4d const& attitude) { + q.x() = attitude(0, 0); + q.y() = attitude(1, 0); + q.z() = attitude(2, 0); + q.w() = attitude(3, 0); + return q; + } +}; +} // end namespace x +#endif /* _TOOLS_H_ */ diff --git a/include/x/vio/track_manager.h b/include/x/vio/track_manager.h new file mode 100644 index 0000000..f47c712 --- /dev/null +++ b/include/x/vio/track_manager.h @@ -0,0 +1,127 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TRACK_MANAGER_H_ +#define TRACK_MANAGER_H_ + +#include +#include +#include +#include + +class TrackManager +{ +public: + TrackManager(); + TrackManager(const x::Camera& camera, const double min_baseline_n); + void setCamera(x::Camera camera); + + // All track getters are in normalized image coordinates + x::TrackList getMsckfTracks() const; // Tracks ready for MSCKF update + x::TrackList getNewSlamStdTracks() const; + x::TrackList getNewSlamMsckfTracks() const; + + /** + * Normalize SLAM tracks. + * + * @param[in] size_out Max size of output tracks, cropping from the end + * (default 0: no cropping) + */ + x::TrackList normalizeSlamTracks(const int size_out) const; + + // Clear all the tracks (for re-init) + void clear(); + void removePersistentTracksAtIndex(const unsigned int idx); + // Remove invalid new persistent features at given indexes + void removeNewPersistentTracksAtIndexes(const std::vector invalid_tracks_idx); + + /** @brief Get indexes of SLAM tracks that were just lost + */ + std::vector getLostSlamTrackIndexes() const; + + /** + * Sorts input matches in various track types. + * + * Manages the various types of feature tracks based on the latest set of + * matches, e.g.: + * - append match to an existing track, + * - detect the end of a track, + * - change a track's type (e.g. opportunistic to persistent). + * + * @param[in] matches Latest output from the feature tracker (image front end) + * @param[in] cam_rots Camera orientations (up to current frame), for MSCKF baseline check + * @param[in] n_poses_max Maximum number of poses in sliding window. + * @param[in] n_slam_features_max Maximum number of SLAM features allowed in + * the state vector. + * @param[in] min_track_length Minimum track length for a visual feature to be + * processed. + * @param[out] img Output image for GUI + */ + void manageTracks(x::MatchList& matches, + const x::AttitudeList cam_rots, + const int n_poses_max, + const int n_slam_features_max, + const int min_track_length, + x::TiledImage& img); + + std::vector featureTriangleAtPoint(const x::Feature& lrf_img_pt, x::TiledImage& img) const; + void draw_delaunay(cv::Mat& img, cv::Subdiv2D& subdiv, cv::Scalar delaunay_color) const; +private: + x::Camera camera_; + x::TrackList slam_trks_; // persistent feature tracks (excluding new ones) + // New SLAM tracks (both semi-infinite depths uncertainty and MSCKF) + x::TrackList new_slam_trks_; + + /** @brief Indexes of SLAM tracks that were just lost. + */ + std::vector lost_slam_idxs_; + + x::TrackList opp_trks_; // opportunistic feature tracks + x::TrackList msckf_trks_n_; // Normalized tracks for MSCKF update + // New SLAM features initialized with semi-infinite depth uncertainty + // (regular SLAM) + x::TrackList new_slam_std_trks_n_; + // New SLAM features initialized with MSCKF tracks (MSCKF-SLAM) + x::TrackList new_slam_msckf_trks_n_; + // Minimum baseline for MSCKF measurements (in normal plane) + double min_baseline_n_ = 0.0; + + /** \brief MSCKF baseline check. + * + * Checks if the camera has enough baseline for triangulation (for + * MSCKF). This is done by rectifying all the feature observations + * as if the camera maintained the same attitude, here that of the + * last camera, and comparing the maximum displacement in pixels + * to a threshold. This is done in pixel space to be independent + * of scale. + * + * @param track Feature measurement list (size n_obs) + * @param cam_att Camera attitude list (size n_quats >= n_obs) + * @return True if baseline is larger than threshold + */ + bool checkBaseline(const x::Track& track, const x::AttitudeList& C_q_G) const; + + /** + * Track manager debug image. + * + * @param[in] img Camera image + * @param[in] min_track_length Minimum track length for a visual feature to be + * processed. + */ + void plotFeatures(x::TiledImage& img, const int min_track_length); +}; + +#endif // TRACK_MANAGER_H diff --git a/include/x/vio/types.h b/include/x/vio/types.h new file mode 100644 index 0000000..0733a5f --- /dev/null +++ b/include/x/vio/types.h @@ -0,0 +1,274 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_VIO_TYPES_H +#define X_VIO_TYPES_H + +#include +#include +#include + +#include +#include + +/** + * This header defines common types used in xVIO. + */ +namespace x +{ + /** + * User-defined parameters + */ + struct Params + { + Vector3 p; + Vector3 v; + Quaternion q; + Vector3 b_w; + Vector3 b_a; + double cam_fx; + double cam_fy; + double cam_cx; + double cam_cy; + double cam_s; + int img_height; + int img_width; + Vector3 p_ic; + Quaternion q_ic; + + /** + * Standard deviation of feature measurement [in normalized coordinates] + */ + double sigma_img; + + /** + * Standard deviation of range measurement noise [m]. + */ + double sigma_range; + + Quaternion q_sc; + Vector3 w_s; + double n_a; + double n_ba; + double n_w; + double n_bw; + int fast_detection_delta; + bool non_max_supp; + int block_half_length; + int margin; + int n_feat_min; + int outlier_method; + double outlier_param1; + double outlier_param2; + int n_tiles_h; + int n_tiles_w; + int max_feat_per_tile; + double time_offset; + + /** + * Maximum number of poses in the sliding window. + */ + int n_poses_max; + + /** + * Maximum number of SLAM features. + */ + int n_slam_features_max; + + /** + * Initial inverse depth of SLAM features [1/m]. + * + * This is when SLAM features can't be triangulated for MSCKK-SLAM. By + * default, this should be 1 / (2 * d_min), with d_min the minimum + * expected feature depth (2-sigma) [Montiel, 2006] + */ + double rho_0; + + /** + * Initial standard deviation of SLAM inverse depth [1/m]. + * + * This is when SLAM features can't be triangulated for MSCKK-SLAM. By + * default, this should be 1 / (4 * d_min), with d_min the minimum + * expected feature depth (2-sigma) [Montiel, 2006]. + */ + double sigma_rho_0; + + /** + * Number of IEKF iterations (EKF <=> 1). + */ + int iekf_iter; + + /** + * Minimum baseline to trigger MSCKF measurement (pixels). + */ + double msckf_baseline; + + /** + * Minimum track length for a visual feature to be processed. + */ + int min_track_length; + + double traj_timeout_gui; + bool init_at_startup; + }; + + /** + * MSCKF-SLAM matrices + */ + struct MsckfSlamMatrices { + /** + * H1 matrix in Li's paper (stacked for all features) + */ + Matrix H1; + + /** + * H2 matrix in Li's paper (stacked for all features) + */ + Matrix H2; + + /** + * z1 residual vector in Li's paper (stacked for all features) + */ + Matrix r1; + + /** + * New SLAM feature vectors (stacked for all features). + * + * Features' inverse-depth coordinates, assuming the latest camera as an + * anchor. These estimates are taken from the MSCKF triangulation prior. + */ + Matrix features; + }; + + /** + * Range measurement. + * + * Coming from a Laser Range Finder (LRF). + */ + struct RangeMeasurement + { + double timestamp { kInvalid }; + + double range { 0.0 }; + + /** + * Image coordinate of the LRF beam. + * + * This is assumed to be a single point in the image, i.e. the LRF axis + * passes by the optical center of the camera. + */ + Feature img_pt { Feature() }; + + /** + * Normalized image coordinates of the LRF beam. + * + * Follows the same assumptions as img_pt. + */ + Feature img_pt_n { Feature() }; + }; + + /** + * Sun angle measurement. + * + * Coming from a sun sensor. + */ + struct SunAngleMeasurement + { + double timestamp { kInvalid }; + double x_angle { 0.0 }; + double y_angle { 0.0 }; + }; + + /** + * VIO update measurement. + * + * This struct includes all sensor the measurements that will processed to + * create an EKF update. This should be at least an image (or a set of visual + * matches) in xVIO, along side optional additional sensor measurements that + * are assumed to be synced with the visual data (LRF and sun sensor). + */ + struct VioMeasurement { + /** + * Timestamp. + * + * This timestamp is the visual measurement timestamp (camera or match + * list). Range and sun angle measurement timestamps might different but + * will be processed at the sam timestamp as a single EKF update. + */ + double timestamp { kInvalid }; + + /** + * Sequence ID. + * + * Consecutively increasing ID associated with the visual measurement + * (matches or image). + */ + unsigned int seq { 0 }; + + /** + * Visual match measurements. + * + * Output of a visual feature tracker (or simulation). + */ + MatchList matches; + + /** + * Image measurement. + * + * Will only be used if the visual match list struct member has size zero, + * in which case a feature track will run on that image. + */ + TiledImage image; + + /** + * Range measurement. + */ + RangeMeasurement range; + + /** + * Sun angle measurement. + */ + SunAngleMeasurement sun_angle; + + /** + * Default constructor. + */ + VioMeasurement() {} + + /** + * Full constructor. + */ + VioMeasurement(const double timestamp, + const unsigned int seq, + const MatchList& matches, + const TiledImage& image, + const RangeMeasurement& range, + const SunAngleMeasurement& sun_angle) + : timestamp { timestamp } + , seq { seq } + , matches { matches } + , image { image } + , range { range } + , sun_angle { sun_angle } + {} + }; + + using Vector3dArray = std::vector; + + using Time = double; +} + +#endif // X_VIO_TYPES_H diff --git a/include/x/vio/update.h b/include/x/vio/update.h new file mode 100644 index 0000000..7f8263f --- /dev/null +++ b/include/x/vio/update.h @@ -0,0 +1,81 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_VIO_UPDATE_H_ +#define X_VIO_UPDATE_H_ + +#include +#include + +namespace x +{ + /** + * An abstract Kalman update class. + * + * It defines the members and getters to be used by any derived + * update class: Jacobian matrix, residual and measurement + * covariance diagonal. + */ + class Update + { + public: + + /******************************* Getters ********************************/ + + /** + * Returns a constant reference to the residual vector. + */ + const Matrix& getResidual() const { return res_; }; + + /** + * Returns a constant reference to the Jacobian matrix. + */ + const Matrix& getJacobian() const { return jac_; }; + + /** + * Returns a constant reference to the diagonal of the measurement + * covariance matrix. + */ + const Eigen::VectorXd& getCovDiag() const { return cov_m_diag_; }; + + protected: + // Column number for generic Jacobian block (3 error states) + static unsigned constexpr kJacCols = 3; + + // Vision Jacobian block + static unsigned constexpr kVisJacRows = 2; + using VisJacBlock = Eigen::Matrix; + + /** + * Residual vector. + */ + Matrix res_; + + /** + * Jacobian matrix. + */ + Matrix jac_; + + /** + * Diagonal of the measurement covariance matrix. + */ + Eigen::VectorXd cov_m_diag_; + }; +} + +#endif // X_VIO_UPDATE_H_ diff --git a/include/x/vio/vio.h b/include/x/vio/vio.h new file mode 100644 index 0000000..c067e2e --- /dev/null +++ b/include/x/vio/vio.h @@ -0,0 +1,141 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_VIO_VIO_H_ +#define X_VIO_VIO_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace x { + class VIO + { + public: + VIO(); + bool isInitialized() const; + void setUp(const Params& params); + void setLastRangeMeasurement(RangeMeasurement range_measurement); + void setLastSunAngleMeasurement(SunAngleMeasurement angle_measurement); + void initAtTime(double now); + void getMsckfFeatures(Vector3dArray& inliers, Vector3dArray& outliers); + + /** + * Pass IMU measurements to EKF for propagation. + * + * @param[in] timestamp + * @param[in] msg_seq Message ID. + * @param[in] w_m Angular velocity (gyroscope). + * @param[in] a_m Specific force (accelerometer). + * @return The propagated state. + */ + State processImu(const double timestamp, + const unsigned int seq, + const Vector3& w_m, + const Vector3& a_m); + + /** + * Creates an update measurement from image and pass it to EKF. + * + * @param[in] timestamp Image timestamp. + * @param[in] seq Image sequence ID. + * @param[in,out] match_img Image input, overwritten as tracker debug image + * in output. + * @param[out] feature_img Track manager image output. + * @return The updated state, or invalide if the update could not happen. + */ + State processImageMeasurement(double timestamp, + const unsigned int seq, + TiledImage& match_img, + TiledImage& feature_img); + + /** + * Creates an update measurement from visual matches and pass it to EKF. + * + * @param[in] timestamp Image timestamp. + * @param[in] seq Image sequence ID. + * @param[in,out] match_vector Feature matches vector. + * @param[out] match_img Tracker image output. + * @param[out] feature_img Track manager image output. + * @return The updated state, or invalid if the update could not happen. + */ + State processMatchesMeasurement(double timestamp, + const unsigned int seq, + const std::vector& match_vector, + TiledImage& match_img, + TiledImage& feature_img); + + /** + * Compute cartesian coordinates of SLAM features for input state. + * + * @param[in] state Input state. + * @return A vector with the 3D cartesian coordinates. + */ + std::vector + computeSLAMCartesianFeaturesForState(const State& state); + + private: + /** + * Extended Kalman filter estimation back end. + */ + Ekf ekf_; + + /** + * VIO EKF updater. + * + * Constructs and applies an EKF update from a VIO measurement. The EKF + * class owns a reference to this object through Updater abstract class, + * which it calls to apply the update. + */ + VioUpdater vio_updater_; + + Params params_; + + /** + * Minimum baseline for MSCKF (in normalized plane). + */ + double msckf_baseline_n_; + + Camera camera_; + Tracker tracker_; + TrackManager track_manager_; + StateManager state_manager_; + RangeMeasurement last_range_measurement_; + SunAngleMeasurement last_angle_measurement_; + bool initialized_ { false }; + + /** + * Import a feature match list from a std::vector. + * + * @param[in] match_vector Input vector of matches. + * @param[in] seq Image sequence ID. + * @param[out] img_plot Debug image. + * @return The list of matches. + */ + MatchList importMatches(const std::vector& match_vector, + const unsigned int seq, + x::TiledImage& img_plot) const; + + double GetTimestamp(size_t index); + }; +} // namespace x + +#endif // X_VIO_VIO_H_ diff --git a/include/x/vio/vio_updater.h b/include/x/vio/vio_updater.h new file mode 100644 index 0000000..659fb7b --- /dev/null +++ b/include/x/vio/vio_updater.h @@ -0,0 +1,214 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_VIO_VIO_UPDATER_H_ +#define X_VIO_VIO_UPDATER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace x { + class VioUpdater : public Updater + { + public: + /** + * Default constructor. + */ + VioUpdater() {}; + + /** + * Construct with known parameters. + */ + VioUpdater(Tracker& tracker, + StateManager& state_manager, + TrackManager& track_manager, + const double sigma_img, + const double sigma_range, + const double rho_0, + const double sigma_rho_0, + const int min_track_length, + const int iekf_iter = 1); + + void setMeasurement(const VioMeasurement& measurement); + + /** + * Get measurement timestamp. + */ + double getTime() const; + + /** + * Get reference to the tracker debug image. + */ + TiledImage& getMatchImage(); + + /** + * Get reference to the track manager debug image. + */ + TiledImage& getFeatureImage(); + + /** + * Get list of MSCKF inliers' 3D coordinates. + */ + Vector3dArray getMsckfInliers() const; + + /** + * Get list of MSCKF outliers' 3D coordinates. + */ + Vector3dArray getMsckfOutliers() const; + + private: + /** + * VIO measurement: image + optional range and sun angle. + */ + VioMeasurement measurement_; + + Tracker tracker_; + StateManager state_manager_; + TrackManager track_manager_; + + /** + * Tracker debug image. + */ + TiledImage match_img_; + + /** + * Track manager debug image. + */ + TiledImage feature_img_; + + /** + * Standard deviation of feature measurement [in normalized coordinates]. + */ + double sigma_img_; + + /** + * Standard deviation of range measurement noise [m]. + */ + double sigma_range_; + + /** + * Initial inverse depth of SLAM features [1/m]. + */ + double rho_0_; + + /** + * Initial standard deviation of SLAM inverse depth [1/m]. + */ + double sigma_rho_0_; + + /** + * Minimum track length for a visual feature to be processed. + */ + int min_track_length_; + + + // All track members assume normalized undistorted feature coordinates. + TrackList msckf_trks_; // Normalized tracks for MSCKF update + // New SLAM features initialized with semi-infinite depth uncertainty + // (standard SLAM) + TrackList new_slam_std_trks_; + // New SLAM features initialized with MSCKF tracks (MSCKF-SLAM) + TrackList new_msckf_slam_trks_; + TrackList slam_trks_; + std::vector lost_slam_trk_idxs_; + Vector3dArray msckf_inliers_; //!< 3D coordinates prior for MSCKF feature + //!< inliers. + Vector3dArray msckf_outliers_; //!< 3D coordinates prior for MSCKF feature + //!< outliers. + + /** + * SLAM update object. + */ + SlamUpdate slam_; + + /** + * MSCKF-SLAM update object + */ + MsckfSlamUpdate msckf_slam_; + + /** + * Measurement processing + * + * Only happens once, and is stored if measurement later re-applied. Here this + * where image processing takes place. + * + * @param[in] state State prior + */ + void preProcess(const State& state); + + /** + * Pre-update work + * + * Stuff that needs happen before the Kalman update. This function will NOT + * be called at each IEKF iteration. Here, this corresponds to state + * management. + * + * @param[in,out] state Update state + * @return True if an update should be constructed + */ + bool preUpdate(State& state); + + /** + * Update construction. + * + * Prepares measurement Jacobians, residual and noice matrices and applies + * (iterated) EKF update. This function WILL be called at each IEKF iteration. + * + * @param[in] state Update state + * @param[out] h Measurement Jacobian matrix + * @param[out] res Measurement residual vector + * @param[out] r Measurement noise covariance matrix + */ + void constructUpdate(const State& state, + Matrix& h, + Matrix& res, + Matrix& r); + + /** + * Post-update work. + * + * Stuff that need to happen after the Kalman update. Here this SLAM feature + * initialization. + * + * @param[in,out] state Update state. + * @param[in] correction Kalman state correction. + */ + void postUpdate(State& state, const Matrix& correction); + + /** @brief QR decomposition + * + * Computes the QR decomposition of the MSCKF update + * Jacobian and updates the other terms according to Mourikis' + * 2007 paper. + * + * @param[in,out] h Jacobian matrix of MSCKF measurements + * @param[in,out] res MSCKF measurement residuals + * @param[in,out] R Measurement noise matrix + */ + void applyQRDecomposition(Matrix& h, Matrix& res, Matrix& R); + + friend class VIO; + }; +} // namespace x + +#endif // XVIO_MEASUREMENT_H_ diff --git a/include/x/vision/camera.h b/include/x/vision/camera.h new file mode 100644 index 0000000..359aeda --- /dev/null +++ b/include/x/vision/camera.h @@ -0,0 +1,67 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef JPL_VPP_CAMERA_H_ +#define JPL_VPP_CAMERA_H_ + +#include +#include "opencv2/highgui/highgui.hpp" + +namespace x +{ +class Camera +{ +public: + Camera(); + Camera(double fx, + double fy, + double cx, + double cy, + double s, + unsigned int img_width, + unsigned int img_height); + unsigned int getWidth() const; + unsigned int getHeight() const; + double getInvFx() const; + double getInvFy() const; + double getCxN() const; + double getCyN() const; + void undistort(FeatureList& features) const; + void undistort(Feature& feature) const; + // Returns image coordinates in normal plane + Feature normalize(const Feature& feature) const; + Track normalize(const Track& track, const size_t max_size = 0) const; + TrackList normalize(const TrackList& tracks, const size_t max_size = 0) const; +private: + double fx_ = 0.0; // Focal length + double fy_ = 0.0; + double cx_ = 0.0; // Principal point + double cy_ = 0.0; + double s_ = 0.0; // Distortion + unsigned int img_width_ = 0.0; + unsigned int img_height_ = 0.0; + // Distortion terms we only want to compute once + double inv_fx_ = -1; // Inverse focal length + double inv_fy_ = -1; + double cx_n_ = -1; // Principal point in normalized coordinates + double cy_n_ = -1; + double s_term_ = -1; // Distortion term + // Inverse FOV distortion transformation + double inverseTf(const double dist) const; +}; +} + +#endif diff --git a/include/x/vision/feature.h b/include/x/vision/feature.h new file mode 100644 index 0000000..de7e4c5 --- /dev/null +++ b/include/x/vision/feature.h @@ -0,0 +1,133 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_FEATURE_H_ +#define X_FEATURE_H_ + +namespace x +{ +class Feature +{ +public: + + /************************** Constructors **************************/ + + Feature(); + + Feature(double timestamp, + double x, + double y); + + Feature(double timestamp, + unsigned int frame_number, + double x, + double y, + double x_dist, + double y_dist); + + Feature(double timestamp, + unsigned int frame_number, + double x_dist, + double y_dist, + unsigned int pyramid_level, + float fast_score); + + /**************************** Setters *****************************/ + + void setX(const double x) { x_ = x; }; + + void setY(const double y) { y_ = y; }; + + void setXDist(const double x_dist) { x_dist_ = x_dist; }; + + void setYDist(const double y_dist) { y_dist_ = y_dist; }; + + void setTile(int row, int col) { tile_row_ = row; tile_col_ = col; }; + + /**************************** Getters *****************************/ + + double getTimestamp() const {return timestamp_; }; + + double getX() const { return x_; }; + + double getY() const { return y_; } + + double getXDist() const { return x_dist_; }; + + double getYDist() const { return y_dist_; }; + + unsigned int getPyramidLevel() const { return pyramid_level_; }; + + float getFastScore() const { return fast_score_; }; + + int getTileRow() const { return tile_row_; }; + + int getTileCol() const { return tile_col_; }; + +private: + /** + * Image timestamp [s] + */ + double timestamp_ = 0.0; + + /** + * Image ID + */ + unsigned int frame_number_ = 0; + + /** + * (Undistorted) X pixel coordinate + */ + double x_ = 0.0; + + /** + * (Undistorted) Y pixel coordinate + */ + double y_ = 0.0; + + /** + * Distorted X pixel coordinate + */ + double x_dist_ = 0.0; + + /** + * Distorted Y pixel coordinate + */ + double y_dist_ = 0.0; + + /** + * Pyramid level in which the feature was detected + */ + unsigned int pyramid_level_ = 0; + + /** + * FAST score + */ + float fast_score_ = 0.0; + + /** + * Row index of the image tile in which the feature is located + */ + int tile_row_ = -1; + + /** + * Column index of the image tile in which the feature is located + */ + int tile_col_ = -1; +}; +} + +#endif // X_FEATURE diff --git a/include/x/vision/tiled_image.h b/include/x/vision/tiled_image.h new file mode 100644 index 0000000..3f9c097 --- /dev/null +++ b/include/x/vision/tiled_image.h @@ -0,0 +1,80 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef JPL_VPP_TILED_IMAGE_H_ +#define JPL_VPP_TILED_IMAGE_H_ + +#include + +#include + +namespace x +{ +class TiledImage : public cv::Mat +{ +public: + TiledImage(); + TiledImage(const Mat& cv_img); + TiledImage(const TiledImage& img); + TiledImage(const Mat& img, + double timestamp, + unsigned int frame_number, + unsigned int n_tiles_h, + unsigned int n_tiles_w, + unsigned int max_feat_per_tile); + TiledImage& operator=(const TiledImage& other); + TiledImage clone() const; + void setTileSize(const double tile_height, const double tile_width); + + void setTileParams(unsigned int n_tiles_h, + unsigned int n_tiles_w, + unsigned int max_feat_per_tile); + + double getTimestamp() const; + unsigned int getFrameNumber() const; + double getTileHeight() const; + double getTileWidth() const; + unsigned int getNTilesH() const; + unsigned int getNTilesW() const; + unsigned int getMaxFeatPerTile() const; + + // Get the feature count for a specific tile + unsigned int getFeatureCountAtTile(unsigned int row, unsigned int col) const; + + // Increment feature count for a specific tile + void incrementFeatureCountAtTile(unsigned int row, unsigned int col); + void resetFeatureCounts(); + void setTileForFeature(Feature& feature) const; + // Plot one feature in the image + void plotFeature(Feature& feature, cv::Scalar color); + // Plot tiles on the image + void plotTiles(); + +private: + double timestamp_ = 0.0; + unsigned int frame_number_ = 0; + double tile_height_ = 0.0; + double tile_width_ = 0.0; + unsigned int n_tiles_h_ = 1; // number of tiles along height + unsigned int n_tiles_w_ = 1; // number of tiles along width (1 <=> no tiling) + unsigned int max_feat_per_tile_ = 40; // maximum count of features per image tile + unsigned int n_tiles_ = 1; // total number of image tiles (n_tiles_w_ * n_tiles_h) + std::vector tiles_; // n_tiles_w_ x n_tiles_h 1D array that holds count + // of features per tile (row, col) +}; +} + +#endif diff --git a/include/x/vision/timing.h b/include/x/vision/timing.h new file mode 100644 index 0000000..b810d12 --- /dev/null +++ b/include/x/vision/timing.h @@ -0,0 +1,120 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef JPL_VPP_TIMING_H +#define JPL_VPP_TIMING_H + +#ifdef TIMING + +#include +#include +#include +#include +#include + +#include + +namespace x +{ +// If not defined, timing output is only printed at program end. +#define TIMER_TO_COUT + +struct TimeSum { + using Dur = int; + using Unit = std::chrono::microseconds; + int runs; + Dur total_time; + TimeSum(Dur time) : runs(1), total_time(time) {} + TimeSum(void) : runs(0), total_time(0) {} +}; + +static inline void FinishTimer(int signal); +class Timer { +public: + using Id = std::string; + using Time = std::chrono::time_point; + using Dur = TimeSum::Dur; + using Unit = TimeSum::Unit; + static std::string DurAsString; + static void Start(Id const id); + static void Stop(Id const id); + static void Finish(void); + + static inline Time Now() { return std::chrono::system_clock::now(); } + +private: + using ClockMap = std::map; + using Iterator = std::map::const_iterator; + using ClockSums = std::map; + + static Timer* timer_; + ClockSums sums_; + ClockMap clocks_; + +#ifndef TIMER_TO_COUT + std::stringstream stream_; +#endif + + /// Default Constructor. This installs an anonymous signalhandler + /// to be executed on ctrl-c-events and call the Timer::Finish() + /// method. + Timer(void) { signal(SIGINT, FinishTimer); } + inline Iterator GetClock(Timer::Id id) { + auto it = clocks_.find(id); + return it; + } + + inline void RemoveClock(Timer::Iterator it) { clocks_.erase(it); } + + inline Timer::Iterator AddClock(Id id) { clocks_.insert({id, Now()}); } + + inline bool Valid(Timer::Iterator it) const { return it != clocks_.end(); } + Iterator GetClock(Id const id) const; + void StopClock(Iterator start, Timer::Id id); + + void Sum(Timer::Id id, Timer::Dur secs); + + std::ostream& Stream(void) + { +#ifdef TIMER_TO_COUT + return std::cout; +#else + return stream_; +#endif + } +}; + +static inline void FinishTimer(int signal) +{ + (void)signal; + Timer::Finish(); + exit(0); +} + +#define TIME_ON(id) Timer::Start(id); +#define TIME_OFF(id) Timer::Stop(id); +} +#else // #ifdef TIMING + +namespace x +{ +inline static void nothing(void) {} +#define TIME_ON(id) nothing(); +#define TIME_OFF(id) nothing(); +} +#endif // TIMING + +#endif diff --git a/include/x/vision/tracker.h b/include/x/vision/tracker.h new file mode 100644 index 0000000..64b58d1 --- /dev/null +++ b/include/x/vision/tracker.h @@ -0,0 +1,141 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef JPL_VPP_TRACKER_H_ +#define JPL_VPP_TRACKER_H_ + +#include +#include + +namespace x +{ +class Tracker +{ +public: + + Tracker(); + Tracker(const Camera& cam, + const unsigned int fast_detection_delta, + const bool non_max_supp, + const unsigned int block_half_length, + unsigned int margin, + unsigned int n_feat_min, + int outlier_method, + double outlier_param1, + double outlier_param2); + void setParams(const Camera& cam, + const unsigned int fast_detection_delta, + const bool non_max_supp, + const unsigned int block_half_length, + unsigned int margin, + unsigned int n_feat_min, + int outlier_method, + double outlier_param1, + double outlier_param2); + // Get latest matches + MatchList getMatches() const; + // Get ID of last image processed + int getImgId() const; + // Tracks features from last image into the current one + void track(TiledImage& current_img, + double timestamp, + unsigned int frame_number); + bool checkMatches(); + // Plots matches in the image + static void plotMatches(MatchList matches, TiledImage& img); +private: + MatchList matches_; + int img_id_ = 0; + FeatureList previous_features_; + double previous_timestamp_ = 0.0; + TiledImage previous_img_; + // Tracker params + Camera camera_; + unsigned int fast_detection_delta_ = 9; // the intensity difference threshold for the FAST feature detector + bool non_max_supp_ = true; // if true, non-maximum suppression is applied to detected corners (keypoints) + unsigned int block_half_length_ = 20; // the blocked region for other features to occupy will be of size (2 * block_half_length_ + 1)^2 [px] + unsigned int margin_ = 20; // the margin from the edge of the image within which all detected features must fall into + unsigned int n_feat_min_ = 40; + int outlier_method_ = 8; + double outlier_param1_ = 0.3; + double outlier_param2_ = 0.99; + // Placeholder members from an older version to enable multi-scale pyramid + // detection and tracking + // TODO(Jeff or Roland): implement based on best Mars Heli test results + ImagePyramid img_pyramid_; + unsigned int pyramid_depth_ = 1; // depth of the half-sampling pyramid + // Feature detection. One can avoid detection in the neighborhood of features + // already tracked and specified in the optional argument old_points. + void featureDetection(const TiledImage& img, + FeatureList& new_pts, + double timestamp, + unsigned int frame_number, + FeatureList& old_pts); + // Get the image pyramid given the highest resolution image using down-sampling + void getImagePyramid (const TiledImage& img, ImagePyramid& pyramid); + // Get FAST features for an image pyramid at all levels. Optionally avoids + // detection on the neighborhood of old features. + void getFASTFeaturesPyramid(ImagePyramid& pyramid, + double timestamp, + unsigned int frame_number, + FeatureList& features, + FeatureList& old_features); + // Get fast features for a single image pyramid level. Avoids detection + // on the neighborhood of old features, if applicable. + void getFASTFeaturesImage(TiledImage& img, + double timestamp, + unsigned int frame_number, + unsigned int pyramid_level, + FeatureList& features, + FeatureList& old_features); + // Computes the neighborhood mask for the feature list at input. + void computeNeighborhoodMask(const FeatureList& features, + const cv::Mat& img, + cv::Mat& mask); + // Check whether a feature falls within a certain border at a certain pyramid level + bool isFeatureInsideBorder(Feature& feature, + const TiledImage& img, + unsigned int margin, + unsigned int pyramid_level) const; + // Get scaled image coordinate for a feature + void getScaledPixelCoordinate(const Feature& feature, + PixelCoor& scaled_coord) const; + + // Compare the score of two FAST features (return true if + // feature1.fastScore > feature2.fastScore) + bool compareFASTFeaturesScore(const Feature& feature1, + const Feature& feature2) const; + // Appends candidate features which are far enough from existing features. The + // neighborhood mask needs to be provided beforehands. The order of the + // features matters. + void appendNonNeighborFeatures(TiledImage& img, + FeatureList& features, + FeatureList& candidate_features, + cv::Mat& mask); + void removeOverflowFeatures(TiledImage& img1, + TiledImage& img2, + FeatureList& features1, + FeatureList& features2) const; + // Feature tracking using KLT + void featureTracking(const cv::Mat& img1, + const cv::Mat& img2, + FeatureList& pts1, + FeatureList& pts2, + const double timestamp2, + const unsigned int frame_number2) const; +}; +} +#endif diff --git a/include/x/vision/triangulation.h b/include/x/vision/triangulation.h new file mode 100644 index 0000000..d47616d --- /dev/null +++ b/include/x/vision/triangulation.h @@ -0,0 +1,128 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_TRIANGULATION_H_ +#define X_TRIANGULATION_H_ + +#include +#include + +/** + * A triangulation class. + * + * A triangulation object is defined by a series of camera poses. + * It include methods to compute the 3D coordinates of a feature + * from on a series of 2D image observations taken at the object's + * poses. Methods include Direct Linear Transform and + * Gauss-Newton non-linear least squares. + */ +namespace x +{ +class Triangulation +{ +public: + + /** + * Constructors + */ + Triangulation(const x::QuaternionArray& quat_l, + const x::Vector3Array& pos_l, + const unsigned int max_iter = 10 , + const double term = 0.00001); + + Triangulation(const x::AttitudeList& attitudes, + const x::TranslationList& translations, + const unsigned int max_iter = 10 , + const double term = 0.00001); + + /** + * 2-view linear triangulation. + * + * This function is a wrapper around OpenCV's Direct Linear + * Transform (DLT) implementation. + * + * @param[in] obs1, obs2 Image feature coordinates + * @param[in] i1, i2 Matching indices in the pose vectors + * @param[out] pt_xyz Triangulated cartesian 3D coordinates + */ + void triangulateDlt(const x::Feature& obs1, + const x::Feature& obs2, + const int i1, + const int i2, + Eigen::Vector3d& pt_xyz) const; + + /** + * Non-linear Gauss-Newton triangulation. + * + * This function is an implementation of the non-linear least + * squares multi-view triangulation from the 2007 MSCKF paper. + * The last track observation is assumed to correspond to the + * last pose. + * + * @param[in] track Feature observations (size <= n_poses_) + * @param[out] pt_ivd Triangulated inverse-depth coordinates in + * last frame + */ + void triangulateGN(const x::Track& track, + Eigen::Vector3d& pt_ivd) const; +private: + + /** + * Number of poses. + */ + size_t n_poses_; + + /** + * Camera rotation matrices. + */ + std::vector rotations_; + + /** + * Camera positions. + */ + Vector3Array positions_; + + /** + * Camera projection matrices + */ + std::vector projs_; + + /** + * Maximum number of iterations. + * Only used in non-linear methods. + */ + unsigned int max_iter_; + + /** + * Termination criterion. + * Residual change threshold only used in non-linear methods. + */ + double term_; + + /** + * @brief Convert quaternion and position to projection matrix + * + * @param[in] rot Rotation matrix + * @param[in] pos Position + * @param[out] proj 3x4 projection matrix + */ + void pose2proj(const Eigen::Matrix3d& rot, + const Eigen::Vector3d& pos, + cv::Mat& proj) const; +}; +} + +#endif // X_TRIANGULATION_H_ diff --git a/include/x/vision/types.h b/include/x/vision/types.h new file mode 100644 index 0000000..04c9525 --- /dev/null +++ b/include/x/vision/types.h @@ -0,0 +1,80 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef X_HAT_TYPES_H +#define X_HAT_TYPES_H + +#include +#include +#include + +#include + +#define JPL_VPP_WARN_STREAM(x) std::cerr<<"\033[0;33m[WARN] "<; + using Vector3Array = std::vector; + // Feature match + struct Match + { + Feature previous; + Feature current; + }; + // Pixel coordinates + struct PixelCoor + { + int x; // x pixel coordinate + int y; // y pixel coordinate + }; + typedef std::vector FeatureList; + typedef std::vector Track; + typedef std::vector TrackList; + typedef std::vector MatchList; + typedef std::vector ImagePyramid; + typedef std::vector Keypoints; + + //TODO(jeff) Get rid of this. Use Eigen::Quaternion instead. + struct Attitude + { + double ax = 0; //< quat x + double ay = 0; //< quat y + double az = 0; //< quat z + double aw = 0; //< quat orientation + Attitude(double ax, double ay, double az, double aw) + : ax(ax), ay(ay), az(az), aw(aw) {} + Attitude() = default; + }; + + //TODO(jeff) Get rid of this. Use Vector3 instead. + struct Translation + { + double tx = 0; //< cam position x + double ty = 0; //< cam position y + double tz = 0; //< cam position z + Translation(double tx, double ty, double tz) : tx(tx), ty(ty), tz(tz) {} + Translation(void) = default; + }; + + //TODO(jeff) Get rid of below and use the above + using AttitudeList = std::vector; + using TranslationList = std::vector; + +} // namespace x + +#endif diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..25251ff --- /dev/null +++ b/package.xml @@ -0,0 +1,14 @@ + + x + 1.0.0 + Generic C++ library for vision-based navigation, with + multi-sensor fusion capabilities for thermal, range, solar and GPS + measurements. + Jeff Delaune + + California Institute of Technology + + catkin + cmake_modules + + diff --git a/src/x/ekf/ekf.cpp b/src/x/ekf/ekf.cpp new file mode 100644 index 0000000..73dea3a --- /dev/null +++ b/src/x/ekf/ekf.cpp @@ -0,0 +1,219 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include + +using namespace x; + +Ekf::Ekf(Updater& updater) + : updater_ { updater } +{} + +Ekf::Ekf(const Ekf& ekf) + : propagator_ { ekf.propagator_ } + , state_buffer_ { ekf.state_buffer_ } + , updater_ { ekf.updater_ } +{} + +Updater& Ekf::getUpdaterRef() { + return updater_; +} + +void Ekf::set(const Updater& updater, + Vector3& g, + const ImuNoise& imu_noise, + const size_t state_buffer_sz, + const State& default_state, + const double a_m_max, + const unsigned int delta_seq_imu, + const double time_margin_bfr) { + updater_ = updater; + state_buffer_ = StateBuffer(state_buffer_sz, + default_state, + time_margin_bfr); + propagator_.set(g, imu_noise); + a_m_max_ = a_m_max; + delta_seq_imu_ = delta_seq_imu; +} + +void Ekf::initializeFromState(const State& init_state) { + // Check allocated buffer size is not zero + if (state_buffer_.size() == 0) + throw std::runtime_error("the EKF state buffer must have non-zero size."); + + // Check the size of the dynamic state arrays matches between the + // initialization state and the state buffers. + if (init_state.p_array_.rows() != state_buffer_[0].p_array_.rows() + || init_state.p_array_.cols() != state_buffer_[0].p_array_.cols() + || init_state.q_array_.rows() != state_buffer_[0].q_array_.rows() + || init_state.q_array_.cols() != state_buffer_[0].q_array_.cols() + || init_state.f_array_.rows() != state_buffer_[0].f_array_.rows() + || init_state.f_array_.cols() != state_buffer_[0].f_array_.cols() + || init_state.cov_.rows() != state_buffer_[0].cov_.rows() + || init_state.cov_.cols() != state_buffer_[0].cov_.cols()) { + throw init_bfr_mismatch {}; + } + + // Reset buffer from init_state + state_buffer_.resetFromState(init_state); + init_status_ = kStandBy; +} + +State Ekf::processImu(const double timestamp, + const unsigned int seq, + const Vector3& w_m, + const Vector3& a_m) { + // Seq ID for last IMU message + static unsigned int last_seq = 0; + + // If the filter is not initialized, do nothing and return invalid state. + if (init_status_ == kNotInitialized) + return State(); + + // Reference to last state in buffer + lock(); + State& last_state = state_buffer_.getTailStateRef(); + + // If this is the first IMU measurement, change the filter status, fill the + // IMU data in the first state and return it. + if (init_status_ == kStandBy) { + if (a_m.norm() < a_m_max_) { // accel spike check + last_state.setImu(timestamp, seq, w_m, a_m); + unlock(); + last_seq = seq; + init_status_ = kInitialized; + std::cout << "Initial state:" << std::endl; + std::cout << last_state.toString() << std::endl; + return last_state; + } else { + std::cout << "Accelerometer spike detected at seq_id " << seq + << ". Standing by for next IMU message to initialize." << std::endl; + return State(); + } + } + + // Check if timestamp is after the last state + if (timestamp <= last_state.time_) { + std::cout << "IMU going back in time. Discarding IMU seq_id " << seq << "." + << std::endl; + unlock(); + return State(); + } + + // Warn about missing IMU messages + if (seq != last_seq + delta_seq_imu_) { + std::cout << "Missing IMU messages. Expected seq_id " + << last_seq + delta_seq_imu_ << ", but received seq_id " << seq << "." + << std::endl; + } + last_seq = seq; + + // Remove accelerometer spikes + Vector3 a_m_smoothed; + if (a_m.norm() < a_m_max_) + a_m_smoothed = a_m; + else { + a_m_smoothed = last_state.a_m_; + std::cout << "Accelerometer spike detected at seq_id " << seq + << ". Reading specific force from previous state in buffer." << std::endl; + } + + // Set up propagated state + State& next_state = state_buffer_.enqueueInPlace(); + next_state.setImu(timestamp, seq, w_m, a_m_smoothed); + + // Propagate state and covariance estimates + propagator_.propagateState(last_state, next_state); + propagator_.propagateCovariance(last_state, next_state); + + unlock(); + return next_state; +} + +State Ekf::processUpdateMeasurement() { + // If the filter is not initialized, do nothing and return invalid state. + if (init_status_ == kNotInitialized) + return State(); + + // Get the index of the update state in buffer + lock(); + const int update_state_idx = state_buffer_.closestIdx(updater_.getTime()); + unlock(); + + // If no valid state could be found, do nothing and return invalid state. + if (update_state_idx == kInvalidIndex) + return State(); + + // Get a copy of that state + State update_state = state_buffer_[update_state_idx]; + + // Update state + updater_.update(update_state); + + // Repropagate buffer from update state + bool update_success = false; + lock(); + update_success = repropagateFromStateAtIdx(update_state, update_state_idx); + unlock(); + + // Check if update was applied (can fail if buffer was overwritten) + if (update_success) + return update_state; + else + return State(); +} + +void Ekf::lock() { +#ifdef DUAL_THREAD + mutex_.lock(); +#endif +} + +void Ekf::unlock() { +#ifdef DUAL_THREAD + mutex_.unlock(); +#endif +} + +bool Ekf::repropagateFromStateAtIdx(const State& state, const int idx) { + // Check the buffer slot wasn't overwritten and copy input + if (state_buffer_[idx].getTime() == state.getTime()) + state_buffer_[idx] = state; + else { + std::cout << "Buffer slot at index " << idx << " was overwritten by IMU " + "thread. Update measurement at time " + << std::setprecision(17) << state.getTime() + << " could not be applied." << std::endl; + return false; + } + + // Repropagation loop + int curr_state_idx = idx; + int next_state_idx = state_buffer_.nextIdx(idx); + const int last_state_idx = state_buffer_.getTailIdx(); + while (curr_state_idx != last_state_idx) { + State& state_curr = state_buffer_[curr_state_idx]; + State& state_next = state_buffer_[next_state_idx]; + propagator_.propagateState(state_curr, state_next); + propagator_.propagateCovariance(state_curr, state_next); + curr_state_idx = next_state_idx; + next_state_idx = state_buffer_.nextIdx(next_state_idx); + } + + return true; +} diff --git a/src/x/ekf/propagator.cpp b/src/x/ekf/propagator.cpp new file mode 100644 index 0000000..6343912 --- /dev/null +++ b/src/x/ekf/propagator.cpp @@ -0,0 +1,872 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +using namespace x; + +Propagator::Propagator(const Vector3& g, + const ImuNoise& imu_noise) + : g_ { g } + , imu_noise_ { imu_noise } +{} + +void Propagator::set(const Vector3& g, + const ImuNoise& imu_noise) { + g_ = g; + imu_noise_ = imu_noise; +} + +void Propagator::propagateState(const State& state_0, State& state_1) { + // Copy state estimates that don't vary at IMU rate + state_1.setStaticStatesFrom(state_0); + + // Bias-free IMU measurements + Vector3 e_w_1, e_a_1, e_w_0, e_a_0; + state_1.computeUnbiasedImuMeasurements(e_w_1, e_a_1); + state_0.computeUnbiasedImuMeasurements(e_w_0, e_a_0); + + // First-order quaternion integration + const double dt = state_1.time_ - state_0.time_; + const Matrix4 delta_q = quaternionIntegrator(e_w_0, e_w_1, dt); + state_1.q_.coeffs() = delta_q * state_0.q_.coeffs(); // (x,y,z,w) order + state_1.q_.normalize(); + + // First-order velocity and position integration + const Vector3 dv = ( state_1.q_.toRotationMatrix() * e_a_1 + + state_0.q_.toRotationMatrix() * e_a_0 ) / 2.0; + state_1.v_ = state_0.v_ + (dv + g_) * dt; + state_1.p_ = state_0.p_ + (state_1.v_ + state_0.v_) / 2.0 * dt; +} + +void Propagator::propagateCovariance(const State& state_0, State& state_1) { + // Bias-free IMU measurements + Vector3 e_w_1, e_a_1, e_w_0, e_a_0; + state_1.computeUnbiasedImuMeasurements(e_w_1, e_a_1); + state_0.computeUnbiasedImuMeasurements(e_w_0, e_a_0); + + // Compute coefficients of the discrete state transition matrix + const double dt = state_1.time_ - state_0.time_; + const CoreCovMatrix f_d + = discreteStateTransition(dt, + e_w_1, + e_a_1, + state_1.q_); + + // Construct the discrete process noise covariance matrix + const CoreCovMatrix q_d + = discreteProcessNoiseCov(dt, + state_1.q_, + e_w_1, + e_a_1, + imu_noise_.n_w, + imu_noise_.n_bw, + imu_noise_.n_a, + imu_noise_.n_ba); + + // Covariance propagation using f_d and q_d + propagateCovarianceMatrices(state_0.cov_, f_d, q_d,state_1.cov_); +} + +Matrix4 Propagator::quaternionIntegrator(const Vector3& e_w_0, + const Vector3& e_w_1, + const double dt) const { + // Angular rate matrices + const Matrix4 omega_1 = e_w_1.toOmegaMatrix(); + const Matrix4 omega_0 = e_w_0.toOmegaMatrix(); + const Vector3 e_w_mean = (e_w_1 + e_w_0) / 2.0; + const Matrix4 omega_mean = e_w_mean.toOmegaMatrix(); + + // Matrix exponential Taylor series expansion at 4th order (eq. 130) + int fac = 1; + const Matrix4 a = omega_mean * 0.5 * dt; + Matrix4 a_k = a; + Matrix4 mat_exp = Matrix4::Identity(); + + for (int k = 1; k < 5; k++) { + fac = fac * k; // factorial(k) + mat_exp = mat_exp + a_k / fac; // exponential series + a_k = a_k * a; // increasing exponent k at each iteration + } + + // Return quaternion integation matrix (eq. 131) + return mat_exp + + 1.0 / 48.0 * (omega_1 * omega_0 - omega_0 * omega_1) * dt * dt; +} + +CoreCovMatrix +Propagator::discreteStateTransition(const double dt, + const Vector3& e_w, + const Vector3& e_a, + const Quaternion& q) const { + const Matrix3 w_x = e_w.toCrossMatrix(); + const Matrix3 a_x = e_a.toCrossMatrix(); + const Matrix3 eye3 = Eigen::Matrix::Identity(); + + const Matrix3 c_q = q.toRotationMatrix(); + + const double dt_2_f2 = dt * dt * 0.5; + const double dt_3_f3 = dt_2_f2 * dt / 3.0; + const double dt_4_f4 = dt_3_f3 * dt * 0.25; + const double dt_5_f5 = dt_4_f4 * dt * 0.2; + + const Matrix3 c_q_a_x = c_q * a_x; + const Matrix3 a = c_q_a_x + * (-dt_2_f2 * eye3 + dt_3_f3 * w_x - dt_4_f4 * w_x * w_x); + const Matrix3 b = c_q_a_x + * (dt_3_f3 * eye3 - dt_4_f4 * w_x + dt_5_f5 * w_x * w_x); + const Matrix3 d = -a; + const Matrix3 e = eye3 - dt * w_x + dt_2_f2 * w_x * w_x; + const Matrix3 f = -dt * eye3 + dt_2_f2 * w_x - dt_3_f3 * (w_x * w_x); + const Matrix3 c = c_q_a_x * f; + + // Construct the discrete state transition matrix + CoreCovMatrix + f_d = CoreCovMatrix::Identity(); + + f_d.block<3,3>(kIdxP,kIdxV) = dt * eye3; + f_d.block<3,3>(kIdxP,kIdxQ) = a; + f_d.block<3,3>(kIdxP,kIdxBw) = b; + f_d.block<3,3>(kIdxP,kIdxBa) = -c_q * dt_2_f2; + + f_d.block<3,3>(kIdxV,kIdxQ) = c; + f_d.block<3,3>(kIdxV,kIdxBw) = d; + f_d.block<3,3>(kIdxV,kIdxBa) = -c_q * dt; + + f_d.block<3,3>(kIdxQ,kIdxQ) = e; + f_d.block<3,3>(kIdxQ,kIdxBw) = f; + + return f_d; +} + +void Propagator::propagateCovarianceMatrices( + const Eigen::MatrixXd& cov_0, + const CoreCovMatrix& f_d, + const CoreCovMatrix& q_d, + Eigen::MatrixXd& cov_1) { + // References to covariance blocks (no memory copy). + // Naming based on Eq. 2.15 in xVIO tech report. + const int n = cov_0.rows(); + assert(n > kSizeCoreErr - 1); + const int n_v = n - kSizeCoreErr; // number of non-core states + + const Eigen::Ref + cov_0_ii = cov_0.block(0,0); + const Eigen::Ref + cov_0_iv = cov_0.topRightCorner(kSizeCoreErr,n_v); + const Eigen::Ref + cov_0_vi = cov_0.bottomLeftCorner(n_v,kSizeCoreErr); + const Eigen::Ref + cov_0_vv = cov_0.bottomRightCorner(n_v,n_v); + + Eigen::Ref + cov_1_ii = cov_1.block(0,0); + Eigen::Ref cov_1_iv = cov_1.topRightCorner(kSizeCoreErr,n_v); + Eigen::Ref cov_1_vi = cov_1.bottomLeftCorner(n_v,kSizeCoreErr); + Eigen::Ref cov_1_vv = cov_1.bottomRightCorner(n_v,n_v); + + // Covariance block propagation + // (eq. 2.16-18 in xVIO tech report and eq. 22 in Weiss et al.) + cov_1_ii = f_d * cov_0_ii * f_d.transpose() + q_d; + cov_1_iv = f_d * cov_0_iv; + // Note: doing cov_1_vi = cov_1_iv.transpose() behaves differently. + // Specifically, it causes divergence if the IMU runs for too long (e.g. + // 0.5s) before the first update. It should be equivalent though. There are + // non-symmetric numerical differences in the q_d matrix. Could this be the + // cause? TODO: check if the 2 covariance formulations give the same results + // when q_d is exactly symmetric. + cov_1_vi = cov_0_vi * f_d.transpose(); + cov_1_vv = cov_0_vv; +} + +CoreCovMatrix +Propagator::discreteProcessNoiseCov(const double dt, + const Quaternion& q, + const Vector3& e_w, + const Vector3& e_a, + const double n_w, + const double n_bw, + const double n_a, + const double n_ba) const { + + const double q1 = q.w(), q2 = q.x(), q3 = q.y(), q4 = q.z(); + const double e_w1 = e_w(0), e_w2 = e_w(1), e_w3 = e_w(2); + const double e_a1 = e_a(0), e_a2 = e_a(1), e_a3 = e_a(2); + + const double t343 = dt * dt; + const double t348 = q1 * q4 * 2.0; + const double t349 = q2 * q3 * 2.0; + const double t344 = t348 - t349; + const double t356 = q1 * q3 * 2.0; + const double t357 = q2 * q4 * 2.0; + const double t345 = t356 + t357; + const double t350 = q1 * q1; + const double t351 = q2 * q2; + const double t352 = q3 * q3; + const double t353 = q4 * q4; + const double t346 = t350 + t351 - t352 - t353; + const double t347 = n_a * n_a; + const double t354 = n_a * n_a; + const double t355 = n_a * n_a; + const double t358 = q1 * q2 * 2.0; + const double t359 = t344 * t344; + const double t360 = t345 * t345; + const double t361 = t346 * t346; + const double t363 = e_a2 * t345; + const double t364 = e_a3 * t344; + const double t362 = t363 + t364; + const double t365 = t362 * t362; + const double t366 = t348 + t349; + const double t367 = t350 - t351 + t352 - t353; + const double t368 = q3 * q4 * 2.0; + const double t369 = t356 - t357; + const double t370 = t350 - t351 - t352 + t353; + const double t371 = n_w * n_w; + const double t372 = t358 + t368; + const double t373 = n_w * n_w; + const double t374 = n_w * n_w; + const double t375 = dt * t343 * t346 * t347 * t366 * (1.0 / 3.0); + const double t376 = t358 - t368; + const double t377 = t343 * t346 * t347 * t366 * (1.0 / 2.0); + const double t378 = t366 * t366; + const double t379 = t376 * t376; + const double t380 = e_a1 * t367; + const double t391 = e_a2 * t366; + const double t381 = t380 - t391; + const double t382 = e_a3 * t367; + const double t383 = e_a2 * t376; + const double t384 = t382 + t383; + const double t385 = t367 * t367; + const double t386 = e_a1 * t376; + const double t387 = e_a3 * t366; + const double t388 = t386 + t387; + const double t389 = e_a2 * t370; + const double t407 = e_a3 * t372; + const double t390 = t389 - t407; + const double t392 = e_a1 * t372; + const double t393 = e_a2 * t369; + const double t394 = t392 + t393; + const double t395 = e_a1 * t370; + const double t396 = e_a3 * t369; + const double t397 = t395 + t396; + const double t398 = n_ba * n_ba; + const double t399 = n_ba * n_ba; + const double t400 = n_ba * n_ba; + const double t401 = dt * t343 * t345 * t355 * t370 * (1.0 / 3.0); + const double t402 = t401 - dt * t343 * t346 * t347 * t369 * (1.0 / 3.0) + - dt * t343 * t344 * t354 * t372 * (1.0 / 3.0); + const double t403 = dt * t343 * t354 * t367 * t372 * (1.0 / 3.0); + const double t404 = t403 - dt * t343 * t347 * t366 * t369 * (1.0 / 3.0) + - dt * t343 * t355 * t370 * t376 * (1.0 / 3.0); + const double t405 = t343 * t345 * t355 * t370 * (1.0 / 2.0); + const double t406 = dt * t343 * t362 * t373 * t397 * (1.0 / 6.0); + const double t421 = t343 * t346 * t347 * t369 * (1.0 / 2.0); + const double t422 = dt * t343 * t362 * t371 * t394 * (1.0 / 6.0); + const double t423 = t343 * t344 * t354 * t372 * (1.0 / 2.0); + const double t424 = dt * t343 * t362 * t374 * t390 * (1.0 / 6.0); + const double t408 = t405 + t406 - t421 - t422 - t423 - t424; + const double t409 = t343 * t354 * t367 * t372 * (1.0 / 2.0); + const double t410 = dt * t343 * t374 * t384 * t390 * (1.0 / 6.0); + const double t411 = dt * t343 * t373 * t388 * t397 * (1.0 / 6.0); + const double t463 = t343 * t355 * t370 * t376 * (1.0 / 2.0); + const double t464 = t343 * t347 * t366 * t369 * (1.0 / 2.0); + const double t465 = dt * t343 * t371 * t381 * t394 * (1.0 / 6.0); + const double t412 = t409 + t410 + t411 - t463 - t464 - t465; + const double t413 = t369 * t369; + const double t414 = t372 * t372; + const double t415 = t370 * t370; + const double t416 = t343 * t354 * t359 * (1.0 / 2.0); + const double t417 = t343 * t355 * t360 * (1.0 / 2.0); + const double t418 = t343 * t347 * t361 * (1.0 / 2.0); + const double t419 = t416 + t417 + t418 - dt * t343 * t365 * t371 * (1.0 / 6.0) + - dt * t343 * t365 * t373 * (1.0 / 6.0) + - dt * t343 * t365 * t374 * (1.0 / 6.0); + const double t453 = t343 * t344 * t354 * t367 * (1.0 / 2.0); + const double t454 = t343 * t345 * t355 * t376 * (1.0 / 2.0); + const double t420 = t377 - t453 - t454; + const double t426 = e_w2 * t362; + const double t427 = e_w3 * t362; + const double t425 = t426 - t427; + const double t428 = dt * t365; + const double t429 = e_w1 * e_w1; + const double t430 = e_w2 * e_w2; + const double t431 = e_w3 * e_w3; + const double t432 = t430 + t431; + const double t433 = t362 * t432; + const double t434 = e_w1 * t343 * t365; + const double t435 = t429 + t431; + const double t436 = t362 * t435; + const double t443 = e_w2 * e_w3 * t362; + const double t437 = t433 + t436 - t443; + const double t438 = e_w1 * t362 * t394; + const double t511 = e_w1 * t362 * t397; + const double t439 = t438 - t511; + const double t440 = t343 * t439 * (1.0 / 2.0); + const double t441 = t429 + t430; + const double t442 = t362 * t441; + const double t444 = t390 * t432; + const double t445 = e_w2 * t394; + const double t446 = e_w3 * t397; + const double t447 = t445 + t446; + const double t448 = e_w1 * e_w2 * t362; + const double t449 = e_w1 * e_w3 * t362; + const double t450 = e_w1 * e_w3 * t362 * (1.0 / 2.0); + const double t451 = dt * t362; + const double t452 = e_w1 * t343 * t362 * (1.0 / 2.0); + const double t455 = dt * t343 * t362 * t374 * t384 * (1.0 / 6.0); + const double t456 = t343 * t347 * t378 * (1.0 / 2.0); + const double t457 = t343 * t355 * t379 * (1.0 / 2.0); + const double t458 = t381 * t381; + const double t459 = t384 * t384; + const double t460 = t343 * t354 * t385 * (1.0 / 2.0); + const double t461 = t388 * t388; + const double t462 = t456 + t457 + t460 - dt * t343 * t371 * t458 * (1.0 / 6.0) + - dt * t343 * t374 * t459 * (1.0 / 6.0) + - dt * t343 * t373 * t461 * (1.0 / 6.0); + const double t466 = t433 + t442 - t443; + const double t467 = e_w1 * t362 * t388; + const double t468 = e_w1 * t362 * t381; + const double t469 = t467 + t468; + const double t470 = t343 * t469 * (1.0 / 2.0); + const double t471 = t384 * t432; + const double t472 = e_w2 * t381; + const double t479 = e_w3 * t388; + const double t473 = t472 - t479; + const double t474 = -t433 + t448 + t449; + const double t475 = dt * t343 * t346 * t366 * t398 * (1.0 / 3.0); + const double t476 = dt * t346 * t347 * t366; + const double t477 = e_w2 * e_w3 * t381; + const double t492 = t388 * t435; + const double t478 = t471 + t477 - t492; + const double t480 = t472 - t479; + const double t481 = e_w1 * e_w3 * t381; + const double t482 = e_w1 * e_w2 * t388; + const double t483 = t471 + t481 + t482; + const double t484 = e_w2 * e_w3 * t388; + const double t486 = t381 * t441; + const double t485 = t471 + t484 - t486; + const double t487 = t394 * t441; + const double t488 = e_w2 * e_w3 * t397; + const double t489 = t444 + t487 + t488; + const double t490 = t397 * t435; + const double t491 = e_w2 * e_w3 * t394; + const double t493 = e_w1 * t381 * t397; + const double t541 = e_w1 * t388 * t394; + const double t494 = t493 - t541; + const double t495 = t343 * t494 * (1.0 / 2.0); + const double t496 = e_w1 * e_w2 * t397; + const double t527 = e_w1 * e_w3 * t394; + const double t497 = t444 + t496 - t527; + const double t498 = e_w2 * e_w3 * t381 * (1.0 / 2.0); + const double t499 = e_w1 * t343 * t381 * (1.0 / 2.0); + const double t500 = t384 * t432 * (1.0 / 2.0); + const double t501 = e_w2 * e_w3 * t388 * (1.0 / 2.0); + const double t502 = n_bw * n_bw; + const double t503 = n_bw * n_bw; + const double t504 = t343 * t347 * t413 * (1.0 / 2.0); + const double t505 = t343 * t354 * t414 * (1.0 / 2.0); + const double t506 = t397 * t397; + const double t507 = t390 * t390; + const double t508 = t343 * t355 * t415 * (1.0 / 2.0); + const double t509 = t394 * t394; + const double t510 = t504 + t505 + t508 - dt * t343 * t373 * t506 * (1.0 / 6.0) + - dt * t343 * t371 * t509 * (1.0 / 6.0) + - dt * t343 * t374 * t507 * (1.0 / 6.0); + const double t512 = -t444 + t490 + t491; + const double t513 = t397 * t437 * (1.0 / 2.0); + const double t514 = t362 * t394 * t429; + const double t515 = dt * t362 * t397; + const double t516 = t362 * t489 * (1.0 / 2.0); + const double t517 = t394 * t466 * (1.0 / 2.0); + const double t518 = t362 * t397 * t429; + const double t519 = t516 + t517 + t518; + const double t520 = dt * t362 * t394; + const double t521 = t440 + t520 - dt * t343 * t519 * (1.0 / 3.0); + const double t522 = t371 * t521; + const double t523 = t362 * t447; + const double t524 = t390 * t425; + const double t525 = t523 + t524; + const double t526 = t343 * t525 * (1.0 / 2.0); + const double t528 = t425 * t447; + const double t529 = t390 * t474 * (1.0 / 2.0); + const double t530 = t528 + t529 - t362 * t497 * (1.0 / 2.0); + const double t531 = dt * t343 * t530 * (1.0 / 3.0); + const double t532 = dt * t362 * t390; + const double t533 = t526 + t531 + t532; + const double t534 = t374 * t533; + const double t535 = dt * t343 * t345 * t370 * t400 * (1.0 / 3.0); + const double t536 = dt * t345 * t355 * t370; + const double t537 = t381 * t489 * (1.0 / 2.0); + const double t538 = t388 * t397 * t429; + const double t539 = t537 + t538 - t394 * t485 * (1.0 / 2.0); + const double t540 = dt * t343 * t539 * (1.0 / 3.0); + const double t542 = t495 + t540 - dt * t381 * t394; + const double t543 = t388 * t512 * (1.0 / 2.0); + const double t544 = t381 * t394 * t429; + const double t545 = t543 + t544 - t397 * t478 * (1.0 / 2.0); + const double t546 = dt * t343 * t545 * (1.0 / 3.0); + const double t547 = t495 + t546 - dt * t388 * t397; + const double t548 = t373 * t547; + const double t549 = t384 * t447; + const double t550 = t549 - t390 * t473; + const double t551 = t343 * t550 * (1.0 / 2.0); + const double t552 = t384 * t497 * (1.0 / 2.0); + const double t553 = t390 * t483 * (1.0 / 2.0); + const double t554 = t447 * t473; + const double t555 = t552 + t553 + t554; + const double t556 = dt * t384 * t390; + const double t557 = t551 + t556 - dt * t343 * t555 * (1.0 / 3.0); + const double t558 = dt * t343 * t367 * t372 * t399 * (1.0 / 3.0); + const double t559 = dt * t354 * t367 * t372; + const double t560 = t548 + t558 + t559 - t371 * t542 - t374 * t557 + - dt * t347 * t366 * t369 - dt * t355 * t370 * t376 + - dt * t343 * t366 * t369 * t398 * (1.0 / 3.0) + - dt * t343 * t370 * t376 * t400 * (1.0 / 3.0); + const double t561 = e_w1 * t343 * t394 * t397; + const double t562 = e_w1 * t343 * t397 * (1.0 / 2.0); + const double t563 = n_bw * n_bw; + const double t564 = dt * t343 * t362 * t374 * (1.0 / 6.0); + const double t565 = dt * t343 * t374 * t390 * (1.0 / 6.0); + const double t566 = e_w1 * e_w2 * t362 * (1.0 / 2.0); + const double t567 = -t433 + t450 + t566; + const double t568 = dt * t343 * t567 * (1.0 / 3.0); + const double t569 = t343 * t425 * (1.0 / 2.0); + const double t570 = t451 + t568 + t569; + const double t571 = dt * t343 * t362 * t373 * t432 * (1.0 / 6.0); + const double t572 = dt * t343 * t362 * t371 * t432 * (1.0 / 6.0); + const double t573 = t571 + t572 - t374 * t570; + const double t574 = e_w1 * e_w2 * t397 * (1.0 / 2.0); + const double t575 = t444 + t574 - e_w1 * e_w3 * t394 * (1.0 / 2.0); + const double t576 = t343 * t447 * (1.0 / 2.0); + const double t577 = dt * t390; + const double t578 = t576 + t577 - dt * t343 * t575 * (1.0 / 3.0); + const double t579 = dt * t343 * t371 * t394 * t432 * (1.0 / 6.0); + const double t580 = t579 - t374 * t578 + - dt * t343 * t373 * t397 * t432 * (1.0 / 6.0); + const double t581 = dt * t343 * t373 * t388 * (1.0 / 6.0); + const double t582 = t362 * t432 * (1.0 / 2.0); + const double t583 = e_w2 * e_w3 * t362 * (1.0 / 2.0); + const double t584 = t362 * t429; + const double t585 = t583 + t584; + const double t586 = e_w3 * t473; + const double t587 = e_w1 * e_w2 * t384 * (1.0 / 2.0); + const double t588 = t586 + t587; + const double t589 = dt * t343 * t588 * (1.0 / 3.0); + const double t590 = t374 * (t589 - e_w3 * t343 * t384 * (1.0 / 2.0)); + const double t591 = t388 * t429; + const double t592 = t498 + t591; + const double t593 = dt * t343 * t592 * (1.0 / 3.0); + const double t594 = t499 + t593; + const double t595 = -t492 + t498 + t500; + const double t596 = dt * t343 * t595 * (1.0 / 3.0); + const double t597 = dt * t388; + const double t598 = -t499 + t596 + t597; + const double t599 = t590 - t371 * t594 - t373 * t598; + const double t600 = t397 * t429; + const double t601 = e_w2 * e_w3 * t394 * (1.0 / 2.0); + const double t602 = e_w1 * t343 * t394 * (1.0 / 2.0); + const double t603 = e_w3 * t447; + const double t604 = t603 - e_w1 * e_w2 * t390 * (1.0 / 2.0); + const double t605 = dt * t343 * t604 * (1.0 / 3.0); + const double t606 = e_w3 * t343 * t390 * (1.0 / 2.0); + const double t607 = t605 + t606; + const double t608 = t374 * t607; + const double t609 = t390 * t432 * (1.0 / 2.0); + const double t610 = dt * t397; + const double t611 = t430 * (1.0 / 2.0); + const double t612 = t431 * (1.0 / 2.0); + const double t613 = t611 + t612; + const double t614 = e_w1 * t343 * (1.0 / 2.0); + const double t615 = dt * t343 * t362 * t371 * (1.0 / 6.0); + const double t616 = dt * t343 * t371 * t381 * (1.0 / 6.0); + const double t617 = dt * t343 * t371 * t394 * (1.0 / 6.0); + const double t618 = e_w2 * t425; + const double t619 = t450 + t618; + const double t620 = dt * t343 * t619 * (1.0 / 3.0); + const double t621 = e_w2 * t343 * t362 * (1.0 / 2.0); + const double t622 = t620 + t621; + const double t623 = dt * t343 * t585 * (1.0 / 3.0); + const double t624 = t381 * t429; + const double t625 = t501 + t624; + const double t626 = dt * t343 * t625 * (1.0 / 3.0); + const double t627 = e_w1 * t343 * t388 * (1.0 / 2.0); + const double t628 = e_w2 * t473; + const double t629 = t628 - e_w1 * e_w3 * t384 * (1.0 / 2.0); + const double t630 = dt * t343 * t629 * (1.0 / 3.0); + const double t631 = t630 - e_w2 * t343 * t384 * (1.0 / 2.0); + const double t632 = -t486 + t500 + t501; + const double t633 = dt * t343 * t632 * (1.0 / 3.0); + const double t634 = dt * t381; + const double t635 = t627 + t633 + t634; + const double t636 = e_w2 * t447; + const double t637 = e_w1 * e_w3 * t390 * (1.0 / 2.0); + const double t638 = t636 + t637; + const double t639 = dt * t343 * t638 * (1.0 / 3.0); + const double t640 = e_w2 * t343 * t390 * (1.0 / 2.0); + const double t641 = t639 + t640; + const double t642 = t394 * t429; + const double t643 = e_w2 * e_w3 * t397 * (1.0 / 2.0); + const double t644 = t487 + t609 + t643; + const double t645 = dt * t343 * t644 * (1.0 / 3.0); + const double t646 = t562 + t645 - dt * t394; + const double t647 = t371 * t646; + const double t648 = e_w2 * t343 * (1.0 / 2.0); + const double t649 = dt * e_w1 * e_w3 * t343 * (1.0 / 6.0); + const double t650 = t648 + t649; + const double t651 = t374 * t650; + const double t652 = t651 - dt * t343 * t371 * t613 * (1.0 / 3.0); + const double t653 = dt * e_w2 * e_w3 * t343 * (1.0 / 6.0); + const double t654 = t614 + t653; + const double t655 = t371 * t654; + const double t656 = dt * t343 * t397 * t563 * (1.0 / 6.0); + const double t657 = dt * e_w1 * t343 * t563 * (1.0 / 6.0); + const double t658 = dt * t343 * t369 * t398 * (1.0 / 6.0); + const double t659 = t343 * t369 * t398 * (1.0 / 2.0); + const double t660 = dt * t343 * t344 * t399 * (1.0 / 6.0); + const double t661 = t343 * t344 * t399 * (1.0 / 2.0); + const double t662 = dt * t343 * t376 * t400 * (1.0 / 6.0); + const double t663 = t343 * t376 * t400 * (1.0 / 2.0); + + CoreCovMatrix q_d = CoreCovMatrix::Zero(); + q_d(kIdxP + 0, kIdxP + 0) = dt * t343 * t347 * t361 + * (1.0 / 3.0) + dt * t343 * t354 * t359 * (1.0 / 3.0) + + dt * t343 * t355 * t360 * (1.0 / 3.0); + q_d(kIdxP + 0, kIdxP + 1) = t375 + - dt * t343 * t345 * t355 * (t358 - q3 * q4 * 2.0) * (1.0 / 3.0) + - dt * t343 * t344 * t354 * t367 * (1.0 / 3.0); + q_d(kIdxP + 0, kIdxP + 2) = t402; + q_d(kIdxP + 0, kIdxV + 0) = t419; + q_d(kIdxP + 0, kIdxV + 1) = t420; + q_d(kIdxP + 0, kIdxV + 2) = t408; + q_d(kIdxP + 0, kIdxQ + 0) = t564; + q_d(kIdxP + 0, kIdxQ + 2) = t615; + q_d(kIdxP + 0, kIdxBa + 0) = dt * t343 * t346 * t398 + * (-1.0 / 6.0); + q_d(kIdxP + 0, kIdxBa + 1) = t660; + q_d(kIdxP + 0, kIdxBa + 2) = dt * t343 * t345 * t400 + * (-1.0 / 6.0); + q_d(kIdxP + 1, kIdxP + 0) = t375 + - dt * t343 * t344 * t354 * t367 * (1.0 / 3.0) + - dt * t343 * t345 * t355 * t376 * (1.0 / 3.0); + q_d(kIdxP + 1, kIdxP + 1) = dt * t343 * t347 * t378 + * (1.0 / 3.0) + dt * t343 * t355 * t379 * (1.0 / 3.0) + + dt * t343 * t354 * t385 * (1.0 / 3.0); + q_d(kIdxP + 1, kIdxP + 2) = t404; + q_d(kIdxP + 1, kIdxV + 0) = t377 + t455 + - t343 * t344 * t354 * t367 * (1.0 / 2.0) + - t343 * t345 * t355 * t376 * (1.0 / 2.0) + - dt * t343 * t362 * t371 * t381 * (1.0 / 6.0) + - dt * t343 * t362 * t373 * t388 * (1.0 / 6.0); + q_d(kIdxP + 1, kIdxV + 1) = t462; + q_d(kIdxP + 1, kIdxV + 2) = t412; + q_d(kIdxP + 1, kIdxQ + 0) = dt * t343 * t374 * t384 + * (-1.0 / 6.0); + q_d(kIdxP + 1, kIdxQ + 1) = t581; + q_d(kIdxP + 1, kIdxQ + 2) = t616; + q_d(kIdxP + 1, kIdxBa + 0) = dt * t343 * t366 * t398 + * (-1.0 / 6.0); + q_d(kIdxP + 1, kIdxBa + 1) = dt * t343 * t367 * t399 + * (-1.0 / 6.0); + q_d(kIdxP + 1, kIdxBa + 2) = t662; + q_d(kIdxP + 2, kIdxP + 0) = t402; + q_d(kIdxP + 2, kIdxP + 1) = t404; + q_d(kIdxP + 2, kIdxP + 2) = dt * t343 * t347 * t413 + * (1.0 / 3.0) + dt * t343 * t354 * t414 * (1.0 / 3.0) + + dt * t343 * t355 * t415 * (1.0 / 3.0); + q_d(kIdxP + 2, kIdxV + 0) = t408; + q_d(kIdxP + 2, kIdxV + 1) = t412; + q_d(kIdxP + 2, kIdxV + 2) = t510; + q_d(kIdxP + 2, kIdxQ + 0) = t565; + q_d(kIdxP + 2, kIdxQ + 1) = dt * t343 * t373 * t397 + * (-1.0 / 6.0); + q_d(kIdxP + 2, kIdxQ + 2) = t617; + q_d(kIdxP + 2, kIdxBa + 0) = t658; + q_d(kIdxP + 2, kIdxBa + 1) = dt * t343 * t372 * t399 + * (-1.0 / 6.0); + q_d(kIdxP + 2, kIdxBa + 2) = dt * t343 * t370 * t400 + * (-1.0 / 6.0); + q_d(kIdxV + 0, kIdxP + 0) = t419; + q_d(kIdxV + 0, kIdxP + 1) = t420; + q_d(kIdxV + 0, kIdxP + 2) = t408; + q_d(kIdxV + 0, kIdxV + 0) = + t374 + * (t428 + t343 * t362 * t425 + + dt * t343 * (t362 * (t448 + t449 - t362 * t432) + t425 * t425) + * (1.0 / 3.0)) + + t373 + * (t428 - t434 + + dt * t343 * (t365 * t429 - t362 * t437) * (1.0 / 3.0)) + + t371 + * (t428 + t434 + + dt * t343 + * (t365 * t429 - t362 * (t433 + t442 - e_w2 * e_w3 * t362)) + * (1.0 / 3.0)) + dt * t347 * t361 + dt * t354 * t359 + + dt * t355 * t360 + dt * t343 * t359 * t399 * (1.0 / 3.0) + + dt * t343 * t361 * t398 * (1.0 / 3.0) + + dt * t343 * t360 * t400 * (1.0 / 3.0); + q_d(kIdxV + 0, kIdxV + 1) = t475 + t476 + - dt * t344 * t354 * t367 - dt * t345 * t355 * t376 + - dt * t343 * t344 * t367 * t399 * (1.0 / 3.0) + - dt * t343 * t345 * t376 * t400 * (1.0 / 3.0); + q_d(kIdxV + 0, kIdxV + 2) = t522 + t534 + t535 + t536 + - t373 + * (t440 + t515 + - dt * t343 + * (t513 + t514 + + t362 * (t490 + t491 - t390 * t432) * (1.0 / 2.0)) + * (1.0 / 3.0)) - dt * t346 * t347 * t369 + - dt * t344 * t354 * t372 - dt * t343 * t346 * t369 * t398 * (1.0 / 3.0) + - dt * t343 * t344 * t372 * t399 * (1.0 / 3.0); + q_d(kIdxV + 0, kIdxQ + 0) = t573; + q_d(kIdxV + 0, kIdxQ + 2) = -t371 + * (t451 + t452 + - dt * t343 * (t442 + t582 - e_w2 * e_w3 * t362 * (1.0 / 2.0)) + * (1.0 / 3.0)) - t374 * t622 + + t373 * (t452 - dt * t343 * t585 * (1.0 / 3.0)); + q_d(kIdxV + 0, kIdxBw + 0) = dt * t343 * t362 * t502 + * (-1.0 / 6.0); + q_d(kIdxV + 0, kIdxBw + 2) = dt * t343 * t362 * t503 + * (-1.0 / 6.0); + q_d(kIdxV + 0, kIdxBa + 0) = t343 * t346 * t398 + * (-1.0 / 2.0); + q_d(kIdxV + 0, kIdxBa + 1) = t661; + q_d(kIdxV + 0, kIdxBa + 2) = t343 * t345 * t400 + * (-1.0 / 2.0); + q_d(kIdxV + 1, kIdxP + 0) = t377 - t453 - t454 + t455 + - dt * t343 * t362 * t371 * t381 * (1.0 / 6.0) + - dt * t343 * t362 * t373 * t388 * (1.0 / 6.0); + q_d(kIdxV + 1, kIdxP + 1) = t462; + q_d(kIdxV + 1, kIdxP + 2) = t412; + q_d(kIdxV + 1, kIdxV + 0) = t475 + t476 + - t374 + * (t343 * (t384 * t425 - t362 * t473) * (1.0 / 2.0) + - dt * t343 + * (t362 * t483 * (1.0 / 2.0) - t384 * t474 * (1.0 / 2.0) + + t425 * t473) * (1.0 / 3.0) + dt * t362 * t384) + + t371 + * (t470 + dt * t362 * t381 + + dt * t343 + * (t362 * t485 * (1.0 / 2.0) - t381 * t466 * (1.0 / 2.0) + + t362 * t388 * t429) * (1.0 / 3.0)) + + t373 + * (-t470 + dt * t362 * t388 + + dt * t343 + * (t388 * t437 * (-1.0 / 2.0) + t362 * t478 * (1.0 / 2.0) + + t362 * t381 * t429) * (1.0 / 3.0)) + - dt * t344 * t354 * t367 - dt * t345 * t355 * t376 + - dt * t343 * t344 * t367 * t399 * (1.0 / 3.0) + - dt * t343 * t345 * t376 * t400 * (1.0 / 3.0); + q_d(kIdxV + 1, kIdxV + 1) = -t374 + * (-dt * t459 + dt * t343 * (t384 * t483 - t480 * t480) * (1.0 / 3.0) + + t343 * t384 * t473) + + t373 + * (dt * t461 + dt * t343 * (t388 * t478 + t429 * t458) * (1.0 / 3.0) + - e_w1 * t343 * t381 * t388) + + t371 + * (dt * t458 + dt * t343 * (t381 * t485 + t429 * t461) * (1.0 / 3.0) + + e_w1 * t343 * t381 * t388) + dt * t347 * t378 + dt * t355 * t379 + + dt * t354 * t385 + dt * t343 * t378 * t398 * (1.0 / 3.0) + + dt * t343 * t385 * t399 * (1.0 / 3.0) + + dt * t343 * t379 * t400 * (1.0 / 3.0); + q_d(kIdxV + 1, kIdxV + 2) = t560; + q_d(kIdxV + 1, kIdxQ + 0) = -t374 + * (-dt * t384 + t343 * t473 * (1.0 / 2.0) + + dt * t343 + * (t471 + e_w1 * e_w2 * t388 * (1.0 / 2.0) + + e_w1 * e_w3 * t381 * (1.0 / 2.0)) * (1.0 / 3.0)) + + dt * t343 * t371 * t381 * t432 * (1.0 / 6.0) + + dt * t343 * t373 * t388 * t432 * (1.0 / 6.0); + q_d(kIdxV + 1, kIdxQ + 1) = t599; + q_d(kIdxV + 1, kIdxQ + 2) = -t374 * t631 - t371 * t635 + - t373 * (t626 - e_w1 * t343 * t388 * (1.0 / 2.0)); + q_d(kIdxV + 1, kIdxBw + 0) = dt * t343 * t384 * t502 + * (1.0 / 6.0); + q_d(kIdxV + 1, kIdxBw + 1) = dt * t343 * t388 * t563 + * (-1.0 / 6.0); + q_d(kIdxV + 1, kIdxBw + 2) = dt * t343 * t381 * t503 + * (-1.0 / 6.0); + q_d(kIdxV + 1, kIdxBa + 0) = t343 * t366 * t398 + * (-1.0 / 2.0); + q_d(kIdxV + 1, kIdxBa + 1) = t343 * t367 * t399 + * (-1.0 / 2.0); + q_d(kIdxV + 1, kIdxBa + 2) = t663; + q_d(kIdxV + 2, kIdxP + 0) = t408; + q_d(kIdxV + 2, kIdxP + 1) = t412; + q_d(kIdxV + 2, kIdxP + 2) = t510; + q_d(kIdxV + 2, kIdxV + 0) = t522 + t534 + t535 + t536 + - t373 + * (t440 + t515 + - dt * t343 * (t513 + t514 + t362 * t512 * (1.0 / 2.0)) + * (1.0 / 3.0)) - dt * t346 * t347 * t369 + - dt * t344 * t354 * t372 - dt * t343 * t346 * t369 * t398 * (1.0 / 3.0) + - dt * t343 * t344 * t372 * t399 * (1.0 / 3.0); + q_d(kIdxV + 2, kIdxV + 1) = t560; + q_d(kIdxV + 2, kIdxV + 2) = -t371 + * (t561 - dt * t509 + + dt * t343 * (t394 * t489 - t429 * t506) * (1.0 / 3.0)) + + t373 + * (t561 + dt * t506 + - dt * t343 * (t397 * t512 - t429 * t509) * (1.0 / 3.0)) + + t374 + * (dt * t507 - dt * t343 * (t390 * t497 - t447 * t447) * (1.0 / 3.0) + + t343 * t390 * t447) + dt * t347 * t413 + dt * t354 * t414 + + dt * t355 * t415 + dt * t343 * t398 * t413 * (1.0 / 3.0) + + dt * t343 * t399 * t414 * (1.0 / 3.0) + + dt * t343 * t400 * t415 * (1.0 / 3.0); + q_d(kIdxV + 2, kIdxQ + 0) = t580; + q_d(kIdxV + 2, kIdxQ + 1) = t608 + + t371 + * (dt * t343 * (t600 - e_w2 * e_w3 * t394 * (1.0 / 2.0)) * (1.0 / 3.0) + - e_w1 * t343 * t394 * (1.0 / 2.0)) + + t373 + * (t602 + t610 + - dt * t343 * (t490 + t601 - t390 * t432 * (1.0 / 2.0)) + * (1.0 / 3.0)); + q_d(kIdxV + 2, kIdxQ + 2) = t647 - t374 * t641 + - t373 + * (t562 + + dt * t343 * (t642 - e_w2 * e_w3 * t397 * (1.0 / 2.0)) + * (1.0 / 3.0)); + q_d(kIdxV + 2, kIdxBw + 0) = dt * t343 * t390 * t502 + * (-1.0 / 6.0); + q_d(kIdxV + 2, kIdxBw + 1) = t656; + q_d(kIdxV + 2, kIdxBw + 2) = dt * t343 * t394 * t503 + * (-1.0 / 6.0); + q_d(kIdxV + 2, kIdxBa + 0) = t659; + q_d(kIdxV + 2, kIdxBa + 1) = t343 * t372 * t399 + * (-1.0 / 2.0); + q_d(kIdxV + 2, kIdxBa + 2) = t343 * t370 * t400 + * (-1.0 / 2.0); + q_d(kIdxQ + 0, kIdxP + 0) = t564; + q_d(kIdxQ + 0, kIdxP + 2) = t565; + q_d(kIdxQ + 0, kIdxV + 0) = t573; + q_d(kIdxQ + 0, kIdxV + 2) = t580; + q_d(kIdxQ + 0, kIdxQ + 0) = t374 + * (dt - dt * t343 * t432 * (1.0 / 3.0)) + dt * t343 * t502 * (1.0 / 3.0); + q_d(kIdxQ + 0, kIdxQ + 2) = t652; + q_d(kIdxQ + 0, kIdxBw + 0) = t343 * t502 * (-1.0 / 2.0); + q_d(kIdxQ + 1, kIdxP + 0) = dt * t343 * t362 * t373 + * (1.0 / 6.0); + q_d(kIdxQ + 1, kIdxP + 1) = t581; + q_d(kIdxQ + 1, kIdxP + 2) = dt * t343 * t373 * t397 + * (-1.0 / 6.0); + q_d(kIdxQ + 1, kIdxV + 0) = -t371 * (t452 + t623) + - t374 + * (dt * t343 * (t566 - e_w3 * t425) * (1.0 / 3.0) + - e_w3 * t343 * t362 * (1.0 / 2.0)) + + t373 * (-t451 + t452 + dt * t343 * (t436 + t582 - t583) * (1.0 / 3.0)); + q_d(kIdxQ + 1, kIdxV + 1) = t599; + q_d(kIdxQ + 1, kIdxV + 2) = t608 + + t373 * (t602 + t610 - dt * t343 * (t490 + t601 - t609) * (1.0 / 3.0)) + - t371 * (t602 - dt * t343 * (t600 - t601) * (1.0 / 3.0)); + q_d(kIdxQ + 1, kIdxQ + 0) = -t374 + * (e_w3 * t343 * (1.0 / 2.0) - dt * e_w1 * e_w2 * t343 * (1.0 / 6.0)) + - dt * t343 * t373 * t613 * (1.0 / 3.0); + q_d(kIdxQ + 1, kIdxQ + 1) = t373 + * (dt - dt * t343 * t435 * (1.0 / 3.0)) + dt * t343 * t563 * (1.0 / 3.0) + + dt * t343 * t371 * t429 * (1.0 / 3.0) + + dt * t343 * t374 * t431 * (1.0 / 3.0); + q_d(kIdxQ + 1, kIdxQ + 2) = t655 + - t373 * (t614 - dt * e_w2 * e_w3 * t343 * (1.0 / 6.0)) + - dt * e_w2 * e_w3 * t343 * t374 * (1.0 / 3.0); + q_d(kIdxQ + 1, kIdxBw + 0) = dt * e_w3 * t343 * t502 + * (1.0 / 6.0); + q_d(kIdxQ + 1, kIdxBw + 1) = t343 * t563 * (-1.0 / 2.0); + q_d(kIdxQ + 1, kIdxBw + 2) = dt * e_w1 * t343 * t503 + * (-1.0 / 6.0); + q_d(kIdxQ + 2, kIdxP + 0) = t615; + q_d(kIdxQ + 2, kIdxP + 1) = t616; + q_d(kIdxQ + 2, kIdxP + 2) = t617; + q_d(kIdxQ + 2, kIdxV + 0) = -t374 * t622 + - t371 * (t451 + t452 - dt * t343 * (t442 + t582 - t583) * (1.0 / 3.0)) + + t373 * (t452 - t623); + q_d(kIdxQ + 2, kIdxV + 1) = -t374 * t631 - t371 * t635 + - t373 * (t626 - t627); + q_d(kIdxQ + 2, kIdxV + 2) = t647 - t374 * t641 + - t373 * (t562 + dt * t343 * (t642 - t643) * (1.0 / 3.0)); + q_d(kIdxQ + 2, kIdxQ + 0) = t652; + q_d(kIdxQ + 2, kIdxQ + 1) = t655 - t373 * (t614 - t653) + - dt * e_w2 * e_w3 * t343 * t374 * (1.0 / 3.0); + q_d(kIdxQ + 2, kIdxQ + 2) = t371 + * (dt - dt * t343 * t441 * (1.0 / 3.0)) + dt * t343 * t503 * (1.0 / 3.0) + + dt * t343 * t373 * t429 * (1.0 / 3.0) + + dt * t343 * t374 * t430 * (1.0 / 3.0); + q_d(kIdxQ + 2, kIdxBw + 0) = dt * e_w2 * t343 * t502 + * (-1.0 / 6.0); + q_d(kIdxQ + 2, kIdxBw + 1) = t657; + q_d(kIdxQ + 2, kIdxBw + 2) = t343 * t503 * (-1.0 / 2.0); + q_d(kIdxBw + 0, kIdxV + 0) = dt * t343 * t362 * t502 + * (-1.0 / 6.0); + q_d(kIdxBw + 0, kIdxV + 2) = dt * t343 * t390 * t502 + * (-1.0 / 6.0); + q_d(kIdxBw + 0, kIdxQ + 0) = t343 * t502 * (-1.0 / 2.0); + q_d(kIdxBw + 0, kIdxQ + 2) = dt * e_w2 * t343 * t502 + * (-1.0 / 6.0); + q_d(kIdxBw + 0, kIdxBw + 0) = dt * t502; + q_d(kIdxBw + 1, kIdxV + 0) = dt * t343 * t362 * t563 + * (-1.0 / 6.0); + q_d(kIdxBw + 1, kIdxV + 1) = dt * t343 * t388 * t563 + * (-1.0 / 6.0); + q_d(kIdxBw + 1, kIdxV + 2) = t656; + q_d(kIdxBw + 1, kIdxQ + 1) = t343 * t563 * (-1.0 / 2.0); + q_d(kIdxBw + 1, kIdxQ + 2) = t657; + q_d(kIdxBw + 1, kIdxBw + 1) = dt * t563; + q_d(kIdxBw + 2, kIdxV + 0) = dt * t343 * t362 * t503 + * (-1.0 / 6.0); + q_d(kIdxBw + 2, kIdxV + 1) = dt * t343 * t381 * t503 + * (-1.0 / 6.0); + q_d(kIdxBw + 2, kIdxV + 2) = dt * t343 * t394 * t503 + * (-1.0 / 6.0); + q_d(kIdxBw + 2, kIdxQ + 1) = dt * e_w1 * t343 * t503 + * (-1.0 / 6.0); + q_d(kIdxBw + 2, kIdxQ + 2) = t343 * t503 * (-1.0 / 2.0); + q_d(kIdxBw + 2, kIdxBw + 2) = dt * t503; + q_d(kIdxBa + 0, kIdxP + 0) = dt * t343 * t346 * t398 + * (-1.0 / 6.0); + q_d(kIdxBa + 0, kIdxP + 1) = dt * t343 * t366 * t398 + * (-1.0 / 6.0); + q_d(kIdxBa + 0, kIdxP + 2) = t658; + q_d(kIdxBa + 0, kIdxV + 0) = t343 * t346 * t398 + * (-1.0 / 2.0); + q_d(kIdxBa + 0, kIdxV + 1) = t343 * t366 * t398 + * (-1.0 / 2.0); + q_d(kIdxBa + 0, kIdxV + 2) = t659; + q_d(kIdxBa + 0, kIdxBa + 0) = dt * t398; + q_d(kIdxBa + 1, kIdxP + 0) = t660; + q_d(kIdxBa + 1, kIdxP + 1) = dt * t343 * t367 * t399 + * (-1.0 / 6.0); + q_d(kIdxBa + 1, kIdxP + 2) = dt * t343 * t372 * t399 + * (-1.0 / 6.0); + q_d(kIdxBa + 1, kIdxV + 0) = t661; + q_d(kIdxBa + 1, kIdxV + 1) = t343 * t367 * t399 + * (-1.0 / 2.0); + q_d(kIdxBa + 1, kIdxV + 2) = t343 * t372 * t399 + * (-1.0 / 2.0); + q_d(kIdxBa + 1, kIdxBa + 1) = dt * t399; + q_d(kIdxBa + 2, kIdxP + 0) = dt * t343 * t345 * t400 + * (-1.0 / 6.0); + q_d(kIdxBa + 2, kIdxP + 1) = t662; + q_d(kIdxBa + 2, kIdxP + 2) = dt * t343 * t370 * t400 + * (-1.0 / 6.0); + q_d(kIdxBa + 2, kIdxV + 0) = t343 * t345 * t400 + * (-1.0 / 2.0); + q_d(kIdxBa + 2, kIdxV + 1) = t663; + q_d(kIdxBa + 2, kIdxV + 2) = t343 * t370 * t400 + * (-1.0 / 2.0); + q_d(kIdxBa + 2, kIdxBa + 2) = dt * t400; + + return q_d; +} diff --git a/src/x/ekf/state.cpp b/src/x/ekf/state.cpp new file mode 100644 index 0000000..883c084 --- /dev/null +++ b/src/x/ekf/state.cpp @@ -0,0 +1,293 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +using namespace x; + +State::State(const size_t n_poses, const size_t n_features) { + // Allocate and initialize vision with zero values + // Note: other states are already initialed at default values by the default + // constructor. + p_array_ = Matrix::Zero(n_poses * 3, 1); + q_array_ = Matrix::Zero(n_poses * 4, 1); + f_array_ = Matrix::Zero(n_features * 3, 1); + const size_t n = kSizeCoreErr + n_poses * 6 + n_features * 3; + cov_ = Matrix::Identity(n, n); +} + +State::State(const double time, + const unsigned int seq, + const Vector3& p, + const Vector3& v, + const Quaternion& q, + const Vector3& b_w, + const Vector3& b_a, + const Matrix& p_array, + const Matrix& q_array, + const Matrix& f_array, + const Matrix& cov, + const Quaternion& q_ic, + const Vector3& p_ic, + const Vector3& w_m, + const Vector3& a_m) + : time_ { time } + , seq_ { seq } + , p_ { p } + , v_ { v } + , q_ { q } + , b_w_ { b_w } + , b_a_ { b_a } + , p_array_ { p_array } + , q_array_ { q_array } + , f_array_ { f_array } + , cov_ { cov } + , q_ic_ { q_ic } + , p_ic_ { p_ic } + , w_m_ { w_m } + , a_m_ { a_m } +{ + // The number of positions and orientations should always be the same + assert( p_array.rows() / 3 == q_array.rows() / 4 + || p_array.cols() == 1 || q_array.cols() == 1); +} + +double State::getTime() const { + return time_; +} + +unsigned int State::getSeq() const { + return seq_; +} + +Vector3 State::getPosition() const { + return p_; +} + +Quaternion State::getOrientation() const { + return q_; +} + +Matrix State::getPositionArray() const { + return p_array_; +} + +Matrix State::getOrientationArray() const { + return q_array_; +} + +Matrix State::getFeatureArray() const { + return f_array_; +} + +Quaternion State::getOrientationExtrinsics() const { + return q_ic_; +} + +Vector3 State::getPositionExtrinsics() const { + return p_ic_; +} + +Matrix State::getCovariance() const { + return cov_; +} + +Matrix State::getPoseCovariance() const { + Matrix pose_cov(6,6); + + // Fill in the four 3x3 blocks from the full covariance matrix + pose_cov.topLeftCorner(3,3) = cov_.topLeftCorner(3,3); + pose_cov.bottomRightCorner(3,3) = cov_.block<3,3>(kIdxQ,kIdxQ); + pose_cov.topRightCorner(3,3) = cov_.block<3,3>(kIdxP,kIdxQ); + pose_cov.bottomLeftCorner(3,3) = cov_.block<3,3>(kIdxQ,kIdxP); + + return pose_cov; +} + +Matrix& State::getCovarianceRef() { + return cov_; +} + +void State::setTime(const double time) { + time_ = time; +} + +void State::setPositionArray(const Matrix& p_array) { + // The number of positions and orientations should always be the same + assert( p_array.rows() / 3 == q_array_.rows() / 4 || p_array.cols() == 1); + p_array_ = p_array; +} + +void State::setOrientationArray(const Matrix& q_array) { + // The number of positions and orientations should always be the same + assert( q_array.rows() / 4 == p_array_.rows() / 3 || q_array.cols() == 1); + q_array_ = q_array; +} + +void State::setFeatureArray(const Matrix& f_array) { + f_array_ = f_array; +} + +void State::setCovariance(const Matrix& cov) { + cov_ = cov; +} + +void State::setImu(const double time, + const unsigned int seq, + const Vector3& w_m, + const Vector3& a_m) { + time_ = time; + seq_ = seq; + w_m_ = w_m; + a_m_ = a_m; +} + +void State::setStaticStatesFrom(const State& state) { + b_w_ = state.b_w_; + b_a_ = state.b_a_; + q_ic_ = state.q_ic_; + p_ic_ = state.p_ic_; + p_array_ = state.p_array_; + q_array_ = state.q_array_; + f_array_ = state.f_array_; +} + +void State::reset() { + time_ = kInvalid; +} + +int State::nPosesMax() const { + return p_array_.rows() / 3; +} + +int State::nFeaturesMax() const { + return f_array_.rows() / 3; +} + +int State::nErrorStates() const { + return kSizeCoreErr + + p_array_.rows() + + q_array_.rows() / 4 * 3 + + f_array_.rows(); +} + +void State::computeUnbiasedImuMeasurements(Vector3& e_w, Vector3& e_a) const { + // Bias-free gyro measurement + e_w = w_m_ - b_w_; + // Bias-free accel measurement + e_a = a_m_ - b_a_; +} + +Attitude State::computeCameraAttitude() const { + const Quaternion quat = q_.normalized() * q_ic_.normalized(); + return Attitude(quat.x(), quat.y(), quat.z(), quat.w()); +} + +Vector3 State::computeCameraPosition() const { + return p_ + q_.normalized().toRotationMatrix() * p_ic_; +} + +Quaternion State::computeCameraOrientation() const { + return q_.normalized() * q_ic_.normalized(); +} + +void State::correct(const Eigen::VectorXd& correction) { + // Check correction size + assert(correction.rows() == nErrorStates() && correction.cols() == 1); + + // Parse state corrections + const int n_p_array = p_array_.rows(); + const int n_delta_theta_array = n_p_array; + const int n_f_array = f_array_.rows(); + const Vector3 delta_p = correction.segment(kIdxP, 3); + const Vector3 delta_v = correction.segment(kIdxV, 3); + const Vector3 delta_theta = correction.segment(kIdxQ, 3); + const Vector3 delta_b_w = correction.segment(kIdxBw, 3); + const Vector3 delta_b_a = correction.segment(kIdxBa, 3); + const Eigen::VectorXd delta_p_array + = correction.segment(kSizeCoreErr, n_p_array); + const Eigen::VectorXd delta_theta_array + = correction.segment(kSizeCoreErr + n_p_array, n_delta_theta_array); + const Eigen::VectorXd delta_f_array + = correction.segment(kSizeCoreErr + n_p_array + n_delta_theta_array, n_f_array); + + // Correct all states but quaternions (additive correction) + p_ += delta_p; + v_ += delta_v; + b_w_ += delta_b_w; + b_a_ += delta_b_a; + p_array_ += delta_p_array; + f_array_ += delta_f_array; + + // Correct current orientation (quaternion multiplication) + const Quaternion delta_q = errorQuatFromSmallAngles(delta_theta); + q_ = (q_ * delta_q).normalized(); + + // Correction orientation array + const int n_poses = n_p_array / 3; + for (int i = 0; i < n_poses; ++i) { + // Delta quaternion + const Vector3 delta_theta_i + = delta_theta_array.segment(3*i, 3); + const Quaternion delta_q_i = errorQuatFromSmallAngles(delta_theta_i); + + // Prior quaternion estimate (stored as x,y,z,w in state array) + // TODO(jeff) Change to w,z,y,z. This is confusing wrt Eigen. + Quaternion q_i(q_array_(4*i+3, 0), // w + q_array_(4*i, 0), // x + q_array_(4*i+1, 0), // y + q_array_(4*i+2, 0)); // z + + // Quaternion posterior + q_i = q_i * delta_q_i; + q_i.normalize(); + + // Insert in orientation array + q_array_.block<4, 1>(4*i, 0) << q_i.x(), q_i.y(), q_i.z(), q_i.w(); + } +} + +std::string State::toString() const { + std::stringstream s; +s << "---------------------------------------" + "---------------------------------------" << std::endl; + s << "Timestamp: " << std::setprecision(19) << time_ << std::endl; + s < +#include +#include + +using namespace x; + +State& StateBuffer::getTailStateRef() { + return at(tail_idx_); +} + +int StateBuffer::closestIdx(const double timestamp) const { + // Check if measurement is from the future + if (timestamp > at(tail_idx_).getTime() + time_margin_) { + std::cout << "Closest state request too far in the future. " + "Check for time delay. " + << "Time request: " << std::setprecision(17) << timestamp + << ". Last time in buffer : " << at(tail_idx_).getTime() << std::endl; + return kInvalidIndex; + } + + // Check if measurement is from the past + if (timestamp < at(head_idx_).getTime() - time_margin_) { + std::cout << "Closest state request too far in the past. " + "Increase buffer size." + << "Timestamp : " << std::setprecision(17) << timestamp + << ". Oldest state in buffer : " << at(head_idx_).getTime() << std::endl; + return kInvalidIndex; + } + + // Browse state chronogically backwards until time difference stops reducing, + // or we have been through the whole buffer. + double time_offset = std::abs(timestamp - at(tail_idx_).getTime()); + int idx = previousIdx(tail_idx_); + size_t count = 1; + while (std::abs(timestamp - at(idx).getTime()) < time_offset + && count < n_valid_states_) { + time_offset = std::abs(timestamp - at(idx).getTime()); + idx = previousIdx(idx); + ++count; + } + + // Return closest state. Unless we have been through the whole buffer, that + // was the previous-to-last index. + return nextIdx(idx); +} + +int StateBuffer::nextIdx(const int idx) const { + return (idx + 1) % size(); +} + +int StateBuffer::previousIdx(const int idx) const { + if (idx == 0) + return size() - 1; + else + return idx - 1; +} + +State& StateBuffer::enqueueInPlace() { + // Increment tail + ++tail_idx_ %= size(); + + // Increment count and head + if (n_valid_states_ < size()) + ++n_valid_states_; + else + ++head_idx_ %= size(); + + // Return reference to the state to be filled by caller + return at(tail_idx_); +} + +void StateBuffer::resetFromState(const State& init_state) { + // Reset each state in buffer + for (size_t i = 0; i < size(); ++i) + at(i).reset(); + + // Set the first state of the buffer to init_state + tail_idx_ = 0; + head_idx_ = 0; + n_valid_states_ = 1; + assert(size()); + at(tail_idx_) = init_state; +} diff --git a/src/x/ekf/updater.cpp b/src/x/ekf/updater.cpp new file mode 100644 index 0000000..01708d6 --- /dev/null +++ b/src/x/ekf/updater.cpp @@ -0,0 +1,74 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +using namespace x; + +void Updater::update(State& state) { + // Image pre-processing + preProcess(state); + + // Pre-update work and check if update is needed + const bool update_requested = preUpdate(state); + + if (update_requested) { + // Initialize correction vector + Matrix correction = Matrix::Zero(state.nErrorStates(), 1); + + // EKF Kalman update (iterated if iekf_iter_ > 1) + for (int i=0; i < iekf_iter_; i++) { + // Construct Jacobian, residual and noise covariance matrices + Matrix h, res, r; + constructUpdate(state, h, res, r); + + // Apply update + const bool is_last_iter = i == iekf_iter_ - 1; // true if this is the last loop iteration + applyUpdate(state, h, res, r, correction, is_last_iter); + } + + // Post update: feature initialization + postUpdate(state, correction); + } +} + +void Updater::applyUpdate(State& state, + const Matrix& H, + const Matrix& res, + const Matrix& R, + Matrix& correction_total, + const bool cov_update) { + // Compute Kalman gain and state correction + // TODO(jeff) Assert state correction doesn't have NaNs/Infs. + Matrix& P = state.getCovarianceRef(); + const Matrix S = H * P * H.transpose() + R; + const Matrix K = P * H.transpose() * S.inverse(); + Matrix correction = K * (res + H * correction_total) - correction_total; + + // Covariance update (skipped if this is not the last IEKF iteration) + const size_t n = P.rows(); + if (cov_update) { + P = (Matrix::Identity(n, n) - K * H) * P; + // Make sure P stays symmetric. + P = 0.5 * (P + P.transpose()); + } + + // State update + state.correct(correction); + + // Add correction at current iteration to total (for IEKF) + correction_total += correction; +} diff --git a/src/x/vio/msckf_slam_update.cpp b/src/x/vio/msckf_slam_update.cpp new file mode 100644 index 0000000..007ebc1 --- /dev/null +++ b/src/x/vio/msckf_slam_update.cpp @@ -0,0 +1,272 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +using namespace x; +using namespace Eigen; + +MsckfSlamUpdate::MsckfSlamUpdate(const x::TrackList& trks, + const x::AttitudeList& quats, + const x::TranslationList& pos, + const Triangulation& triangulator, + const MatrixXd& cov_s, + const int n_poses_max, + const double sigma_img) +{ + // Number of features + const size_t n_trks = trks.size(); + + // Number of feature observations + size_t n_obs = 0; + for(size_t i=0; i < n_trks; i++) + n_obs += trks[i].size(); + + // Initialize MSCKF Kalman update matrices + const size_t rows0 = 2 * n_obs - n_trks * 3; + const size_t cols = cov_s.cols(); + jac_ = MatrixXd::Zero(rows0, cols); + cov_m_diag_ = VectorXd::Ones(rows0); + res_ = MatrixXd::Zero(rows0, 1); + + // Initialize MSCKF-SLAM feature initialization matrices + const size_t rows1 = n_trks * 3; + init_mats_.H1 = MatrixXd::Zero(rows1, cols); + init_mats_.H2 = MatrixXd::Zero(rows1, n_trks * 3); + init_mats_.r1 = MatrixXd::Zero(rows1, 1); + init_mats_.features = MatrixXd::Zero(rows1, 1); + + // For each track, compute residual, Jacobian and covariance block + const double var_img = sigma_img * sigma_img; + size_t row_h = 0, row1 = 0; + for (size_t i = 0; i < n_trks; ++i) { + processOneTrack(trks[i], + quats, + pos, + triangulator, + cov_s, + n_poses_max, + var_img, + i, + row_h, + row1); + } +} + +void MsckfSlamUpdate::processOneTrack(const x::Track& track, + const x::AttitudeList& C_q_G, + const x::TranslationList& G_p_C, + const Triangulation& triangulator, + const MatrixXd& P, + const int n_poses_max, + const double var_img, + const size_t& j, + size_t& row_h, + size_t& row1) +{ + const size_t track_size = track.size(); + unsigned int rows_track_j = track_size * 2; + const size_t cols = P.cols(); + MatrixXd h_j(MatrixXd::Zero(rows_track_j, cols)); + MatrixXd Hf_j(MatrixXd::Zero(h_j.rows(), kJacCols)); + MatrixXd res_j(MatrixXd::Zero(rows_track_j, 1)); + + // Feature triangulation + Vector3d feature; // inverse-depth parameters in last observation frame + triangulator.triangulateGN(track, feature); + const double alpha = feature(0); + const double beta = feature(1); + const double rho = feature(2); + + // Anchor pose + x::Quaternion Cn_q_G; + Cn_q_G.x() = C_q_G.back().ax; + Cn_q_G.y() = C_q_G.back().ay; + Cn_q_G.z() = C_q_G.back().az; + Cn_q_G.w() = C_q_G.back().aw; + + Vector3d G_p_Cn(G_p_C.back().tx, G_p_C.back().ty, G_p_C.back().tz); + + // Coordinate of feature in global frame + Vector3d G_p_fj = 1 / (rho)*Cn_q_G.normalized().toRotationMatrix() * Vector3d(alpha, beta, 1) + G_p_Cn; + + x::Quatern attitude_to_quaternion; + + // LOOP OVER ALL FEATURE OBSERVATIONS + for (size_t i = 0; i < track_size; ++i) + { + const unsigned int pos = C_q_G.size() - track_size + i; + + Quaterniond Ci_q_G_ = attitude_to_quaternion(C_q_G[pos]); + Vector3d G_p_Ci_(G_p_C[pos].tx, G_p_C[pos].ty, G_p_C[pos].tz); + + // Feature position expressed in camera frame. + Vector3d Ci_p_fj; + Ci_p_fj << Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_); + + // eq. 20(a) + Vector2d z; + z(0) = track[i].getX(); + z(1) = track[i].getY(); + + Vector2d z_hat(z); + assert(Ci_p_fj(2)); + z_hat(0) = Ci_p_fj(0) / Ci_p_fj(2); + z_hat(1) = Ci_p_fj(1) / Ci_p_fj(2); + + // eq. 20(b) + res_j(i * 2, 0) = z(0) - z_hat(0); + res_j(i * 2 + 1, 0) = z(1) - z_hat(1); + + //============================ + // Measurement Jacobian matrix + //============================ + + if (i == track_size - 1) // Handle special case + { + // Inverse-depth feature coordinates jacobian + MatrixXd mat(MatrixXd::Zero(2, 3)); + mat(0, 0) = 1.0; + mat(1, 1) = 1.0; + + // Update stacked Jacobian matrices associated to the current feature + unsigned int row = i * kVisJacRows; + Hf_j.block(row, 0) = mat; + } + else + { + // Set Jacobian of pose for i'th measurement of feature j (eq.22, 23) + VisJacBlock J_i(VisJacBlock::Zero()); + // first row + J_i(0, 0) = 1.0 / Ci_p_fj(2); + J_i(0, 1) = 0.0; + J_i(0, 2) = -Ci_p_fj(0) / std::pow((double)Ci_p_fj(2), 2); + // second row + J_i(1, 0) = 0.0; + J_i(1, 1) = 1.0 / Ci_p_fj(2); + J_i(1, 2) = -Ci_p_fj(1) / std::pow((double)Ci_p_fj(2), 2); + + // Attitude + Vector3d skew_vector = Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_); + VisJacBlock J_attitude = J_i * x::Skew(skew_vector(0), skew_vector(1), skew_vector(2)).matrix; + + // Position + VisJacBlock J_position = -J_i * Ci_q_G_.normalized().toRotationMatrix().transpose(); + + // Anchor attitude + VisJacBlock J_anchor_att = -1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() * + Cn_q_G.normalized().toRotationMatrix() * x::Skew(alpha, beta, 1).matrix; + + // Anchor position + VisJacBlock J_anchor_pos = -J_position; + + // Inverse-depth feature coordinates + MatrixXd mat(MatrixXd::Identity(3, 3)); + mat(0, 2) = -alpha / rho; + mat(1, 2) = -beta / rho; + mat(2, 2) = -1 / rho; + VisJacBlock Hf_j1 = 1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() * + Cn_q_G.normalized().toRotationMatrix() * mat; + + // Update stacked Jacobian matrices associated to the current feature + unsigned int row = i * kVisJacRows; + Hf_j.block(row, 0) = Hf_j1; + + unsigned int col = pos * kJacCols; + h_j.block(row, kSizeCoreErr + col) = J_position; + + col += n_poses_max * kJacCols; + h_j.block(row, kSizeCoreErr + col) = J_attitude; + + col = (C_q_G.size() - 1) * kJacCols; + h_j.block(row, kSizeCoreErr + col) = J_anchor_pos; + + col += n_poses_max * kJacCols; + h_j.block(row, kSizeCoreErr + col) = J_anchor_att; + } + } // LOOP OVER ALL FEATURE OBSERVATIONS + + //======================================================================== + // Left nullspace projection + //======================================================================== + // Nullspace computation + MatrixXd q = Hf_j.householderQr().householderQ(); + MatrixXd A = x::MatrixBlock(q, 0, 3); + + // Projections + MatrixXd res0_j = A.transpose() * res_j; + MatrixXd h0_j = A.transpose() * h_j; + + // New noise measurement matrix + VectorXd r0_j_diag = var_img * VectorXd::Ones(rows_track_j - 3); + MatrixXd r0_j = r0_j_diag.asDiagonal(); + + //======================================================================== + // Column space projection + //======================================================================== + // Only needed for persistent feature init (Li, 2012) + + // Column space + const MatrixXd U = x::MatrixBlock(q, 0, 0, q.rows(), 3); + + // Projections to be used in Core::CorrectAfterCoreCorrection + MatrixXd H1j = U.transpose() * h_j; + MatrixXd H2j = U.transpose() * Hf_j; + MatrixXd r1j = U.transpose() * res_j; + + init_mats_.H1.block(row1, 0, 3, cols) = H1j; + init_mats_.H2.block(row1, row1, 3, 3) = H2j; + init_mats_.r1.block(row1, 0, 3, 1) = r1j; + init_mats_.features.block(row1, 0, 3, 1)= feature; + row1 += 3; + + //========================================================================== + // Outlier rejection + //========================================================================== + MatrixXd S_inv = (h0_j * P * h0_j.transpose() + r0_j).inverse(); + MatrixXd gamma = res0_j.transpose() * S_inv * res0_j; + boost::math::chi_squared_distribution<> my_chisqr(2 * track_size - 3); // 2*Mj-3 DoFs + double chi = quantile(my_chisqr, 0.95); // 95-th percentile + + if (gamma(0, 0) < chi) // Inlier + { +#ifdef VERBOSE + inliers_.push_back(G_p_fj); +#endif + + jac_.block(row_h, // startRow + 0, // startCol + rows_track_j - 3, // numRows + cols) = h0_j; // numCols + + // Residual vector [feature j] + res_.block(row_h, 0, rows_track_j - 3, 1) = res0_j; + + // Measurement covariance matrix [feature j] + cov_m_diag_.segment(row_h, rows_track_j - 3) = r0_j_diag; + + row_h += rows_track_j - 3; + } + else // outlier + { +#ifdef VERBOSE + outliers_.push_back(G_p_fj); +#endif + } +} diff --git a/src/x/vio/msckf_update.cpp b/src/x/vio/msckf_update.cpp new file mode 100644 index 0000000..5cc4954 --- /dev/null +++ b/src/x/vio/msckf_update.cpp @@ -0,0 +1,210 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +using namespace x; +using namespace Eigen; + +MsckfUpdate::MsckfUpdate(const x::TrackList& trks, + const x::AttitudeList& quats, + const x::TranslationList& pos, + const Triangulation& triangulator, + const MatrixXd& cov_s, + const int n_poses_max, + const double sigma_img) +{ + // Number of features + const size_t n_trks = trks.size(); + + // Number of feature observations + size_t n_obs = 0; + for(size_t i=0; i < n_trks; i++) + n_obs += trks[i].size(); + + // Initialize Kalman update matrices + const size_t rows = 2 * n_obs - n_trks * 3; + const size_t cols = cov_s.cols(); + jac_ = MatrixXd::Zero(rows, cols); + cov_m_diag_ = VectorXd::Ones(rows); + res_ = MatrixXd::Zero(rows, 1); + + // For each track, compute residual, Jacobian and covariance block + const double var_img = sigma_img * sigma_img; + for (size_t i = 0, row_h = 0; i < n_trks; ++i) { + processOneTrack(trks[i], + quats, + pos, + triangulator, + cov_s, + n_poses_max, + var_img, + i, + row_h); + } +} + +void MsckfUpdate::processOneTrack(const x::Track& track, + const x::AttitudeList& C_q_G, + const x::TranslationList& G_p_C, + const Triangulation& triangulator, + const MatrixXd& P, + const int n_poses_max, + const double var_img, + const size_t& j, + size_t& row_h) +{ + // Initialization + const size_t track_size = track.size(); + unsigned int rows_track_j = track_size * 2; + const size_t cols = P.cols(); + MatrixXd jac_j(MatrixXd::Zero(rows_track_j, cols)); + MatrixXd Hf_j(MatrixXd::Zero(jac_j.rows(), kJacCols)); + MatrixXd res_j(MatrixXd::Zero(rows_track_j, 1)); + + // Triangulate feature j + // Inverse-depth parameters in last observation frame + Vector3d feature; // inverse-depth parameters in last observation frame + triangulator.triangulateGN(track, feature); + const double alpha = feature(0); + const double beta = feature(1); + const double rho = feature(2); + + // Coordinate of feature in global frame + x::Quaternion Cn_q_G; + Cn_q_G.x() = C_q_G.back().ax; + Cn_q_G.y() = C_q_G.back().ay; + Cn_q_G.z() = C_q_G.back().az; + Cn_q_G.w() = C_q_G.back().aw; + + Vector3d G_p_Cn(G_p_C.back().tx, G_p_C.back().ty, G_p_C.back().tz); + + Vector3d G_p_fj = 1 / (rho)*Cn_q_G.normalized().toRotationMatrix() * Vector3d(alpha, beta, 1) + G_p_Cn; + + x::Quatern attitude_to_quaternion; + + // LOOP OVER ALL FEATURE OBSERVATIONS + for (size_t i = 0; i < track_size; ++i) + { + const unsigned int pos = C_q_G.size() - track_size + i; + + Quaterniond Ci_q_G_ = attitude_to_quaternion(C_q_G[pos]); + Vector3d G_p_Ci_(G_p_C[pos].tx, G_p_C[pos].ty, G_p_C[pos].tz); + + // Feature position expressed in camera frame. + Vector3d Ci_p_fj; + Ci_p_fj << Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_); + + // eq. 20(a) + Vector2d z; + z(0) = track[i].getX(); + z(1) = track[i].getY(); + + Vector2d z_hat(z); + assert(Ci_p_fj(2)); + z_hat(0) = Ci_p_fj(0) / Ci_p_fj(2); + z_hat(1) = Ci_p_fj(1) / Ci_p_fj(2); + + // eq. 20(b) + res_j(i * 2, 0) = z(0) - z_hat(0); + res_j(i * 2 + 1, 0) = z(1) - z_hat(1); + + // Set Jacobian of pose for i'th measurement of feature j (eq.22, 23) + VisJacBlock J_i = VisJacBlock::Zero(); + // first row + J_i(0, 0) = 1.0 / Ci_p_fj(2); + J_i(0, 1) = 0.0; + J_i(0, 2) = -Ci_p_fj(0) / std::pow((double)Ci_p_fj(2), 2); + // second row + J_i(1, 0) = 0.0; + J_i(1, 1) = 1.0 / Ci_p_fj(2); + J_i(1, 2) = -Ci_p_fj(1) / std::pow((double)Ci_p_fj(2), 2); + + unsigned int row = i * kVisJacRows; + + // Measurement Jacobians wrt attitude, position and feature + // Position + VisJacBlock J_position = -J_i * Ci_q_G_.normalized().toRotationMatrix().transpose(); + + Vector3d dP = Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_); + + // Attitude + VisJacBlock J_attitude = J_i * x::Skew(dP(0), dP(1), dP(2)).matrix; + + // Feature + Hf_j.block(row, 0) = -J_position; + + // Update stacked Jacobian matrix associated to the current feature + unsigned int col = pos * kJacCols; + jac_j.block(row, kSizeCoreErr + col) = J_position; + + col += n_poses_max * kJacCols; + + jac_j.block(row, kSizeCoreErr + col) = J_attitude; + } // LOOP OVER ALL FEATURE OBSERVATIONS + + //======================================================================== + // Left nullspace projection + //======================================================================== + // Nullspace computation + MatrixXd q = Hf_j.householderQr().householderQ(); + MatrixXd A = x::MatrixBlock(q, 0, 3); + + // Projections + MatrixXd res0_j = A.transpose() * res_j; + MatrixXd jac0_j = A.transpose() * jac_j; + + // Noise measurement matrix + const VectorXd r0_j_diag = var_img * VectorXd::Ones(rows_track_j - 3); + const MatrixXd r0_j = r0_j_diag.asDiagonal(); + + //========================================================================== + // Outlier rejection + //========================================================================== + MatrixXd S_inv = (jac0_j * P * jac0_j.transpose() + r0_j).inverse(); + MatrixXd gamma = res0_j.transpose() * S_inv * res0_j; + boost::math::chi_squared_distribution<> my_chisqr(2 * track_size - 3); // 2*Mj-3 DOFs + double chi = quantile(my_chisqr, 0.95); // 95-th percentile + + if (gamma(0, 0) < chi) // Inlier + { +#ifdef VERBOSE + inliers_.push_back(G_p_fj); +#endif + + jac_.block(row_h, // startRow + 0, // startCol + rows_track_j - 3, // numRows + cols) = jac0_j; // numCols + + // Residual vector (for this track) + res_.block(row_h, 0, rows_track_j - 3, 1) = res0_j; + + // Measurement covariance matrix diagonal + cov_m_diag_.segment(row_h, rows_track_j - 3) = r0_j_diag; + + row_h += rows_track_j - 3; + } + else // outlier + { +#ifdef VERBOSE + outliers_.push_back(G_p_fj); +#endif + } +} diff --git a/src/x/vio/range_update.cpp b/src/x/vio/range_update.cpp new file mode 100644 index 0000000..48560c0 --- /dev/null +++ b/src/x/vio/range_update.cpp @@ -0,0 +1,407 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +using namespace x; +using namespace Eigen; + +RangeUpdate::RangeUpdate(const x::RangeMeasurement& range_meas, + const std::vector& tr_feat_ids, + const x::AttitudeList& quats, + const x::TranslationList& poss, + const MatrixXd& feature_states, + const std::vector& anchor_idxs, + const MatrixXd& cov_s, + const int n_poses_max, + const double sigma_range) +{ + // Initialize Kalman update matrices + const size_t cols = cov_s.cols(); + jac_ = MatrixXd::Zero(1, cols); + cov_m_diag_ = VectorXd::Ones(1); + res_ = MatrixXd::Zero(1, 1); + + // Compute residual, Jacobian and covariance block + processRangedFacet(range_meas, + tr_feat_ids, + quats, + poss, + feature_states, + anchor_idxs, + cov_s, + n_poses_max, + sigma_range); + /*processRangedFeature(range_meas, + quats, + poss, + feature_states, + P, + n_poses_max, + sigma_range, + 0);*/ +} + +void RangeUpdate::processRangedFacet(const x::RangeMeasurement& range_meas, + const std::vector& tr_feat_ids, + const x::AttitudeList& C_q_G, + const x::TranslationList& G_p_C, + const MatrixXd& feature_states, + const std::vector& anchor_idxs, + const MatrixXd& P, + const int n_poses_max, + const double sigma_range) +{ + /******************************************************** + * Cartesian world coordinates of the triangle features * + ********************************************************/ + + // Array storing the of the three features + std::array G_p_fj; // world position + std::array G_p_Ca; // anchor position + std::array Ca_q_G; // anchor quaternion + std::array anchor_idx; // anchor quaternion + std::array alpha; // Normalized undistored image coordinates X + std::array beta; // Normalized undistored image coordinates Y + std::array rho; // Inverse depth + + for(unsigned int j=0; j<3; j++) + { + // Inverse-depth parameters in last observation frame + alpha[j] = feature_states(tr_feat_ids[j] * 3, 0); + beta[j] = feature_states(tr_feat_ids[j] * 3 + 1, 0); + rho[j] = feature_states(tr_feat_ids[j] * 3 + 2, 0); + + // Anchor pose + anchor_idx[j] = anchor_idxs[tr_feat_ids[j]]; + Ca_q_G[j].x() = C_q_G[anchor_idx[j]].ax; + Ca_q_G[j].y() = C_q_G[anchor_idx[j]].ay; + Ca_q_G[j].z() = C_q_G[anchor_idx[j]].az; + Ca_q_G[j].w() = C_q_G[anchor_idx[j]].aw; + + const Vector3d anchor_pos(G_p_C[anchor_idx[j]].tx, G_p_C[anchor_idx[j]].ty, G_p_C[anchor_idx[j]].tz); + G_p_Ca[j] = anchor_pos; + + // Coordinate of feature in global frame + G_p_fj[j] = 1 / (rho[j])*Ca_q_G[j].normalized().toRotationMatrix() * Vector3d(alpha[j], beta[j], 1) + G_p_Ca[j]; + } + + /*********************** + * Current camera pose * + ***********************/ + + // Orientation + x::Attitude Cn_q_G(C_q_G.back()); + x::Quatern attitude_to_quaternion; + x::Quaternion Ci_q_G = attitude_to_quaternion(Cn_q_G); + + // Position + x::Translation G_p_Cn(G_p_C.back()); + Vector3d G_p_Ci(G_p_Cn.tx, G_p_Cn.ty, G_p_Cn.tz); + + /************ + * Residual * + ************/ + + // Compute normal vector to the triangle plane in world coordinates + const Vector3d G_n = (G_p_fj[0] - G_p_fj[1]).cross(G_p_fj[2] - G_p_fj[1]); + + // Undistorted normalized homogeneous 2D coordinates of the LRF impact point on the ground + const Vector3d lrf_img_pt_nh(range_meas.img_pt_n.getX(), range_meas.img_pt_n.getY(), 1.0); + + // Measurement prediction + const double a = (G_p_fj[1] - G_p_Ci).dot(G_n); + const double b = lrf_img_pt_nh.dot(Ci_q_G.normalized().toRotationMatrix().transpose() * G_n); + double range_hat = a/b; + + // Measurement + const double range(range_meas.range); + + // Actual residual + MatrixXd res_j(MatrixXd::Zero(1, 1)); + res_j(0, 0) = range - range_hat; + + /******************************* + * Measurement Jacobian matrix * + *******************************/ + + const size_t cols = P.cols(); + MatrixXd h_j(MatrixXd::Zero(1, cols)); + + // Camera Position + const RangeJacobian J_pc = - 1.0 / b * G_n.transpose(); + + // Camera attitude + const RangeJacobian J_qc = a / std::pow(b, 2.0) + * G_n.transpose() * Ci_q_G.normalized().toRotationMatrix() + * x::Skew(lrf_img_pt_nh(0), lrf_img_pt_nh(1), lrf_img_pt_nh(2)).matrix; + + // Position of the LRF target point in world frame + const Vector3d G_p_r = a / b * Ci_q_G.normalized().toRotationMatrix() * lrf_img_pt_nh + G_p_Ci; + + // Barycenter + const Vector3d G_p_bary = 1.0/3.0 * (G_p_fj[0]+G_p_fj[1]+G_p_fj[2]); + /* Triangle feature 1 */ + // Jacobian wrt cartesian feature coordinates + const RangeJacobian J_f0 = 1.0 / b * (1.0/3.0 * G_n + (G_p_fj[2] - G_p_fj[1]).cross(G_p_bary - G_p_r)).transpose(); + // Anchor position + const RangeJacobian J_pc0 = J_f0; + // Anchor attitude + const RangeJacobian J_qc0 = - 1.0 / rho[0] * J_f0 * Ca_q_G[0].normalized().toRotationMatrix() + * x::Skew(alpha[0], beta[0], 1.0).matrix; + // Inverse-depth feature coordinates + MatrixXd mat(MatrixXd::Identity(3, 3)); + mat(0, 2) = - alpha[0] / rho[0]; + mat(1, 2) = - beta[0] / rho[0]; + mat(2, 2) = - 1 / rho[0]; + RangeJacobian J_fi0 = 1 / rho[0] * J_f0 * Ca_q_G[0].normalized().toRotationMatrix() * mat; + + /* Triangle feature 2 */ + // Jacobian wrt cartesian feature coordinates + const RangeJacobian J_f1 = 1.0 / b * (1.0/3.0 * G_n + (G_p_fj[0] - G_p_fj[2]).cross(G_p_bary - G_p_r)).transpose(); + // Anchor position + const RangeJacobian J_pc1 = J_f1; + // Anchor attitude + const RangeJacobian J_qc1 = - 1.0 / rho[1] * J_f1 * Ca_q_G[1].normalized().toRotationMatrix() + * x::Skew(alpha[1], beta[1], 1.0).matrix; + // Inverse-depth feature coordinates + mat = MatrixXd::Identity(3, 3); + mat(0, 2) = - alpha[1] / rho[1]; + mat(1, 2) = - beta[1] / rho[1]; + mat(2, 2) = - 1 / rho[1]; + RangeJacobian J_fi1 = 1 / rho[1] * J_f1 * Ca_q_G[1].normalized().toRotationMatrix() * mat; + + // Triangle feature 3 + // Jacobian wrt cartesian feature coordinates + const RangeJacobian J_f2 = 1.0 / b * (1.0/3.0 * G_n + (G_p_fj[1] - G_p_fj[0]).cross(G_p_bary - G_p_r)).transpose(); + // Anchor position + const RangeJacobian J_pc2 = J_f2; + // Anchor attitude + const RangeJacobian J_qc2 = - 1.0 / rho[2] * J_f2 * Ca_q_G[2].normalized().toRotationMatrix() + * x::Skew(alpha[2], beta[2], 1.0).matrix; + // Inverse-depth feature coordinates + mat = MatrixXd::Identity(3, 3); + mat(0, 2) = - alpha[2] / rho[2]; + mat(1, 2) = - beta[2] / rho[2]; + mat(2, 2) = - 1 / rho[2]; + RangeJacobian J_fi2 = 1 / rho[2] * J_f2 * Ca_q_G[2].normalized().toRotationMatrix() * mat; + + // Update stacked Jacobian matrices associated to the current feature + const unsigned int row = 0; + const unsigned int pos = C_q_G.size()- 1; + unsigned int col = pos * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_pc; + + col += n_poses_max * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_qc; + + col = anchor_idx[0] * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_pc0; + col += n_poses_max * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_qc0; + col = (n_poses_max * 2 + tr_feat_ids[0]) * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_fi0; + + col = anchor_idx[1] * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_pc1; + col += n_poses_max * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_qc1; + col = (n_poses_max * 2 + tr_feat_ids[1]) * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_fi1; + + col = anchor_idx[2] * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_pc2; + col += n_poses_max * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) + J_qc2; + col = (n_poses_max * 2 + tr_feat_ids[2]) * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_fi2; + + //========================================================================== + // Outlier rejection + //========================================================================== + VectorXd r_j(1); + const double var_range = sigma_range * sigma_range; + r_j << var_range; + MatrixXd S_inv = (h_j * P * h_j.transpose() + r_j).inverse(); + MatrixXd gamma = res_j.transpose() * S_inv * res_j; + boost::math::chi_squared_distribution<> my_chisqr(1); + double chi = quantile(my_chisqr, 0.9); // 95-th percentile + + if (gamma(0, 0) < chi) // Inlier + { + jac_.block(0, // startRow + 0, // startCol + 1, // numRows + cols) = h_j; // numCols + + // Residual vector + res_.block(0, 0, 1, 1) = res_j; + + // Measurement covariance matrix + cov_m_diag_ = r_j; + } +} + +void RangeUpdate::processRangedFeature(const x::RangeMeasurement& range_meas, + const x::AttitudeList& C_q_G, + const x::TranslationList& G_p_C, + const MatrixXd& feature_states, + const std::vector& anchor_idxs, + const MatrixXd& P, + const int n_poses_max, + const double sigma_range, + const size_t& j) +{ + const size_t cols = P.cols(); + MatrixXd h_j(MatrixXd::Zero(1, cols)); + MatrixXd res_j(MatrixXd::Zero(1, 1)); + + //========================================================================== + // Feature information + //========================================================================== + // A-priori inverse-depth parameters in last observation frame + double alpha = feature_states(j * 3, 0); + double beta = feature_states(j * 3 + 1, 0); + double rho = feature_states(j * 3 + 2, 0); + + // Anchor pose + unsigned int anchor_idx = anchor_idxs[j]; + x::Quaternion Ca_q_G; + Ca_q_G.x() = C_q_G[anchor_idx].ax; + Ca_q_G.y() = C_q_G[anchor_idx].ay; + Ca_q_G.z() = C_q_G[anchor_idx].az; + Ca_q_G.w() = C_q_G[anchor_idx].aw; + + Vector3d G_p_Ca(G_p_C[anchor_idx].tx, G_p_C[anchor_idx].ty, G_p_C[anchor_idx].tz); + + // Coordinate of feature in global frame + Vector3d G_p_fj = 1 / (rho)*Ca_q_G.normalized().toRotationMatrix() * Vector3d(alpha, beta, 1) + G_p_Ca; + + // FOR LAST FEATURE OBSERVATION + x::Translation G_p_Cn(G_p_C.back()); + x::Attitude Cn_q_G(C_q_G.back()); + + x::Quatern attitude_to_quaternion; + Quaterniond Ci_q_G_ = attitude_to_quaternion(Cn_q_G); + Vector3d G_p_Ci_(G_p_Cn.tx, G_p_Cn.ty, G_p_Cn.tz); + + // Feature position expressed in camera frame. + Vector3d Ci_p_fj; + Ci_p_fj << Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_); + + //========== + // Residual + //========== + double range(range_meas.range); + double range_hat(Ci_p_fj(2)); + res_j(0, 0) = range - range_hat; + + //============================ + // Measurement Jacobian matrix + //============================ + const unsigned int pos = C_q_G.size() - 1; + if (anchor_idx == pos) // Handle special case + { + // Inverse-depth feature coordinates jacobian + RangeJacobian mat(MatrixXd::Zero(1, 3)); + mat(0, 2) = -1 / std::pow(rho, 2); + + // Update stacked Jacobian matrices associated to the current feature + unsigned int row = 0; + unsigned int col = (n_poses_max * 2 + j) * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = mat; + } + else + { + // Set Jacobian of pose for i'th measurement of feature j (eq.22, 23) + RangeJacobian J_i(MatrixXd::Zero(1, 3)); + J_i(0, 2) = 1.0; + + // Attitude + Vector3d skew_vector = Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_); + RangeJacobian J_attitude = J_i * x::Skew(skew_vector(0), skew_vector(1), skew_vector(2)).matrix; + + // Position + RangeJacobian J_position = -J_i * Ci_q_G_.normalized().toRotationMatrix().transpose(); + + // Anchor attitude + RangeJacobian J_anchor_att = -1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() * + Ca_q_G.normalized().toRotationMatrix() * x::Skew(alpha, beta, 1).matrix; + + // Anchor position + RangeJacobian J_anchor_pos = -J_position; + + // Inverse-depth feature coordinates + MatrixXd mat(MatrixXd::Identity(3, 3)); + mat(0, 2) = -alpha / rho; + mat(1, 2) = -beta / rho; + mat(2, 2) = -1 / rho; + RangeJacobian Hf_j1 = 1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() * + Ca_q_G.normalized().toRotationMatrix() * mat; + + // Update stacked Jacobian matrices associated to the current feature + unsigned int row = 0; + const unsigned int pos = C_q_G.size() - 1; + unsigned int col = pos * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_position; + + col += n_poses_max * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_attitude; + + col = anchor_idx * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_anchor_pos; + + col += n_poses_max * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = J_anchor_att; + + col = (n_poses_max * 2 + j) * kJacCols; + h_j.block<1, kJacCols>(row, kSizeCoreErr + col) = Hf_j1; + } + + //========================================================================== + // Outlier rejection + //========================================================================== + VectorXd r_j(1); + const double var_range = sigma_range * sigma_range; + r_j << var_range; + // MatrixXd S_inv = (h_j * P * h_j.transpose() + R_j).inverse(); + // MatrixXd gamma = res_j.transpose() * S_inv * res_j; + // chi_squared_distribution<> my_chisqr(2 * features[j].size()); + // double chi = quantile(my_chisqr, 0.9); // 95-th percentile + + if (true) // gamma(0, 0) < chi) // Inlier + { + jac_.block(0, // startRow + 0, // startCol + 1, // numRows + cols) = h_j; // numCols + + // Residual vector + res_.block(0, 0, 1, 1) = res_j; + + // Measurement covariance matrix + cov_m_diag_ = r_j; + } +} diff --git a/src/x/vio/slam_update.cpp b/src/x/vio/slam_update.cpp new file mode 100644 index 0000000..f356ea7 --- /dev/null +++ b/src/x/vio/slam_update.cpp @@ -0,0 +1,251 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +using namespace x; +using namespace Eigen; + +SlamUpdate::SlamUpdate(const x::TrackList& trks, + const x::AttitudeList& quats, + const x::TranslationList& poss, + const MatrixXd& feature_states, + const std::vector& anchor_idxs, + const MatrixXd& cov_s, + const int n_poses_max, + const double sigma_img) +{ + // Number of features + const size_t n_trks = trks.size(); + + // Initialize Kalman update matrices + const size_t rows = 2 * n_trks; + const size_t cols = cov_s.cols(); + jac_ = MatrixXd::Zero(rows, cols); + cov_m_diag_ = VectorXd::Ones(rows); + res_ = MatrixXd::Zero(rows, 1); + + // For each track, compute residual, Jacobian and covariance block + const double var_img = sigma_img * sigma_img; + for (size_t i = 0, row_h = 0; i < n_trks; ++i) { + processOneTrack(trks[i], + quats, + poss, + feature_states, + anchor_idxs, + cov_s, + n_poses_max, + var_img, + i, + row_h); + } +} + +void SlamUpdate::processOneTrack(const x::Track& track, + const x::AttitudeList& C_q_G, + const x::TranslationList& G_p_C, + const MatrixXd& feature_states, + const std::vector& anchor_idxs, + const MatrixXd& P, + const int n_poses_max, + const double var_img, + const size_t& j, + size_t& row_h) +{ + const size_t cols = P.cols(); + MatrixXd h_j(MatrixXd::Zero(2, cols)); + MatrixXd Hf_j(MatrixXd::Zero(2, cols)); + MatrixXd res_j(MatrixXd::Zero(2, 1)); + + //========================================================================== + // Feature information + //========================================================================== + // A-priori inverse-depth parameters in last observation frame + double alpha = feature_states(j * 3, 0); + double beta = feature_states(j * 3 + 1, 0); + double rho = feature_states(j * 3 + 2, 0); + + // Anchor pose + unsigned int anchor_idx = anchor_idxs[j]; + x::Quaternion Ca_q_G; + Ca_q_G.x() = C_q_G[anchor_idx].ax; + Ca_q_G.y() = C_q_G[anchor_idx].ay; + Ca_q_G.z() = C_q_G[anchor_idx].az; + Ca_q_G.w() = C_q_G[anchor_idx].aw; + + Vector3d G_p_Ca(G_p_C[anchor_idx].tx, G_p_C[anchor_idx].ty, G_p_C[anchor_idx].tz); + + // Coordinate of feature in global frame + Vector3d G_p_fj = 1 / (rho)*Ca_q_G.normalized().toRotationMatrix() * Vector3d(alpha, beta, 1) + G_p_Ca; + + // FOR LAST FEATURE OBSERVATION + x::Translation G_p_Cn(G_p_C.back()); + x::Attitude Cn_q_G(C_q_G.back()); + + x::Quatern attitude_to_quaternion; + Quaterniond Ci_q_G_ = attitude_to_quaternion(Cn_q_G); + Vector3d G_p_Ci_(G_p_Cn.tx, G_p_Cn.ty, G_p_Cn.tz); + + // Feature position expressed in camera frame. + Vector3d Ci_p_fj; + Ci_p_fj << Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_); + + // eq. 20(a) + Vector2d z; + const size_t track_size(track.size()); + const unsigned int i = track_size - 1; + z(0) = track[i].getX(); + z(1) = track[i].getY(); + + Vector2d z_hat(z); + assert(Ci_p_fj(2)); + z_hat(0) = Ci_p_fj(0) / Ci_p_fj(2); + z_hat(1) = Ci_p_fj(1) / Ci_p_fj(2); + + // eq. 20(b) + res_j(0, 0) = z(0) - z_hat(0); + res_j(1, 0) = z(1) - z_hat(1); + + //============================ + // Measurement Jacobian matrix + //============================ + + const unsigned int pos = C_q_G.size() - 1; + if (anchor_idx == pos) // Handle special case + { + // Inverse-depth feature coordinates jacobian + MatrixXd mat(MatrixXd::Zero(2, 3)); + mat(0, 0) = 1.0; + mat(1, 1) = 1.0; + + // Update stacked Jacobian matrices associated to the current feature + unsigned int row = 0; + unsigned int col = (n_poses_max * 2 + j) * kJacCols; + h_j.block(row, kSizeCoreErr + col) = mat; + } + else + { + // Set Jacobian of pose for i'th measurement of feature j (eq.22, 23) + VisJacBlock J_i(VisJacBlock::Zero()); + // first row + J_i(0, 0) = 1.0 / Ci_p_fj(2); + J_i(0, 1) = 0.0; + J_i(0, 2) = -Ci_p_fj(0) / std::pow((double)Ci_p_fj(2), 2); + // second row + J_i(1, 0) = 0.0; + J_i(1, 1) = 1.0 / Ci_p_fj(2); + J_i(1, 2) = -Ci_p_fj(1) / std::pow((double)Ci_p_fj(2), 2); + + // Attitude + Vector3d skew_vector = Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_); + VisJacBlock J_attitude = J_i * x::Skew(skew_vector(0), skew_vector(1), skew_vector(2)).matrix; + + // Position + VisJacBlock J_position = -J_i * Ci_q_G_.normalized().toRotationMatrix().transpose(); + + // Anchor attitude + VisJacBlock J_anchor_att = -1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() * + Ca_q_G.normalized().toRotationMatrix() * x::Skew(alpha, beta, 1).matrix; + + // Anchor position + VisJacBlock J_anchor_pos = -J_position; + + // Inverse-depth feature coordinates + MatrixXd mat(MatrixXd::Identity(3, 3)); + mat(0, 2) = -alpha / rho; + mat(1, 2) = -beta / rho; + mat(2, 2) = -1 / rho; + VisJacBlock Hf_j1 = 1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() * + Ca_q_G.normalized().toRotationMatrix() * mat; + + // Update stacked Jacobian matrices associated to the current feature + unsigned int row = 0; + const unsigned int pos = C_q_G.size() - 1; + unsigned int col = pos * kJacCols; + h_j.block(row, kSizeCoreErr + col) = J_position; + + col += n_poses_max * kJacCols; + h_j.block(row, kSizeCoreErr + col) = J_attitude; + + col = anchor_idx * kJacCols; + h_j.block(row, kSizeCoreErr + col) = J_anchor_pos; + + col += n_poses_max * kJacCols; + h_j.block(row, kSizeCoreErr + col) = J_anchor_att; + + col = (n_poses_max * 2 + j) * kJacCols; + h_j.block(row, kSizeCoreErr + col) = Hf_j1; + } + + //========================================================================== + // Outlier rejection + //========================================================================== + const VectorXd r_j_diag = var_img * VectorXd::Ones(2); + MatrixXd r_j = var_img * MatrixXd::Identity(2, 2); + MatrixXd S_inv = (h_j * P * h_j.transpose() + r_j).inverse(); + MatrixXd gamma = res_j.transpose() * S_inv * res_j; + boost::math::chi_squared_distribution<> my_chisqr(2 * track_size); + double chi = quantile(my_chisqr, 0.9); // 95-th percentile + + if (gamma(0, 0) < chi) // Inlier + { + jac_.block(row_h, // startRow + 0, // startCol + 2, // numRows + cols) = h_j; // numCols + + // Residual vector (one feature) + res_.block(row_h, 0, 2, 1) = res_j; + + // Measurement covariance matrix + cov_m_diag_.segment(row_h, 2) = r_j_diag; + + row_h += 2; + } +} + +void SlamUpdate::computeInverseDepthsNew(const x::TrackList& new_trks, + const double rho_0, + MatrixXd& ivds) const +{ + const size_t n_new_slam_std_trks = new_trks.size(); + ivds = MatrixXd::Zero(n_new_slam_std_trks * 3, 1); + + // For each standard SLAM feature to init + for (size_t j = 0; j < n_new_slam_std_trks; ++j) { + // Compute inverse-depth coordinates of new feature state + // (last observation in track) + computeOneInverseDepthNew(new_trks[j].back(), rho_0, j, ivds); + } +} + +void SlamUpdate::computeOneInverseDepthNew(const x::Feature& feature, + const double rho_0, + const unsigned int& idx, + MatrixXd& ivds) const +{ + // Inverse-depth parameters anchored in last observation frame + const double alpha = feature.getX(); + const double beta = feature.getY(); + // const double rho = 1.0 / G_p_Ci.back().tz; // height-based init: + + ivds(3*idx) = alpha; + ivds(3*idx + 1) = beta; + ivds(3*idx + 2) = rho_0; +} diff --git a/src/x/vio/solar_update.cpp b/src/x/vio/solar_update.cpp new file mode 100644 index 0000000..e241464 --- /dev/null +++ b/src/x/vio/solar_update.cpp @@ -0,0 +1,94 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +using namespace x; +using namespace Eigen; + +SolarUpdate::SolarUpdate(const x::SunAngleMeasurement& angle, + const x::Quaternion& quat, + const MatrixXd& cov_s) +{ + // Initialize Kalman update matrices + const size_t cols = cov_s.cols(); + jac_ = MatrixXd::Zero(2, cols); + cov_m_diag_ = VectorXd::Ones(2); + res_ = MatrixXd::Zero(2, 1); + + // Construct update + processSunAngle(angle, quat); +} + +void SolarUpdate::processSunAngle(const x::SunAngleMeasurement& angle, + const x::Quaternion& quat) +{ + //======================== + // Calibration parameters + //======================== + // TODO(Harel) Import these from param file + + // Sun angle measurement noise (variance in deg^2) + const double var_sun_angle = 10000 * 0.01777777777; + + // Sun sensor orientation relative to IMU + x::Quaternion S_q_I(0.360346005598587, -0.063338979194957, 0.007502445522018, 0.930635612981541); + + // Sun vector in world frames (should be calibrated before each flight) + // z+ points to zenith, x+ points north + Vector3d G_sun(-0.29385515271891938, -0.55080445540063927, 0.78119370269565391); + G_sun.normalize(); + + //========== + // Residual + //========== + Vector2d angles(angle.x_angle, angle.y_angle); + Vector3d S_sun_hat = S_q_I.normalized().toRotationMatrix().transpose() * + quat.normalized().toRotationMatrix().transpose() * + G_sun; + S_sun_hat.normalize(); + constexpr double RAD2DEG = 57.2957795130; + Vector2d angles_hat(RAD2DEG * std::atan2(S_sun_hat(0), S_sun_hat(2)), + RAD2DEG * std::atan2(S_sun_hat(1), S_sun_hat(2))); + + res_ = angles - angles_hat; + + //============================ + // Measurement Jacobian matrix + //============================ + + MatrixXd mat(MatrixXd::Zero(2, 3)); + mat(0, 0) = S_sun_hat(2) / (std::pow(S_sun_hat(0), 2) + std::pow(S_sun_hat(2), 2)); + mat(1, 1) = S_sun_hat(2) / (std::pow(S_sun_hat(1), 2) + std::pow(S_sun_hat(2), 2)); + mat(0, 2) = -S_sun_hat(0) / (std::pow(S_sun_hat(0), 2) + std::pow(S_sun_hat(2), 2)); + mat(1, 2) = -S_sun_hat(1) / (std::pow(S_sun_hat(1), 2) + std::pow(S_sun_hat(2), 2)); + + Vector3d skew_vector = quat.normalized().toRotationMatrix().transpose() * G_sun; + + SolarUpdate::SunAngleJacobian J_attitude = RAD2DEG * mat * S_q_I.normalized().toRotationMatrix().transpose() * + x::Skew(skew_vector(0), skew_vector(1), skew_vector(2)).matrix; + + jac_.block<2, kJacCols>(0, kIdxQ) = J_attitude; + + //================== + // Noise covariance + //================== + + cov_m_diag_ = var_sun_angle * VectorXd::Ones(2); +} diff --git a/src/x/vio/state_manager.cpp b/src/x/vio/state_manager.cpp new file mode 100644 index 0000000..e97aef6 --- /dev/null +++ b/src/x/vio/state_manager.cpp @@ -0,0 +1,634 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +using namespace x; + +void StateManager::clear() +{ + n_poses_ = 0; + n_features_ = 0; + std::vector pose_times(n_poses_max_, -1); + std::vector persistent_features_anchor_idx(n_features_max_, -1); + anchor_idxs_ = persistent_features_anchor_idx; + + stateHasBeenFilledBefore_ = false; +} + +void StateManager::manage(State& state, + std::vector del_feat_idx) { + // Retrieve current sliding-windows poses + Eigen::VectorXd att = state.getOrientationArray(); + Eigen::VectorXd pos = state.getPositionArray(); + + // Retrieve latest state estimate + Quaternion cae = state.computeCameraOrientation(); + Vector3 cpe = state.computeCameraPosition(); + + // Retrieve latest covariance + Matrix cov = state.getCovariance(); + + // Access the current feature state vector and initialize the new one + Eigen::VectorXd new_features = state.getFeatureArray(); + + //============================================================================ + // Persistent feature removal + //============================================================================ + + // For each feature to remove + std::sort (del_feat_idx.begin(), del_feat_idx.end()); + size_t i = del_feat_idx.size(); + while(i) + { + // Current feature index the feature states + const unsigned int idx = del_feat_idx[i-1]; + + //===================== + // State vector removal + //===================== + // Remove it from the feature state vector and + // shift upward those coming after it + const unsigned int n1 = n_features_ - idx - 1; // number of coming features after it + new_features.block(idx * 3, 0, n1 * 3, 1) = + new_features.block((idx + 1) * 3, 0, n1 * 3, 1); + new_features.block((n_features_ - 1) * 3, 0, 3, 1) << 0u, 0u, 0u; + + //=============== + // Anchor removal + //=============== + anchor_idxs_.erase(anchor_idxs_.begin() + idx); + anchor_idxs_.push_back(-1); + + //========================== + // Covariance matrix removal + //========================== + + // Actual size of covariance matrix + const size_t n = cov.rows(); + // Starting index of row/col of the feature to be deleted + const unsigned int idx0 = 21 + n_poses_max_ * 6 + idx * 3; + // Starting index of row/col after the feature to be deleted + const unsigned int idx1 = idx0 + 3; + // Number of active feature cols/rows after the one to be deleted + const size_t dim0 = (n_features_max_-idx-1) * 3; + + // Draw the block matrix makes it easier the understand the following + // Retrieve columns after the feature to be removed + Matrix cols_after = cov.block(0, + idx1, + n, + dim0); + + // Remove the rows corresponding to the removed feature in them + cols_after.block(idx0, + 0, + dim0, + dim0) + = cols_after.block(idx1, + 0, + dim0, + dim0); + + // Retrieve rows after the feature to be removed + // No need to retrieve the column block after the feature as it is already + // stored in cols_after + Matrix rows_after = cov.block(idx1, + 0, + dim0, + idx0); + + // Shift cols_after and rows_after one index up in the covariance matrix + cov.block(0, + idx0, + n, + dim0) = cols_after; + cov.block(idx0, + 0, + dim0, + idx0) = rows_after; + + // Replace the last 3 columns and rows of the covariance matrix with zeros + cov.block(0, n-3, n, 3) = Matrix::Zero(n, 3); + cov.block(n-3, 0, 3, n) = Matrix::Zero(3, n); + + // Update counts + i--; + n_features_--; + } + + //============================================================================ + // Sliding window and persistent feature reparametrization + //============================================================================ + + // If there is no slot available in the sliding window, slide it up + if (n_poses_ == n_poses_max_) + { + // Reparametrize persistent features anchored to the oldest pose + reparametrizeFeatures(att, pos, new_features, cov); + + // Slide the window of poses + slideWindow(att, pos, cov); + } + + //============================================================================ + // Pose augmentation + //============================================================================ + + //TODO(jeff): Write a unit test looking for zeros in the sliding window + // states. Can detect augmentation propagation errors. + att.block<4, 1>(n_poses_ * 4, 0) << cae.x(), cae.y(), cae.z(), cae.w(); + pos.block<3, 1>(n_poses_ * 3, 0) << cpe.x(), cpe.y(), cpe.z(); + + // Augment the state covariance matrix with the new pose + augmentCovariance(state, n_poses_, cov); + + n_poses_++; + + //============================================================================ + // Finalize changes + //============================================================================ + + state.setCovariance(cov); + state.setOrientationArray(att); + state.setPositionArray(pos); + state.setFeatureArray(new_features); +} + +void StateManager::initMsckfSlamFeatures(State& state, + const x::MsckfSlamMatrices& init_mats, + const Matrix& correction, + const double sigma_img) +{ + // Error covariance matrix + Matrix P = state.getCovariance(); + + // Update feature state using Li (2012) + // Note: H2 is singular when camera is in perfect hovering + // (and MSCKF cannot be applied) + const Matrix H2_inv = init_mats.H2.inverse(); + const Matrix H2_inv_H1 = H2_inv * init_mats.H1; + const Matrix new_features = init_mats.features + - H2_inv_H1 * correction + H2_inv * init_mats.r1; + + // Compute new covariance blocks + const double var_img = sigma_img * sigma_img; + Matrix P_cross_new = - H2_inv_H1 * P; + Matrix P_diag_new = H2_inv_H1 * P * H2_inv_H1.transpose() + + var_img * H2_inv * H2_inv.transpose(); + + // Add to state and covariance + addFeatureStates(state, new_features, P_diag_new, P_cross_new); +} + +void StateManager::initStandardSlamFeatures(State& state, + const Matrix& new_features, + const double sigma_img, + const double sigma_rho_0) +{ + const size_t n_new_states ( new_features.size() ); + const size_t n ( state.getCovariance().rows() ); + + // No correlation between new feature states and other states + Matrix P_cross_new = Matrix::Zero(n_new_states, n); + + // Compute initial feature variances from standard deviations + const double var_img = sigma_img * sigma_img; + const double var_rho_0 = sigma_rho_0 * sigma_rho_0; + + // Inverse-depth set like in Civera, alpha and beta follow image noise + Matrix P_diag_new = var_img + * Matrix::Identity(n_new_states, n_new_states); + for(unsigned int i=0; i < n_new_states / 3; i++) + P_diag_new(3*i + 2, 3*i + 2) = var_rho_0; + + // Add to state and covariance + addFeatureStates(state, new_features, P_diag_new, P_cross_new); +} + +void StateManager::addFeatureStates(State& state, + const Matrix& new_features, + const Matrix& cov, + const Matrix& cross) +{ + // Determine number of new states + const size_t n_new_states = new_features.size(); + + // Append new feature at the end of the state vector + Eigen::VectorXd features = state.getFeatureArray(); + assert(n_features_ < n_features_max_); + features.block(n_features_ * 3, 0, n_new_states, 1) = new_features; + state.setFeatureArray(features); + + // Augment covariance matrix + Matrix& P =state.getCovarianceRef(); + const size_t n = P.rows(); + const size_t n_states = 21 + n_poses_max_ * 6 + n_features_ * 3; + P.block(n_states, 0, n_new_states, n) = cross; + P.block(0, n_states, n, n_new_states) = cross.transpose(); + P.block(n_states, n_states, n_new_states, n_new_states) = cov; + + // Add current pose as anchor the new SLAM features + const size_t n_new_features( n_new_states / 3 ); + + for(unsigned int i=0; i + StateManager::computeSLAMCartesianFeaturesForState( + const State& state) const +{ + // Retrieve the required states + const Matrix& atts = state.getOrientationArray(); + const Matrix& poss = state.getPositionArray(); + const Matrix& features = state.getFeatureArray(); + + // Declare array to fill with the cartesian coordinates of the SLAM features + std::vector features_xyz(n_features_); + + // For each persistent feature state + for (unsigned int i = 0; i < n_features_; i++) + { + // Inverse-depth coordinates in anchor pose + double alpha = features(3 * i, 0); + double beta = features(3 * i + 1, 0); + double rho = features(3 * i + 2, 0); + + // Index of the anchor in the pose states + unsigned int idx = anchor_idxs_[i]; + + // Anchor pose + x::Quaternion Ci_q_G; + Ci_q_G.x() = atts(4 * idx, 0); + Ci_q_G.y() = atts(4 * idx + 1, 0); + Ci_q_G.z() = atts(4 * idx + 2, 0); + Ci_q_G.w() = atts(4 * idx + 3, 0); + + Vector3 G_p_Ci = poss.block<3, 1>(idx * 3, 0); + + // Convert to world coordinates + Eigen::Vector3d G_p_fi = + 1 / (rho) + * Ci_q_G.normalized().toRotationMatrix() + * Eigen::Vector3d(alpha, beta, 1) + G_p_Ci; + + // Populate the array + features_xyz[i] = G_p_fi; + } + + return features_xyz; +} + +void StateManager::augmentCovariance(const State& state, + const int pos, + Matrix& covariance) +{ + Matrix jacobianCamPoseWrtStates; + if (not stateHasBeenFilledBefore_) { + jacobianCamPoseWrtStates = Matrix::Zero(21 + n_poses_max_ * 6 + n_features_max_ * 3, + 21 + n_poses_max_ * 6 + n_features_max_ * 3); + } else { + jacobianCamPoseWrtStates = Matrix::Identity(21 + n_poses_max_ * 6 + n_features_max_ * 3, + 21 + n_poses_max_ * 6 + n_features_max_ * 3); + } + + // COVARIANCE AUGMENTATION + // Unchanged: previous Core and cam position + jacobianCamPoseWrtStates.block(0, 0, 21 + (pos + 1) * 3, 21 + (pos + 1) * 3) = + Matrix::Identity(21 + (pos + 1) * 3, 21 + (pos + 1) * 3); + + // Unchanged: previous cam attitude + jacobianCamPoseWrtStates.block(21 + n_poses_max_ * 3, + 21 + n_poses_max_ * 3, + (pos + 1) * 3, + (pos + 1) * 3) + = Matrix::Identity((pos + 1) * 3, (pos + 1) * 3); + + // Unchanged: features + jacobianCamPoseWrtStates.block(21 + n_poses_max_ * 6, + 21 + n_poses_max_ * 6, + n_features_ * 3, + n_features_ * 3) + = Matrix::Identity(n_features_ * 3, n_features_ * 3); + + // cam_position::G_p_I + // Derivative of camera position error wrt imu position error = I3x3 + jacobianCamPoseWrtStates.block(21 + pos*3, 0, 3, 3) = Matrix::Identity(3, 3); + + // Derivative of camera position error wrt imu orientation error = - C(qI2G) * [p_ic x] + Vector3 pci = state.getPositionExtrinsics(); + jacobianCamPoseWrtStates.block(21 + pos*3, 6, 3, 3) + = - state.getOrientation().normalized().toRotationMatrix() + * pci.toCrossMatrix(); + + // Derivative of camera attitude error wrt imu orientation error = C(qI2C) + jacobianCamPoseWrtStates.block(21 + n_poses_max_ * 3 + pos * 3, 6, 3, 3) = + state.getOrientationExtrinsics().conjugate().normalized().toRotationMatrix(); + + Matrix P_current = covariance; + + // Delete rows associated with position + P_current.block(21 + pos * 3, 0, 3, P_current.cols()) = + Matrix::Zero(3, P_current.cols()); + // Delete rows associated with attitude + P_current.block(21 + n_poses_max_ * 3 + pos * 3, 0, 3, P_current.cols()) = + Matrix::Zero(3, P_current.cols()); + + // Delete cols associated with position + P_current.block(0, 21 + pos * 3, P_current.rows(), 3) = + Matrix::Zero(P_current.cols(), 3); + // Delete cols associated with attitude + P_current.block(0, 21 + n_poses_max_ * 3 + pos * 3, P_current.rows(), 3) = + Matrix::Zero(P_current.cols(), 3); + + // If the next pose will be out of bounds + if (pos + 1 == n_poses_max_) + stateHasBeenFilledBefore_ = true; + + // Calculate the covariance incorporating the new camera poses + P_current = + jacobianCamPoseWrtStates * + P_current * + jacobianCamPoseWrtStates.transpose(); + + covariance = P_current; +} + +void StateManager::reparametrizeFeatures(Eigen::VectorXd const& atts_old, + Eigen::VectorXd const& poss_old, + Eigen::VectorXd& features, + Matrix& covariance) +{ + // Retrieve oldest pose in the sliding window + x::Quaternion Ci_q_G_old; + Ci_q_G_old.x() = atts_old[0]; + Ci_q_G_old.y() = atts_old[1]; + Ci_q_G_old.z() = atts_old[2]; + Ci_q_G_old.w() = atts_old[3]; + + Vector3 G_p_Ci_old = poss_old.block<3, 1>(0, 0); + + // Find indexes of features anchored to that pose + std::vector idx_to_chg; + for (unsigned int i = 0; i < n_features_; i++) + { + if (anchor_idxs_[i] == 0) + { + idx_to_chg.push_back(i); + } + } + + // Initialize covariance reparametrization matrix + const size_t n = 21 + n_poses_max_ * 6 + n_features_max_ * 3; + Matrix J(Matrix::Identity(n,n)); + + // For each persistent feature whose anchor has to be changed + for (unsigned int i = 0; i < idx_to_chg.size(); i++) + { + //======================== + // State reparametrization + //======================== + + x::Quaternion Ci_q_G_new; + const unsigned int idx1 = n_poses_max_ - 1; + Ci_q_G_new.x() = atts_old[4 * idx1]; + Ci_q_G_new.y() = atts_old[4 * idx1 + 1]; + Ci_q_G_new.z() = atts_old[4 * idx1 + 2]; + Ci_q_G_new.w() = atts_old[4 * idx1 + 3]; + + const Vector3 G_p_Ci_new1 = poss_old.block<3, 1>(idx1 * 3, 0); + + // Old feature parameters + const unsigned int j = idx_to_chg[i]; + const double alpha_old = features[3 * j]; + const double beta_old = features[3 * j + 1]; + const double rho_old = features[3 * j + 2]; + + // Compute new feature parameters, Eq. 38 in Li (RSS supplementals, 2012) + const Vector3 new_params = + Ci_q_G_new.normalized().toRotationMatrix().transpose() + * (- G_p_Ci_new1 + G_p_Ci_old + + 1 / rho_old + * Ci_q_G_old.normalized().toRotationMatrix() + * Eigen::Vector3d(alpha_old, beta_old, 1) + ); + + const double rho_new = 1 / new_params[2]; + const double alpha_new = new_params[0] * rho_new; + const double beta_new = new_params[1] * rho_new; + + // Update feature states + features.block<3, 1>(j * 3, 0) << alpha_new, beta_new, rho_new; + + // New anchor is the last pose in the window + anchor_idxs_[j] = idx1; + + //==================================== + // Covariance matrix reparametrization + //==================================== + + // Old anchor attitude + Eigen::Vector3d skew_vector = + Eigen::Vector3d(alpha_old, beta_old, 1); + const StateManager::Jacobian J_anchor_att_old = + - 1 / rho_old + * Ci_q_G_new.normalized().toRotationMatrix().transpose() + * Ci_q_G_old.normalized().toRotationMatrix() + * x::Skew(skew_vector(0), + skew_vector(1), + skew_vector(2)).matrix; + + // New anchor attitude + skew_vector = + Ci_q_G_new.normalized().toRotationMatrix().transpose() * + ( + - G_p_Ci_new1 + G_p_Ci_old + + 1 / rho_old + * Ci_q_G_old.normalized().toRotationMatrix() + * Eigen::Vector3d(alpha_old, beta_old, 1) + ); + const StateManager::Jacobian J_anchor_att_new = + x::Skew(skew_vector(0), + skew_vector(1), + skew_vector(2)).matrix; + + // Old anchor position + const StateManager::Jacobian J_anchor_pos_old = + Ci_q_G_new.normalized().toRotationMatrix().transpose(); + + // New anchor position + const StateManager::Jacobian J_anchor_pos_new = + - Ci_q_G_new.normalized().toRotationMatrix().transpose(); + + // Old feature parameters + Matrix mat(Matrix::Identity(3,3)); + mat(0, 2) = - alpha_old / rho_old; + mat(1, 2) = - beta_old / rho_old; + mat(2, 2) = - 1 / rho_old; + const StateManager::Jacobian J_anchor_feat_old = + 1 / rho_old + * Ci_q_G_new.normalized().toRotationMatrix().transpose() + * Ci_q_G_old.normalized().toRotationMatrix() + * mat; + + // Initialize Jacobian of eq.(38) wrt to state in Li (supplementals, 2012) + Matrix A_j(Matrix::Zero(3,n)); + + unsigned int col = idx1 * jac_cols; + + A_j.block(0, kSizeCoreErr + col) = + J_anchor_pos_new; + + col += n_poses_max_ * jac_cols; + A_j.block(0, kSizeCoreErr + col) = + J_anchor_att_new; + + col = 0; + A_j.block(0, kSizeCoreErr + col) = + J_anchor_pos_old; + + col += n_poses_max_ * jac_cols; + A_j.block(0, kSizeCoreErr + col) = + J_anchor_att_old; + + col = j * 3; + A_j.block(0, kSizeCoreErr + n_poses_max_ * 6 + col) = + J_anchor_feat_old; + + // Populate covariance reparametrization matrix + mat = Matrix::Identity(3,3); + mat(0, 2) = - alpha_new; + mat(1, 2) = - beta_new; + mat(2, 2) = - rho_new; + + const size_t n1 = kSizeCoreErr + n_poses_max_ * 6 + j * 3; + J.block(n1, 0, 3, n) = rho_new * mat * A_j; + } + + // Reparametrize covariance + covariance = J * covariance * J.transpose(); +} + +void StateManager::slideWindow(Eigen::VectorXd& atts, + Eigen::VectorXd& poss, + Matrix& covariance) { + // Slide poses in the state vector + atts.block(0, 0, (n_poses_max_-1) * 4, 1) + = atts.block(4, 0, (n_poses_max_-1) * 4, 1); + poss.block(0, 0, (n_poses_max_-1) * 3, 1) + = poss.block(3, 0, (n_poses_max_-1) * 3, 1); + atts.block<4, 1>((n_poses_max_-1) * 4, 0) = Matrix::Zero(4, 1); + poss.block<3, 1>((n_poses_max_-1) * 3, 0) = Matrix::Zero(3, 1); + + // Slides poses in covariance matrix + const size_t n = covariance.rows(); + + Matrix left_mult = Matrix::Zero(n, n); + left_mult.block(0, 0, 21, 21) = Matrix::Identity(21, 21); + + if (n_features_max_) { + left_mult.block(21 + n_poses_max_ * 6, + 21 + n_poses_max_ * 6, + n_features_max_ * 3, + n_features_max_ * 3) + = Matrix::Identity(n_features_max_ * 3, n_features_max_ * 3); + } + + Matrix right_mult = left_mult; + + left_mult.block(21, 24, (n_poses_max_ - 1) * 3, (n_poses_max_ - 1) * 3) + = Matrix::Identity((n_poses_max_ - 1) * 3, (n_poses_max_ - 1) * 3); + + left_mult.block(21 + n_poses_max_ * 3, + 24 + n_poses_max_ * 3, + (n_poses_max_ - 1) * 3, + (n_poses_max_ - 1) * 3) + = Matrix::Identity((n_poses_max_ - 1) * 3, (n_poses_max_ - 1) * 3); + + right_mult.block(24, 21, (n_poses_max_ - 1) * 3, (n_poses_max_ - 1) * 3) + = Matrix::Identity((n_poses_max_ - 1) * 3, (n_poses_max_ - 1) * 3); + + right_mult.block(24 + n_poses_max_ * 3, + 21 + n_poses_max_ * 3, + (n_poses_max_ - 1) * 3, + (n_poses_max_ - 1) * 3) + = Matrix::Identity((n_poses_max_ - 1) * 3, (n_poses_max_ - 1) * 3); + + covariance = left_mult * covariance * right_mult; + + // Slide indexes of anchors + for (unsigned int i = 0; i < n_features_; i++) + { + anchor_idxs_[i]--; + } + + // Correction sliding window position index + n_poses_--; +} + +AttitudeList +StateManager::convertCameraAttitudesToList(const State& state, + const int max_size) const { + // Determine output list size + size_t size_out; + if(max_size) + size_out = std::min(max_size, n_poses_); + else + size_out = n_poses_; + + // Get camera orientation window from state + const Matrix orientation_array = state.getOrientationArray(); + + // Initialize list of n attitudes + AttitudeList attitude_list(size_out, x::Attitude()); + + // Assign each translation + const size_t start_idx(n_poses_ - size_out); + for (int i = start_idx; i < n_poses_; i++) { + const x::Attitude attitude(orientation_array(4*i, 0), + orientation_array(4*i+1, 0), + orientation_array(4*i+2, 0), + orientation_array(4*i+3, 0)); + attitude_list[i - start_idx] = attitude; + } + + return attitude_list; +} + +x::TranslationList StateManager::convertCameraPositionsToList(const State& state) const +{ + // Get camera position from state + const Matrix positions = state.getPositionArray(); + + // Initialize list of n translations + x::TranslationList position_list(n_poses_, x::Translation()); + + // Assign each translation + for (int i=0; i < n_poses_; i++) + { + const x::Translation translation(positions(3*i, 0), positions(3*i+1, 0), positions(3*i+2, 0)); + position_list[i] = translation; + } + + return position_list; +} diff --git a/src/x/vio/track_manager.cpp b/src/x/vio/track_manager.cpp new file mode 100644 index 0000000..271722b --- /dev/null +++ b/src/x/vio/track_manager.cpp @@ -0,0 +1,633 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +TrackManager::TrackManager() +{} + +TrackManager::TrackManager(const x::Camera& camera, const double min_baseline_n) +: camera_(camera) +, min_baseline_n_(min_baseline_n) +{} + +void TrackManager::setCamera(x::Camera camera) +{ + camera_ = camera; +} + +x::TrackList TrackManager::normalizeSlamTracks(const int size_out) const +{ + return camera_.normalize(slam_trks_, size_out); +} + +x::TrackList TrackManager::getMsckfTracks() const +{ + return msckf_trks_n_; +} + +x::TrackList TrackManager::getNewSlamStdTracks() const +{ + return new_slam_std_trks_n_; +} + +x::TrackList TrackManager::getNewSlamMsckfTracks() const +{ + return new_slam_msckf_trks_n_; + +} +void TrackManager::clear() +{ + slam_trks_.clear(); + new_slam_trks_.clear(); + lost_slam_idxs_.clear(); + opp_trks_.clear(); + msckf_trks_n_.clear(); +} + +void TrackManager::removePersistentTracksAtIndex(const unsigned int idx) { + slam_trks_.erase(slam_trks_.begin() + idx); +} + +void TrackManager::removeNewPersistentTracksAtIndexes( + const std::vector invalid_tracks_idx) { + // Note: Assumes indexes are in increasing order + + // For each feature to remove + size_t i = invalid_tracks_idx.size(); + while(i) + { + new_slam_trks_.erase(new_slam_trks_.begin() + invalid_tracks_idx[i-1]); + i--; + } +} + +std::vector TrackManager::getLostSlamTrackIndexes() const { + return lost_slam_idxs_; +} + +void TrackManager::manageTracks(x::MatchList& matches, + const x::AttitudeList cam_rots, + const int n_poses_max, + const int n_slam_features_max, + const int min_track_length, + x::TiledImage& img) { + // Append the new persistent tracks from last image to the persistent track + // list and clear the list for new ones + slam_trks_.insert(slam_trks_.end(), + new_slam_trks_.begin(), + new_slam_trks_.end()); + new_slam_trks_.clear(); + + // Array storing the new and current persistent track indexes per bin in + // preparation to spread them out through all the bins. The indexes assume + // the new persistent track list is appended after the persistent track one. + const unsigned int n_bins = img.getNTilesH() * img.getNTilesW(); + std::vector bin_track_idx[n_bins]; + + // Similar array storing only current persistent tracks per bin, but not + // updating the indexes after deletion so those can be used to remove the + // features from the state later on. + std::vector bin_track_idx_per[n_bins]; + + // Index of bin with max number of features + unsigned int idx_bin_max_per = 0; + + // Loop through all persistent tracks to determine if they are still active. + unsigned int t = 0; + unsigned int lost_per_count = 0; + lost_slam_idxs_.clear(); + while (t < slam_trks_.size()) + { + bool isTrackLost = true; + unsigned int m = 0; + while (m < matches.size()) + { + // Check if the last feature of the persistent track is the previous match + if (slam_trks_[t].back().getX() == matches[m].previous.getX() + && slam_trks_[t].back().getY() == matches[m].previous.getY()) + { + isTrackLost = false; // we found the track, so it is not lost + + // Find and store the current bin for this feature + x::Feature& feature = matches[m].current; + img.setTileForFeature(feature); + const unsigned int bin_nbr = + matches[m].current.getTileRow() * img.getNTilesW() + matches[m].current.getTileCol(); + bin_track_idx[bin_nbr].push_back(t); + bin_track_idx_per[bin_nbr].push_back(t + lost_per_count); + + // Update index of bin with max number of features if necessary + if (bin_track_idx[bin_nbr].size() > + bin_track_idx[idx_bin_max_per].size()) + { + idx_bin_max_per = bin_nbr; + } + + // Add the matched feature to the track + slam_trks_[t].push_back(matches[m].current); + matches.erase(matches.begin() + m); // erase it from the match list + + break; // leave the while loop on the matches + } + m++; // increment the match iterator + } + + if(isTrackLost) + { + // Add its index to lost tracks so it is deleted from the filter state and + // covariance matrix + lost_slam_idxs_.push_back(t + lost_per_count); + + // Erase the persistence feature track + slam_trks_.erase(slam_trks_.begin() + t); + + // Increment counter + lost_per_count++; + + continue; // continue to avoid incrementing track iterator t + } + t++; // increment the track iterator + } + + // Loop through all remaining matches: + // 1/ Extend current opportunistic tracks + // 2/ Convert full MSCKF tracks to persistent tracks if possible + // 3/ Fill up open persistent states with remaining matches + msckf_trks_n_.clear(); + x::TrackList previous_opp_trks = opp_trks_; + opp_trks_.clear(); + unsigned int m = 0; + while (m < matches.size()) // Matches are in descending FAST order + { + t = 0; + bool isTrackLost = true; + while (t < previous_opp_trks.size()) + { + // Check if the last feature of the opportunistic track is the previous match + if (previous_opp_trks[t].back().getX() == matches[m].previous.getX() + && previous_opp_trks[t].back().getY() == matches[m].previous.getY()) + { + isTrackLost = false; // we found the track, so it is not lost + // Add the matched feature to the track + previous_opp_trks[t].push_back(matches[m].current); + break; // leave the while loop on the opportunistic tracks + } + t++; // increment the opportunistic track iterator + } + + if(!isTrackLost) // if the track is not lost + { + // Push it to the opportunistic tracks + opp_trks_.push_back(previous_opp_trks[t]); + // Remove from the previous opportunistic list + previous_opp_trks.erase(previous_opp_trks.begin() + t); + } + else // New opportunistic track! + { + x::Track opp_track; + opp_track.push_back(matches[m].previous); + opp_track.push_back(matches[m].current); + opp_trks_.push_back(opp_track); + } + m++; + } + + // Sort remaining tracks by length, longest to shortest + std::sort(opp_trks_.begin(), opp_trks_.end(), [](const x::Track & a, const x::Track & b){ return a.size() > b.size(); }); + t = 0; + while (t < opp_trks_.size()) // Tracks are sorted by length, longest to shortest + { + // Find and store current bin for this feature + x::Feature& feature = opp_trks_[t].back(); + img.setTileForFeature(feature); + const unsigned int bin_nbr = + feature.getTileRow() * img.getNTilesW() + feature.getTileCol(); + + // If track is long enough (quality metric) + if (opp_trks_[t].size() > min_track_length - 1) + { + // If persistent features spots are available + if((slam_trks_.size() + new_slam_trks_.size()) < n_slam_features_max) + { + // Push the track to the persistent list + new_slam_trks_.push_back(opp_trks_[t]); + opp_trks_.erase(opp_trks_.begin() + t); + bin_track_idx[bin_nbr].push_back(slam_trks_.size() + new_slam_trks_.size() - 1); + + // Update index of bin with max number of features if necessary + if (bin_track_idx[bin_nbr].size() > + bin_track_idx[idx_bin_max_per].size()) + { + idx_bin_max_per = bin_nbr; + } + } + else // determine if another persistent feature should be removed to + // spread out their distribution + { + // If there is a difference of more than one feature between the + // count of the current bin and the one with the maximum count + if (bin_track_idx[idx_bin_max_per].size() > + bin_track_idx[bin_nbr].size() + 1) + { + // Remove youngest track from the max bin + // If it is a new persistent feature + if (bin_track_idx[idx_bin_max_per].back() >= slam_trks_.size()) + { + new_slam_trks_.erase(new_slam_trks_.begin() + bin_track_idx[idx_bin_max_per].back() - slam_trks_.size()); + } + else + { + //TODO(jeff): This persistent track should still be processed and be removed after + // the update + lost_slam_idxs_.push_back(bin_track_idx_per[idx_bin_max_per].back()); + bin_track_idx_per[idx_bin_max_per].pop_back(); + slam_trks_.erase(slam_trks_.begin() + bin_track_idx[idx_bin_max_per].back()); + } + + // Adjust indexes + for (unsigned int i = 0; i < n_bins; i++) + { + for (unsigned int j = 0; j < bin_track_idx[i].size(); j++) + { + if (bin_track_idx[i][j] > bin_track_idx[idx_bin_max_per].back()) + { + bin_track_idx[i][j]--; + } + } + } + + // Remove index + bin_track_idx[idx_bin_max_per].pop_back(); + + // Add current track + new_slam_trks_.push_back(opp_trks_[t]); + opp_trks_.erase(opp_trks_.begin() + t); + bin_track_idx[bin_nbr].push_back(slam_trks_.size() + new_slam_trks_.size() - 1); + + // Update index of bin with max number of features + for (unsigned int i = 0; i < n_bins; i++) + { + if (bin_track_idx[i].size() > + bin_track_idx[idx_bin_max_per].size()) + { + idx_bin_max_per = i; + } + } + } + else + { + // If the track is too long + if (opp_trks_[t].size() > n_poses_max - 1) + { + // Normalize track (and crop it if it longer than the attitude list) + x::Track normalized_track = camera_.normalize(opp_trks_[t], cam_rots.size()); + if(checkBaseline(normalized_track, cam_rots)) + msckf_trks_n_.push_back(normalized_track); + + opp_trks_.erase(opp_trks_.begin() + t); + } + else + t++; // increment the match iterator + } + } + } + else + t++; // increment the match iterator + } + + + // Sort new SLAM tracks: + // - semi-infinite depth uncertainty (standard SLAM) or, + // - MSCKF-SLAM + new_slam_std_trks_n_.clear(); // clean last image's data + new_slam_msckf_trks_n_.clear(); + + x::TrackList new_slam_std_trks, new_slam_msckf_trks; // see use below + + for(size_t i=0; i TrackManager::featureTriangleAtPoint(const x::Feature& lrf_img_pt, x::TiledImage& img) const +{ + /******************************************* + * Delaunay triangulation on SLAM features * + *******************************************/ + + // Rectangle to be used with Subdiv2D (account +/- 0.5 offset on coordinates at edges) + const cv::Size size = img.size(); + const cv::Rect rect(-1, -1, size.width+2, size.height+2); + + // Create an instance of Subdiv2D + cv::Subdiv2D subdiv(rect); + // Create a vector of point to store SLAM features + std::vector points; + + // Add SLAM feature to Delaune triangulation + for (unsigned int i = 0; i < slam_trks_.size(); i++) + { + const x::Feature feature(slam_trks_[i].back()); + points.push_back(cv::Point2d(feature.getXDist(),feature.getYDist())); + cv::Point2d pt(feature.getXDist(),feature.getYDist()); + subdiv.insert(pt); + } + + // Define colors for drawing. + const cv::Scalar delaunay_color(255,255,255); + + // Draw delaunay triangles + draw_delaunay( img, subdiv, delaunay_color ); + + // Define color for drawing the LRF triangle + const cv::Scalar delaunay_color_selected(0,0,255); + + // Find triangle edge to the right of the LRF image point + int edge, vertex; + cv::Point2d lrf_cv_pt(lrf_img_pt.getXDist(), lrf_img_pt.getYDist()); + const int status = subdiv.locate(lrf_cv_pt, edge, vertex); + + //Plot LRF image point + x::Feature lrf_img_feature; + lrf_img_feature.setXDist(lrf_cv_pt.x); + lrf_img_feature.setYDist(lrf_cv_pt.y); + img.plotFeature(lrf_img_feature, delaunay_color_selected); + + /****************************************** + * Find SLAM feature IDs for the triangle * + ******************************************/ + //Note: The first 4 points of the subdivision correspond to the edges of the image. If they are one of the triangle's + // vertex, that triangle is not valid. + + std::vector tr_feat_ids = {-1,-1,-1}; + + // If lrf falls inside or on the edge of a triangle + if(status == cv::Subdiv2D::PTLOC_INSIDE || status == cv::Subdiv2D::PTLOC_ON_EDGE) + { + // Origin vertex of the edge + tr_feat_ids[0] = subdiv.edgeOrg(edge) - 4; + // Destination vertex of the edge + tr_feat_ids[1] = subdiv.edgeDst(edge) - 4; + // Destination vertex of the next edge => Triangle is complete + tr_feat_ids[2] = subdiv.edgeDst(subdiv.nextEdge(edge)) - 4; + } + // else if it falls on one of the vertices + else if(status == cv::Subdiv2D::PTLOC_VERTEX) + { + // LRF vertex + tr_feat_ids[0] = vertex - 4; + // Destination vertex of the first edge in the list of the current vertex + subdiv.getVertex(vertex, &edge); + tr_feat_ids[1] = subdiv.edgeDst(edge) - 4; + // Destination vertex of the next edge => Triangle is complete + tr_feat_ids[2] = subdiv.edgeDst(subdiv.nextEdge(edge)) - 4; + } + // else there was an input error + else + { + tr_feat_ids.clear(); + return tr_feat_ids; + } + + // Plot selected triangle + subdiv = cv::Subdiv2D(rect); + + for (unsigned int i = 0; i < 3; i++) + { + // The first 4 points of the subdivision correspond to the edges of the image. If they are one of the triangle's + // vertex, that triangle is not valid. + if( tr_feat_ids[i] > -1) + { + const x::Feature feature(slam_trks_[tr_feat_ids[i]].back()); + points.push_back(cv::Point2d(feature.getXDist(),feature.getYDist())); + subdiv.insert(cv::Point2d(feature.getXDist(),feature.getYDist())); + } + else // invalid triangle + { + tr_feat_ids.clear(); + return tr_feat_ids; + } + } + + // Plot triangle + draw_delaunay( img, subdiv, delaunay_color_selected ); + + return tr_feat_ids; +} + +// Draw delaunay triangles +void TrackManager::draw_delaunay( cv::Mat& img, cv::Subdiv2D& subdiv, cv::Scalar delaunay_color ) const +{ + std::vector triangleList; + subdiv.getTriangleList(triangleList); + std::vector pt(3); + cv::Size size = img.size(); + cv::Rect rect(0,0, size.width, size.height); + + for( size_t i = 0; i < triangleList.size(); i++ ) + { + cv::Vec6f t = triangleList[i]; + pt[0] = cv::Point(cvRound(t[0]), cvRound(t[1])); + pt[1] = cv::Point(cvRound(t[2]), cvRound(t[3])); + pt[2] = cv::Point(cvRound(t[4]), cvRound(t[5])); + + // Draw rectangles completely inside the image. + if ( rect.contains(pt[0]) && rect.contains(pt[1]) && rect.contains(pt[2])) + { +#if CV_MAJOR_VERSION == 4 + line(img, pt[0], pt[1], delaunay_color, 1, cv::LINE_AA, 0); + line(img, pt[1], pt[2], delaunay_color, 1, cv::LINE_AA, 0); + line(img, pt[2], pt[0], delaunay_color, 1, cv::LINE_AA, 0); +#else + line(img, pt[0], pt[1], delaunay_color, 1, CV_AA, 0); + line(img, pt[1], pt[2], delaunay_color, 1, CV_AA, 0); + line(img, pt[2], pt[0], delaunay_color, 1, CV_AA, 0); +#endif + } + } +} + +bool TrackManager::checkBaseline(const x::Track& track, const x::AttitudeList& C_q_G) const +{ + const int n_quats = C_q_G.size(); + const int n_obs = track.size(); + + // Index of the first and last observation of that feature in the + // pose vector + const int i_last = n_quats - 1; + const int i_first = i_last - n_obs + 1; + + // There must be at least 2 observations in the track to check the + // baseline AND the attitude list size must be greater or equal + // than the track's + assert(n_obs > 1 && n_quats >= n_obs); + + // Init + double min_feat_x = track[n_obs - 1].getX(); + double max_feat_x = track[n_obs - 1].getX(); + double min_feat_y = track[n_obs - 1].getY(); + double max_feat_y = track[n_obs - 1].getY(); + x::Quatern attitude_to_quaternion; + const Eigen::Quaterniond Cn_q_G = attitude_to_quaternion(C_q_G[i_last]); + + // For each observation, going backwards in time + // TODO(jeff): Make consistent wrt linear triangulation, e.g. + // for (int i = 0; i > -1; --i) + for (int i = i_first; i <= i_last; ++i) + { + // Rotate 3d ray coordinates in last camera frame + const Eigen::Quaterniond Ci_q_G = attitude_to_quaternion(C_q_G[i]); + const int i_trk = i - i_first; + const Eigen::Quaterniond Cn_q_Ci = Ci_q_G.normalized().conjugate() * Cn_q_G.normalized(); + const Eigen::Quaterniond q_ray_Ci(0.0, track[i_trk].getX(), track[i_trk].getY(), 1.0); + Eigen::Quaterniond q_ray_Cn = Cn_q_Ci.conjugate() * q_ray_Ci * Cn_q_Ci; + + // Normalized coordinates + const double feat_x = q_ray_Cn.x() / q_ray_Cn.z(); + const double feat_y = q_ray_Cn.y() / q_ray_Cn.z(); + + // Update min/max in X dimension + if (feat_x < min_feat_x) + min_feat_x = feat_x; + else if (feat_x > max_feat_x) + max_feat_x = feat_x; + + // Update min/max in Y dimension + if (feat_y < min_feat_y) + min_feat_y = feat_y; + else if (feat_y > max_feat_y) + max_feat_y = feat_y; + } + + const double delta_feat_x = max_feat_x - min_feat_x; + const double delta_feat_y = max_feat_y - min_feat_y; + + if (delta_feat_x > min_baseline_n_ || delta_feat_y > min_baseline_n_) + return true; + else + return false; +} + +void TrackManager::plotFeatures(x::TiledImage& img, const int min_track_length) +{ + // Convert image in a color image +#if CV_MAJOR_VERSION == 4 + cv::cvtColor(img, img, cv::COLOR_GRAY2BGR); +#else + cv::cvtColor(img, img, CV_GRAY2BGR); +#endif + + // Plot tiles + img.plotTiles(); + + cv::Scalar green(100,255,100); + cv::Scalar orange(0,204,249); + cv::Scalar blue(249,144,24); + + // Draw SLAM features + const size_t n_slam = slam_trks_.size(); + for (size_t ii = 0; ii < n_slam; ii++) + img.plotFeature(slam_trks_[ii][slam_trks_[ii].size() - 1], green); + const size_t n_new_slam = new_slam_trks_.size(); + for (size_t ii = 0; ii < n_new_slam; ii++) + img.plotFeature(new_slam_trks_[ii][new_slam_trks_[ii].size() - 1], green); + + // Draw opportunistic features + const size_t n_opp = opp_trks_.size(); + unsigned int count_pot = 0, count_opp = 0; + for (size_t ii = 0; ii < n_opp; ii++) + { + // Use track length to differentiate potential tracks, which are not long + // enough to be processed as an MSCKF measurement if lost. + const size_t track_sz = opp_trks_[ii].size(); + + if( track_sz >= min_track_length - 1) + { + count_opp++; + img.plotFeature(opp_trks_[ii].back(), orange); + } + else + { + count_pot++; + img.plotFeature(opp_trks_[ii].back(), blue); + } + } + + std::string slam_str = std::string("SLAM: ") + + std::to_string(n_slam + n_new_slam) + + std::string(" - "); + std::string oppStr = std::string("Opp: ") + + std::to_string(count_opp) + + std::string(" - MSCKF: ") + + std::to_string(msckf_trks_n_.size()); + std::string potStr = std::string(" - Pot: ") + + std::to_string(count_pot); + + cv::putText(img, slam_str, cv::Point((int) 10, (int) camera_.getHeight()-10), cv::FONT_HERSHEY_PLAIN, + 1.0, green, 1.5, 8, false); + + int baseline = 0; + cv::Size textSize = cv::getTextSize(slam_str, cv::FONT_HERSHEY_PLAIN, 1.0, 1.5, &baseline); + int offset = textSize.width; + cv::putText(img, oppStr, cv::Point((int) 10 + offset, (int) camera_.getHeight()-10), cv::FONT_HERSHEY_PLAIN, + 1.0, orange, 1.5, 8, false); + textSize = cv::getTextSize(oppStr, cv::FONT_HERSHEY_PLAIN, 1.0, 1.5, &baseline); + offset += textSize.width; + cv::putText(img, potStr, cv::Point((int) 10 + offset, (int) camera_.getHeight()-10), cv::FONT_HERSHEY_PLAIN, + 1.0, blue, 1.5, 8, false); +} diff --git a/src/x/vio/vio.cpp b/src/x/vio/vio.cpp new file mode 100644 index 0000000..1f18d1b --- /dev/null +++ b/src/x/vio/vio.cpp @@ -0,0 +1,386 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include + +// if Boost was compiled with BOOST_NO_EXCEPTIONS defined, it expects a user +// defined trow_exception function, so define a dummy here, if this is the case +#include + +using namespace x; + +namespace boost +{ +#ifdef BOOST_NO_EXCEPTIONS +void throw_exception(std::exception const & e) {}; // user defined +#endif +} + + +VIO::VIO() + : ekf_ { Ekf(vio_updater_) } +{ + // Initialize with invalid last range measurement + // todo: Replace -1 with -min_delay + x::RangeMeasurement no_measurement_range; + no_measurement_range.timestamp = -1; + last_range_measurement_ = no_measurement_range; + + // Initialize with invalid last sun angle measurement + // todo: Replace -1 with -min_delay + x::SunAngleMeasurement no_measurement_sun; + no_measurement_sun.timestamp = -1; + last_angle_measurement_ = no_measurement_sun; +} + +bool VIO::isInitialized() const { + return initialized_; +} + +void VIO::setUp(const x::Params& params) { + const x::Camera cam(params.cam_fx, params.cam_fy, params.cam_cx, params.cam_cy, params.cam_s, params.img_width, + params.img_height); + const x::Tracker tracker(cam, params.fast_detection_delta, params.non_max_supp, params.block_half_length, + params.margin, params.n_feat_min, params.outlier_method, params.outlier_param1, + params.outlier_param2); + + // Compute minimum MSCKF baseline in normal plane (square-pixel assumption) + msckf_baseline_n_ = params.msckf_baseline / (params.img_width * params.cam_fx); + + // Set up tracker and track manager + const TrackManager track_manager(cam, msckf_baseline_n_); + params_ = params; + camera_ = cam; + tracker_ = tracker; + track_manager_ = track_manager; + + // Set up VIO state manager + const int n_poses_state = params.n_poses_max; + const int n_features_state = params.n_slam_features_max; + const StateManager state_manager(n_poses_state, n_features_state); + state_manager_ = state_manager; + + // Gravity - TODO(jeff) Read from params + Vector3 g(0.0, 0.0, -9.81); + + // IMU noise + x::ImuNoise imu_noise; + imu_noise.n_w = params.n_w; + imu_noise.n_bw = params.n_bw; + imu_noise.n_a = params.n_a; + imu_noise.n_ba = params.n_ba; + + // Updater setup + x::MatchList matches; // unused empty match list since it's image callback + TiledImage img; + vio_updater_ = VioUpdater(tracker_, + state_manager_, + track_manager_, + params_.sigma_img, + params_.sigma_range, + params_.rho_0, + params_.sigma_rho_0, + params_.min_track_length, + params_.iekf_iter); + + // EKF setup + const size_t state_buffer_sz = 250; // TODO(jeff) Read from params + const State default_state = State(n_poses_state, n_features_state); + const double a_m_max = 50.0; + const unsigned int delta_seq_imu = 1; + const double time_margin_bfr = 0.02; + ekf_.set(vio_updater_, + g, + imu_noise, + state_buffer_sz, + default_state, + a_m_max, + delta_seq_imu, + time_margin_bfr); +} + +void VIO::setLastRangeMeasurement(x::RangeMeasurement range_measurement) { + last_range_measurement_ = range_measurement; +} + +void VIO::setLastSunAngleMeasurement(x::SunAngleMeasurement angle_measurement) { + last_angle_measurement_ = angle_measurement; +} + +State VIO::processImageMeasurement(double timestamp, + const unsigned int seq, + TiledImage& match_img, + TiledImage& feature_img) { + // Time correction + const double timestamp_corrected = timestamp + params_.time_offset; + + // Pass measurement data to updater + MatchList empty_list; // TODO(jeff) get rid of image callback and process match + // list from a separate tracker module. + VioMeasurement measurement(timestamp_corrected, + seq, + empty_list, + match_img, + last_range_measurement_, + last_angle_measurement_); + + vio_updater_.setMeasurement(measurement); + + // Process update measurement with xEKF + State updated_state = ekf_.processUpdateMeasurement(); + + // Set state timestamp to original image timestamp for ID purposes in output. + // We don't do that if that state is invalid, since the timestamp also carries + // the invalid signature. + if(updated_state.getTime() != kInvalid) + updated_state.setTime(timestamp); + + // Populate GUI image outputs + match_img = vio_updater_.getMatchImage(); + feature_img = vio_updater_.getFeatureImage(); + + return updated_state; +} + +State VIO::processMatchesMeasurement(double timestamp, + const unsigned int seq, + const std::vector& match_vector, + TiledImage& match_img, + TiledImage& feature_img) { + // Time correction + const double timestamp_corrected = timestamp + params_.time_offset; + + // Import matches (except for first measurement, since the previous needs to + // enter the sliding window) + x::MatchList matches; + if (vio_updater_.state_manager_.poseSize()) + matches = importMatches(match_vector, seq, match_img); + + // Compute 2D image coordinates of the LRF impact point on the ground + x::Feature lrf_img_pt; + lrf_img_pt.setXDist(320.5); + lrf_img_pt.setYDist(240.5); + camera_.undistort(lrf_img_pt); + last_range_measurement_.img_pt = lrf_img_pt; + last_range_measurement_.img_pt_n = camera_.normalize(lrf_img_pt); + + // Pass measurement data to updater + VioMeasurement measurement(timestamp_corrected, + seq, + matches, + feature_img, + last_range_measurement_, + last_angle_measurement_); + + vio_updater_.setMeasurement(measurement); + + // Process update measurement with xEKF + State updated_state = ekf_.processUpdateMeasurement(); + + // Set state timestamp to original image timestamp for ID purposes in output. + // We don't do that if that state is invalid, since the timestamp carries the + // invalid signature. + if(updated_state.getTime() != kInvalid) + updated_state.setTime(timestamp); + + // Populate GUI image outputs + match_img = match_img; + feature_img = vio_updater_.getFeatureImage(); + + return updated_state; +} + +/** Calls the state manager to compute the cartesian coordinates of the SLAM features. + */ +std::vector +VIO::computeSLAMCartesianFeaturesForState( + const State& state) { + return vio_updater_.state_manager_.computeSLAMCartesianFeaturesForState(state); +} + +void VIO::initAtTime(double now) { + ekf_.lock(); + initialized_ = false; + vio_updater_.track_manager_.clear(); + vio_updater_.state_manager_.clear(); + + // Initial IMU measurement (specific force, velocity) + Vector3 a_m, w_m; + a_m << 0.0, 0.0, 9.81; + w_m << 0.0, 0.0, 0.0; + + // Initial time cannot be 0, which may happen when using simulated Clock time + // before the first message has been received. + const double timestamp = + now > 0.0 ? now : std::numeric_limits::epsilon(); + + //////////////////////////////// xEKF INIT /////////////////////////////////// + + // Initial core covariance matrix + // TODO(jeff) read from params + const double sigma_dp_x = 0.0; + const double sigma_dp_y = 0.0; + const double sigma_dp_z = 0.0; + const double sigma_dv_x = 0.05; + const double sigma_dv_y = 0.05; + const double sigma_dv_z = 0.05; + const double sigma_dtheta_x = 3.0*M_PI/180.0; + const double sigma_dtheta_y = 3.0*M_PI/180.0; + const double sigma_dtheta_z = 3.0*M_PI/180.0; + const double sigma_dbw_x = 6.0*M_PI/180.0; + const double sigma_dbw_y = 6.0*M_PI/180.0; + const double sigma_dbw_z = 6.0*M_PI/180.0; + const double sigma_dba_x = 0.3; + const double sigma_dba_y = 0.3; + const double sigma_dba_z = 0.3; + const double sigma_dtheta_ic_x = 1.0 * M_PI / 180.0; + const double sigma_dtheta_ic_y = 1.0 * M_PI / 180.0; + const double sigma_dtheta_ic_z = 1.0 * M_PI / 180.0; + const double sigma_dp_ic_x = 0.01; + const double sigma_dp_ic_y = 0.01; + const double sigma_dp_ic_z = 0.01; + + const size_t n_poses_state = params_.n_poses_max; + const size_t n_features_state = params_.n_slam_features_max; + + // Initial vision state estimates and uncertainties are all zero + const Matrix p_array = Matrix::Zero(n_poses_state * 3, 1); + const Matrix q_array = Matrix::Zero(n_poses_state * 4, 1); + const Matrix f_array = Matrix::Zero(n_features_state * 3, 1); + const Eigen::VectorXd sigma_p_array = Eigen::VectorXd::Zero(n_poses_state * 3); + const Eigen::VectorXd sigma_q_array = Eigen::VectorXd::Zero(n_poses_state * 3); + const Eigen::VectorXd sigma_f_array = Eigen::VectorXd::Zero(n_features_state * 3); + + // Construct initial covariance matrix + const size_t n_err = kSizeCoreErr + n_poses_state * 6 + n_features_state * 3; + Eigen::VectorXd sigma_diag(n_err); + sigma_diag << sigma_dp_x, sigma_dp_y, sigma_dp_z, + sigma_dv_x, sigma_dv_y, sigma_dv_z, + sigma_dtheta_x, sigma_dtheta_y, sigma_dtheta_z, + sigma_dbw_x, sigma_dbw_y, sigma_dbw_z, + sigma_dba_x, sigma_dba_y, sigma_dba_z, + sigma_dtheta_ic_x, sigma_dtheta_ic_y, sigma_dtheta_ic_z, + sigma_dp_ic_x, sigma_dp_ic_y, sigma_dp_ic_z, + sigma_p_array, sigma_q_array, sigma_f_array; + + const Eigen::VectorXd cov_diag = sigma_diag.array() * sigma_diag.array(); + const Matrix cov = cov_diag.asDiagonal(); + + // Construct initial state + const unsigned int dummy_seq = 0; + State init_state(timestamp, + dummy_seq, + params_.p, + params_.v, + params_.q, + params_.b_w, + params_.b_a, + p_array, + q_array, + f_array, + cov, + params_.q_ic, + params_.p_ic, + w_m, + a_m); + + // Try to initialize the filter with initial state input + try { + ekf_.initializeFromState(init_state); + } catch (std::runtime_error& e) { + std::cerr << "bad input: " << e.what() << std::endl; + } catch (init_bfr_mismatch) { + std::cerr << "init_bfr_mismatch: the size of dynamic arrays in the " + "initialization state match must match the size allocated in " + "the buffered states." << std::endl; + } + ekf_.unlock(); + + initialized_ = true; +} + +/** \brief Gets 3D coordinates of MSCKF inliers and outliers. + * + * These are computed in the Measurement class instance. + */ +void VIO::getMsckfFeatures(x::Vector3dArray& inliers, + x::Vector3dArray& outliers) { + inliers = vio_updater_.getMsckfInliers(); + outliers = vio_updater_.getMsckfOutliers(); +} + +State VIO::processImu(const double timestamp, + const unsigned int seq, + const Vector3& w_m, + const Vector3& a_m) { + return ekf_.processImu(timestamp, seq, w_m, a_m); +} + +x::MatchList VIO::importMatches(const std::vector& match_vector, + const unsigned int seq, + x::TiledImage& img_plot) const { + // 9N match vector structure: + // 0: time_prev + // 1: x_dist_prev + // 2: y_dist_prev + // 3: time_curr + // 4: x_dist_curr + // 5: x_dist_curr + // 6,7,8: 3D coordinate of feature + + // Number of matches in the input vector + assert(match_vector.size() % 9 == 0); + const unsigned int n_matches = match_vector.size() / 9; + + // Declare new lists + x::MatchList matches(n_matches); + + // Store the match vector into a match list + for (unsigned int i = 0; i < n_matches; ++i) + { + // Undistortion + const double x_dist_prev = match_vector[9 * i + 1]; + const double y_dist_prev = match_vector[9 * i + 2]; + + const double x_dist_curr = match_vector[9 * i + 4]; + const double y_dist_curr = match_vector[9 * i + 5]; + + // Features and match initializations + x::Feature previous_feature(match_vector[9 * i] + params_.time_offset, seq - 1, 0.0, 0.0, x_dist_prev, + y_dist_prev); + camera_.undistort(previous_feature); + + x::Feature current_feature(match_vector[9 * i + 3] + params_.time_offset, seq, 0.0, 0.0, x_dist_curr, + y_dist_curr); + camera_.undistort(current_feature); + + x::Match current_match; + current_match.previous = previous_feature; + current_match.current = current_feature; + + // Add match to list + matches[i] = current_match; + } + + // Publish matches to GUI + x::Tracker::plotMatches(matches, img_plot); + + return matches; +} diff --git a/src/x/vio/vio_updater.cpp b/src/x/vio/vio_updater.cpp new file mode 100644 index 0000000..3228939 --- /dev/null +++ b/src/x/vio/vio_updater.cpp @@ -0,0 +1,360 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include + +using namespace x; + +VioUpdater::VioUpdater(Tracker& tracker, + StateManager& state_manager, + TrackManager& track_manager, + const double sigma_img, + const double sigma_range, + const double rho_0, + const double sigma_rho_0, + const int min_track_length, + const int iekf_iter) + : tracker_(tracker) + , state_manager_(state_manager) + , track_manager_(track_manager) + , sigma_img_ { sigma_img } + , sigma_range_ { sigma_range } + , rho_0_ { rho_0 } + , sigma_rho_0_ { sigma_rho_0 } + , min_track_length_ { min_track_length } +{ + iekf_iter_ = iekf_iter; +} + +double VioUpdater::getTime() const { + return measurement_.timestamp; +} + +TiledImage& VioUpdater::getMatchImage() { + return match_img_; +} + +TiledImage& VioUpdater::getFeatureImage() { + return feature_img_; +} + +Vector3dArray VioUpdater::getMsckfInliers() const { + return msckf_inliers_; +} + +Vector3dArray VioUpdater::getMsckfOutliers() const { + return msckf_outliers_; +} + +void VioUpdater::setMeasurement(const VioMeasurement& measurement) { + measurement_ = measurement; +} + +void VioUpdater::preProcess(const State& state) { + // If the length of matches_ is 0, that means we used the image constructor + // for the current object and the tracker needs to be run + if (measurement_.matches.size() == 0) { + // Track features + match_img_ = measurement_.image.clone(); + tracker_.track(match_img_, measurement_.timestamp, measurement_.seq); + + // If we are processing images and last image didn't go back in time + if (tracker_.checkMatches()) + measurement_.matches = tracker_.getMatches(); + } + + // Construct list of camera orientation states. + // Note: pose window has not been slid yet and only goes up to + // the previous frame. We need to crop the first pose out of the + // list and add the current pose manually. + // This is used to used check MSCKF track baseline. + const int n_poses_max = state.nPosesMax(); + const int list_sz = n_poses_max - 1; + AttitudeList cam_rots + = state_manager_.convertCameraAttitudesToList(state, list_sz); + const Attitude last_rot = state.computeCameraAttitude(); + cam_rots.push_back(last_rot); + + // Sort matches into tracks + feature_img_ = measurement_.image.clone(); + const int n_slam_features_max = state.nFeaturesMax(); + track_manager_.manageTracks(measurement_.matches, + cam_rots, + n_poses_max, + n_slam_features_max, + min_track_length_, + feature_img_); + + // Get image measurements + slam_trks_ = track_manager_.normalizeSlamTracks(n_poses_max); + msckf_trks_ = track_manager_.getMsckfTracks(); + new_slam_std_trks_ = track_manager_.getNewSlamStdTracks(); + new_msckf_slam_trks_ = track_manager_.getNewSlamMsckfTracks(); + + // Collect indexes of persistent tracks that were lost at this time step + lost_slam_trk_idxs_ = track_manager_.getLostSlamTrackIndexes(); +} + +bool VioUpdater::preUpdate(State& state) { + // Manage vision states to be added, removed, reparametrized or sled + state_manager_.manage(state, lost_slam_trk_idxs_); + + // Return true if there are any visual update measurements + return msckf_trks_.size() + || slam_trks_.size() + || new_slam_std_trks_.size() + || new_msckf_slam_trks_.size(); +} + +void VioUpdater::constructUpdate(const State& state, + Matrix& h, + Matrix& res, + Matrix& r) { + // Construct list of pose states (the pose window has been slid + // and includes current camera pose) + const TranslationList G_p_C + = state_manager_.convertCameraPositionsToList(state); + const AttitudeList C_q_G + = state_manager_.convertCameraAttitudesToList(state); + + // Retrieve state covariance prior + Matrix P = state.getCovariance(); + + // Set up triangulation for MSCKF // TODO(jeff) set from params + const unsigned int max_iter = 10; // Gauss-Newton max number of iterations + const double term = 0.00001; // Gauss-Newton termination criterion + const Triangulation triangulator(C_q_G, G_p_C, max_iter, term); + + /* MSCKF */ + + // Construct update + const int n_poses_max = state.nPosesMax(); + const MsckfUpdate msckf(msckf_trks_, + C_q_G, + G_p_C, + triangulator, + P, + n_poses_max, + sigma_img_); + + // Get matrices + const Matrix& res_msckf = msckf.getResidual(); + const Matrix& h_msckf = msckf.getJacobian(); + const Eigen::VectorXd& r_msckf_diag = msckf.getCovDiag(); + const Vector3dArray& msckf_inliers = msckf.getInliers(); + const Vector3dArray& msckf_outliers = msckf.getOutliers(); + const size_t rows_msckf = h_msckf.rows(); + + /* MSCKF-SLAM */ + + // Construct update + msckf_slam_ = MsckfSlamUpdate(new_msckf_slam_trks_, + C_q_G, + G_p_C, + triangulator, + P, + n_poses_max, + sigma_img_); + + // Get matrices + const Matrix& res_msckf_slam = msckf_slam_.getResidual(); + const Matrix& h_msckf_slam = msckf_slam_.getJacobian(); + const Eigen::VectorXd& r_msckf_slam_diag = msckf_slam_.getCovDiag(); + const Vector3dArray& msckf_slam_inliers = msckf_slam_.getInliers(); + const Vector3dArray& msckf_slam_outliers = msckf_slam_.getOutliers(); + const size_t rows_msckf_slam = h_msckf_slam.rows(); + + // Stack all MSCKF inliers/outliers + msckf_inliers_ = msckf_inliers; + msckf_inliers_.insert(msckf_inliers_.end(), + msckf_slam_inliers.begin(), + msckf_slam_inliers.end()); + + msckf_outliers_ = msckf_outliers; + msckf_outliers_.insert(msckf_outliers_.end(), + msckf_slam_outliers.begin(), + msckf_slam_outliers.end()); + + /* SLAM */ + + // Get SLAM feature priors and inverse-depth pose anchors + const Matrix& feature_states = state.getFeatureArray(); + const std::vector anchor_idxs = state_manager_.getAnchorIdxs(); + + // Contruct update + slam_ = SlamUpdate(slam_trks_, + C_q_G, + G_p_C, + feature_states, + anchor_idxs, + P, + n_poses_max, + sigma_img_); + const Matrix& res_slam = slam_.getResidual(); + const Matrix& h_slam = slam_.getJacobian(); + const Eigen::VectorXd& r_slam_diag = slam_.getCovDiag(); + const size_t rows_slam = h_slam.rows(); + + /* Range-SLAM */ + + size_t rows_lrf = 0; + Matrix h_lrf, res_lrf; + Eigen::VectorXd r_lrf_diag; + + if (measurement_.range.timestamp > 0.1 && slam_trks_.size()) { + // 2D image coordinates of the LRF impact point on the ground + Feature lrf_img_pt; + lrf_img_pt.setXDist(320.5); + lrf_img_pt.setYDist(240.5); + + // IDs of the SLAM features in the triangles surrounding the LRF + const std::vector tr_feat_ids = track_manager_.featureTriangleAtPoint(lrf_img_pt, feature_img_); + + // If we found a triangular facet to construct the range update + if (tr_feat_ids.size()) + { + // Contruct update + const RangeUpdate range_slam(measurement_.range, + tr_feat_ids, + C_q_G, + G_p_C, + feature_states, + anchor_idxs, + P, + n_poses_max, + sigma_range_); + res_lrf = range_slam.getResidual(); + h_lrf = range_slam.getJacobian(); + r_lrf_diag = range_slam.getCovDiag(); + rows_lrf = 1; + + // Don't reuse measurement + measurement_.range.timestamp = -1; + } + } + + /* Sun Sensor */ + + size_t rows_sns = 0; + Matrix h_sns, res_sns; + Eigen::VectorXd r_sns_diag; + + if (measurement_.sun_angle.timestamp > -1) { + // Retrieve body orientation + const Quaternion& att = state.getOrientation(); + + // Construct solar update + const SolarUpdate solar_update(measurement_.sun_angle, att, P); + res_sns = solar_update.getResidual(); + h_sns = solar_update.getJacobian(); + r_sns_diag = solar_update.getCovDiag(); + rows_sns = 2; + + // Don't reuse measurement + measurement_.sun_angle.timestamp = -1; + } + + /* Combined update */ + + const size_t rows_total + = rows_msckf + rows_msckf_slam + rows_slam + rows_lrf + rows_sns; + const size_t cols = P.cols(); + h = Matrix::Zero(rows_total, cols); + Eigen::VectorXd r_diag = Eigen::VectorXd::Ones(rows_total); + res = Matrix::Zero(rows_total, 1); + + h << h_msckf, + h_msckf_slam, + h_slam, + h_lrf, + h_sns; + + r_diag << r_msckf_diag, + r_msckf_slam_diag, + r_slam_diag, + r_lrf_diag, + r_sns_diag; + r = r_diag.asDiagonal(); + + res << res_msckf, + res_msckf_slam, + res_slam, + res_lrf, + res_sns; + + // QR decomposition of the update Jacobian + applyQRDecomposition(h, res, r); +} + +void VioUpdater::postUpdate(State& state, const Matrix& correction) { + // MSCKF-SLAM feature init + // Insert all new MSCKF-SLAM features in state and covariance + if (new_msckf_slam_trks_.size()) { + // TODO(jeff) Do not initialize features which have failed the + // Mahalanobis test. They need to be removed from the track + // manager too. + state_manager_.initMsckfSlamFeatures(state, + msckf_slam_.getInitMats(), + correction, + sigma_img_); + } + + // STANDARD SLAM feature initialization + if (new_slam_std_trks_.size()) { + // Compute inverse-depth coordinates of new SLAM features + Matrix features_slam_std; + slam_.computeInverseDepthsNew(new_slam_std_trks_, rho_0_, features_slam_std); + + // Insert it in state and covariance + state_manager_.initStandardSlamFeatures(state, + features_slam_std, + sigma_img_, + sigma_rho_0_); + } +} + +void VioUpdater::applyQRDecomposition(Matrix& h, Matrix& res, Matrix& R) +{ + // Check if the QR decomposition is actually necessary + unsigned int rowsh(h.rows()), colsh(h.cols()); + bool QR = rowsh > colsh + 1; + + // QR decomposition using Householder transformations (same computational + // complexity as Givens) + if (QR) + { + // Compute QR of the augmented [h|res] matrix to avoid forming Q1 + // explicitly (Dongarra et al., Linpack users's guide, 1979) + Matrix hRes(rowsh, colsh + 1); + hRes << h, res; + Eigen::HouseholderQR qr(hRes); + // Get the upper triangular matrix of the augmented hRes QR + Matrix Thz = qr.matrixQR().triangularView(); + + // Extract the upper triangular matrix of the h QR + h = Thz.block(0, 0, colsh, colsh); + // Extract the projected residual vector + res = Thz.block(0, colsh, colsh, 1); + // Form new Kalman update covariance matrix + const double var_img = sigma_img_ * sigma_img_; + R = var_img * Matrix::Identity(colsh, colsh); + } + // else we leave the inputs unchanged +} diff --git a/src/x/vision/camera.cpp b/src/x/vision/camera.cpp new file mode 100644 index 0000000..f6f8425 --- /dev/null +++ b/src/x/vision/camera.cpp @@ -0,0 +1,158 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +using namespace x; + +Camera::Camera() +{} + +Camera::Camera(double fx, + double fy, + double cx, + double cy, + double s, + unsigned int img_width, + unsigned int img_height) +: s_(s) +, img_width_(img_width) +, img_height_(img_height) +{ + fx_ = img_width * fx; + fy_ = img_height * fy; + cx_ = img_width * cx; + cy_ = img_height * cy; + + inv_fx_ = 1.0 / fx_; + inv_fy_ = 1.0 / fy_; + cx_n_ = cx_ * inv_fx_; + cy_n_ = cy_ * inv_fy_; + s_term_ = 1.0 / ( 2.0 * std::tan(s / 2.0) ); +} + +unsigned int Camera::getWidth() const +{ + return img_width_; +} + +unsigned int Camera::getHeight() const +{ + return img_height_; +} + +double Camera::getInvFx() const +{ + return inv_fx_; +} + +double Camera::getInvFy() const +{ + return inv_fy_; +} + +double Camera::getCxN() const +{ + return cx_n_; +} + +double Camera::getCyN() const +{ + return cy_n_; +} + +void Camera::undistort(FeatureList& features) const +{ + // Undistort each point in the input vector + for(unsigned int i = 0; i < features.size(); i++) + undistort(features[i]); +} + +void Camera::undistort(Feature& feature) const +{ + const double cam_dist_x = feature.getXDist() * inv_fx_ - cx_n_; + const double cam_dist_y = feature.getYDist() * inv_fy_ - cy_n_; + + const double dist_r = sqrt(cam_dist_x * cam_dist_x + cam_dist_y * cam_dist_y); + + double distortion_factor = 1.0; + if(dist_r > 0.01) + distortion_factor = inverseTf(dist_r) / dist_r; + + const double xn = distortion_factor * cam_dist_x; + const double yn = distortion_factor * cam_dist_y; + + feature.setX(xn * fx_ + cx_); + feature.setY(yn * fy_ + cy_); +} + +Feature Camera::normalize(const Feature& feature) const +{ + Feature normalized_feature(feature.getTimestamp(), + feature.getX() * inv_fx_ - cx_n_, + feature.getY() * inv_fy_ - cy_n_); + normalized_feature.setXDist(feature.getXDist() * inv_fx_ - cx_n_); + normalized_feature.setYDist(feature.getYDist() * inv_fx_ - cx_n_); + + return normalized_feature; +} + +/** \brief Normalized the image coordinates for all features in the input track + * \param track Track to normalized + * \param max_size Max size of output track, cropping from the end (default 0: no cropping) + * \return Normalized track + */ +Track Camera::normalize(const Track& track, const size_t max_size) const +{ + // Determine output track size + const size_t track_size = track.size(); + size_t size_out; + if(max_size) + size_out = std::min(max_size, track_size); + else + size_out = track_size; + + FeatureList normalized_track(size_out, Feature()); + const size_t start_idx(track_size - size_out); + for (size_t j = start_idx; j < track_size; ++j) + normalized_track[j - start_idx] = normalize(track[j]); + + return normalized_track; +} + +/** \brief Normalized the image coordinates for all features in the input track list + * \param tracks List of tracks + * \param max_size Max size of output tracks, cropping from the end (default 0: no cropping) + * \return Normalized list of tracks + */ +TrackList Camera::normalize(const TrackList& tracks, const size_t max_size) const +{ + const size_t n = tracks.size(); + TrackList normalized_tracks(n, FeatureList()); + + for (size_t i = 0; i < n; ++i) + normalized_tracks[i] = normalize(tracks[i], max_size); + + return normalized_tracks; +} + +double Camera::inverseTf(const double dist) const +{ + if(s_ == 0.0) + return dist; + else + return std::tan(dist * s_) * s_term_; +} diff --git a/src/x/vision/feature.cpp b/src/x/vision/feature.cpp new file mode 100644 index 0000000..15cc4df --- /dev/null +++ b/src/x/vision/feature.cpp @@ -0,0 +1,58 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +using namespace x; + +Feature::Feature() +{} + +Feature::Feature(double timestamp, + double x, + double y) +: timestamp_(timestamp) +, x_(x) +, y_(y) +{} + +Feature::Feature(double timestamp, + unsigned int frame_number, + double x, + double y, + double x_dist, + double y_dist) +: timestamp_(timestamp) +, frame_number_(frame_number) +, x_(x) +, y_(y) +, x_dist_(x_dist) +, y_dist_(y_dist) +{} + +Feature::Feature(double timestamp, + unsigned int frame_number, + double x_dist, + double y_dist, + unsigned int pyramid_level, + float fast_score) +: timestamp_(timestamp) +, frame_number_(frame_number) +, x_dist_(x_dist) +, y_dist_(y_dist) +, pyramid_level_(pyramid_level) +, fast_score_(fast_score) +{} diff --git a/src/x/vision/tiled_image.cpp b/src/x/vision/tiled_image.cpp new file mode 100644 index 0000000..fd5697e --- /dev/null +++ b/src/x/vision/tiled_image.cpp @@ -0,0 +1,218 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include + +using namespace x; + +TiledImage::TiledImage() : cv::Mat() +{ +} + +TiledImage::TiledImage(const Mat& cv_img) : cv::Mat(cv_img) +{ +} + +TiledImage::TiledImage(const TiledImage& img) + : cv::Mat(img) + , timestamp_(img.timestamp_) + , frame_number_(img.frame_number_) + , n_tiles_h_(img.n_tiles_h_) + , n_tiles_w_(img.n_tiles_w_) + , max_feat_per_tile_(img.max_feat_per_tile_) +{ + n_tiles_ = img.n_tiles_; + tile_height_ = img.tile_height_; + tile_width_ = img.tile_width_; + tiles_ = img.tiles_; + +} + +TiledImage::TiledImage(const Mat& img, double timestamp, unsigned int frame_number, unsigned int n_tiles_h, + unsigned int n_tiles_w, unsigned int max_feat_per_tile) + : cv::Mat(img) + , timestamp_(timestamp) + , frame_number_(frame_number) + , n_tiles_h_(n_tiles_h) + , n_tiles_w_(n_tiles_w) + , max_feat_per_tile_(max_feat_per_tile) +{ + n_tiles_ = n_tiles_h * n_tiles_w; + tile_height_ = (double)img.rows / n_tiles_h; // (rows, cols) are int in OpenCV! + tile_width_ = (double)img.cols / n_tiles_w; + tiles_ = std::vector(n_tiles_, 0); +} + +/** \brief Copy assignment operator + */ +TiledImage& TiledImage::operator=(const TiledImage& other) +{ + if (&other == this) + return *this; + + cv::Mat::operator=(other); + timestamp_ = other.timestamp_; + frame_number_ = other.frame_number_; + tile_height_ = other.tile_height_; + tile_width_ = other.tile_width_; + n_tiles_h_ = other.n_tiles_h_; + n_tiles_w_ = other.n_tiles_w_; + max_feat_per_tile_ = other.max_feat_per_tile_; + n_tiles_ = other.n_tiles_; + tiles_ = other.tiles_; + + return *this; +} + +TiledImage TiledImage::clone() const +{ + cv::Mat base(*this); + TiledImage clone(base.clone()); + + clone.timestamp_ = timestamp_; + clone.frame_number_ = frame_number_; + clone.tile_height_ = tile_height_; + clone.tile_width_ = tile_width_; + clone.n_tiles_h_ = n_tiles_h_; + clone.n_tiles_w_ = n_tiles_w_; + clone.max_feat_per_tile_ = max_feat_per_tile_; + clone.n_tiles_ = n_tiles_; + clone.tiles_ = tiles_; + + return clone; +} + +void TiledImage::setTileSize(const double tile_height, const double tile_width) +{ + tile_height_ = tile_height; + tile_width_ = tile_width; +} + +void TiledImage::setTileParams(unsigned int n_tiles_h, unsigned int n_tiles_w, unsigned int max_feat_per_tile) +{ + n_tiles_h_ = n_tiles_h; + n_tiles_w_ = n_tiles_w; + max_feat_per_tile_ = max_feat_per_tile; + n_tiles_ = n_tiles_h * n_tiles_w; + tile_height_ = (double)rows / n_tiles_h; // (rows, cols) are int in OpenCV! + tile_width_ = (double)cols / n_tiles_w; + tiles_ = std::vector(n_tiles_, 0); +} + +double TiledImage::getTimestamp() const +{ + return timestamp_; +} + +unsigned int TiledImage::getFrameNumber() const +{ + return frame_number_; +} + +double TiledImage::getTileHeight() const +{ + return tile_height_; +} + +double TiledImage::getTileWidth() const +{ + return tile_width_; +} + +unsigned int TiledImage::getNTilesH() const +{ + return n_tiles_h_; +} + +unsigned int TiledImage::getNTilesW() const +{ + return n_tiles_w_; +} + +unsigned int TiledImage::getMaxFeatPerTile() const +{ + return max_feat_per_tile_; +} + +unsigned int TiledImage::getFeatureCountAtTile(unsigned int row, unsigned int col) const +{ + return tiles_[row * n_tiles_w_ + col]; +} + +void TiledImage::incrementFeatureCountAtTile(unsigned int row, unsigned int col) +{ + tiles_[row * n_tiles_w_ + col] += 1; +} + +void TiledImage::resetFeatureCounts() +{ + std::fill(tiles_.begin(), tiles_.end(), 0); +} + +void TiledImage::setTileForFeature(Feature& feature) const +{ + // Find the tile column + double c = feature.getXDist() - tile_width_ - 0.5; + unsigned int col = 0; + while (c > 0) + { + col += 1; + c -= tile_width_; + } + + // Find the tile row + double r = rows - feature.getYDist() - 0.5; + unsigned int row = n_tiles_h_ - 1; + while (r > tile_height_) + { + row -= 1; + r -= tile_height_; + } + + // Store these in feature + feature.setTile(row, col); +} + +void TiledImage::plotFeature(Feature& feature, cv::Scalar color) +{ + // Display feature point + const double l = 2; // half length of feature display square + cv::rectangle(*this, cv::Point(feature.getXDist() - l, feature.getYDist() - l), + cv::Point(feature.getXDist() + l, feature.getYDist() + l), color, -1, 8); +} + +void TiledImage::plotTiles() +{ + const cv::Scalar color = cv::Scalar(0, 0, 0); + const int thickness = 1; + + // draw the column dividers + for (unsigned int c = 1; c < n_tiles_w_; ++c) + { + const double xPos = (cols / n_tiles_w_) * c; + // Note: top-left corner is (-0.5,-0.5) in OpenCV + cv::line(*this, cv::Point(xPos, -0.5), cv::Point(xPos, rows - 0.5), color, thickness); + } + + // draw the row dividers + for (unsigned int r = 1; r < n_tiles_h_; ++r) + { + const double yPos = (rows / n_tiles_h_) * r; + cv::line(*this, cv::Point(-0.5, yPos), cv::Point(cols - 0.5, yPos), color, thickness); + } +} diff --git a/src/x/vision/timing.cpp b/src/x/vision/timing.cpp new file mode 100644 index 0000000..88f39cd --- /dev/null +++ b/src/x/vision/timing.cpp @@ -0,0 +1,107 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifdef TIMING +#include + +using namespace x; + +void operator+=(TimeSum& ts, TimeSum::Dur time) { + ts.total_time += time; + ts.runs++; +} + +Timer* Timer::timer_ = nullptr; +std::string Timer::DurAsString = "us"; + +void Timer::Start(Timer::Id const id) { + static Timer timer; // singleton + if (not timer.timer_) { // make singleton available for Stop too + Timer::timer_ = &timer; + } + if (timer.Valid(timer.GetClock(id))) { + timer.Stream() << "Rejecting start of clock that's already running: " << id + << std::endl; + } else { + timer.AddClock(id); + timer.Stream() << ">>>> Clock Start: " << id << std::endl; + } +} + +void Timer::Stop(Timer::Id const id) { + if (not Timer::timer_) { + std::cout << "Trying to stop a clock in uninitialized object: " << id + << std::endl; + } else { + timer_->StopClock(timer_->GetClock(id), id); + } +} + +void Timer::StopClock(Timer::Iterator start, Timer::Id id) { + if (not Valid(start)) { + Stream() << "Trying to stop a clock that hasn't been started: " << id + << std::endl; + } else { + auto dur = Timer::Now() - start->second; + Dur elapsed = std::chrono::duration_cast (dur).count(); + Stream() << "<<<< Clock Stop: " << start->first << ": " << elapsed << " " + << Timer::DurAsString << std::endl; + Sum(start->first, elapsed); + // Sum(start->first, duration); + RemoveClock(start); + } +} + +void Timer::Finish(void) { + if (not Timer::timer_) return; + for (auto clock = timer_->clocks_.begin(); clock != timer_->clocks_.end(); + ++clock) { + Timer::timer_->StopClock(clock, clock->first); + } + + Timer::timer_->Stream() << std::setw(65) << std::setfill('-') << "" << std::endl + << std::setfill(' '); + Timer::timer_->Stream() << "Timing finished. Average times:" << std::endl; + for (auto sum = timer_->sums_.begin(); sum != timer_->sums_.end(); ++sum) { + Timer::timer_->Stream() << std::setw(30) << sum->first << ": "; + Timer::timer_->Stream() << std::setw(15) + << std::to_string(sum->second.total_time / + sum->second.runs) << " " + << Timer::DurAsString << " / " + << std::to_string(1.0 / (sum->second.total_time / + sum->second.runs * std::pow(10, -6))) << " Hz"; + Timer::timer_->Stream() << std::setw(20) << " (" + << std::to_string(sum->second.runs) << " executions)" + << std::endl; + } + Timer::timer_->Stream() << std::setw(65) << std::setfill('-') << "" << std::endl + << std::setfill(' '); + +#ifndef TIMER_TO_COUT + std::cout << timer_->stream_.str() << std::endl; +#endif +} + +void Timer::Sum(Timer::Id id, Timer::Dur secs) { + auto it = sums_.find(id); + if (it == sums_.end()) { + sums_.insert({id, secs}); + } else { + it->second += secs; + } +} + +#endif // TIMING diff --git a/src/x/vision/tracker.cpp b/src/x/vision/tracker.cpp new file mode 100644 index 0000000..48fcc7a --- /dev/null +++ b/src/x/vision/tracker.cpp @@ -0,0 +1,644 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include +#include +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/video/tracking.hpp" +#include "opencv2/calib3d/calib3d.hpp" + +using namespace x; + +Tracker::Tracker() +{} + +Tracker::Tracker(const Camera& cam, + const unsigned int fast_detection_delta, + const bool non_max_supp, + const unsigned int block_half_length, + unsigned int margin, + unsigned int n_feat_min, + int outlier_method, + double outlier_param1, + double outlier_param2) +: camera_(cam) +, fast_detection_delta_(fast_detection_delta) +, non_max_supp_(non_max_supp) +, block_half_length_(block_half_length) +, margin_(margin) +, n_feat_min_(n_feat_min) +, outlier_method_(outlier_method) +, outlier_param1_(outlier_param1) +, outlier_param2_(outlier_param2) +{} + +void Tracker::setParams(const Camera& cam, + const unsigned int fast_detection_delta, + const bool non_max_supp, + const unsigned int block_half_length, + unsigned int margin, + unsigned int n_feat_min, + int outlier_method, + double outlier_param1, + double outlier_param2) +{ + camera_ = cam; + fast_detection_delta_ = fast_detection_delta; + non_max_supp_ = non_max_supp; + block_half_length_ = block_half_length; + margin_ = margin; + n_feat_min_ = n_feat_min; + outlier_method_ = outlier_method; + outlier_param1_ = outlier_param1; + outlier_param2_ = outlier_param2; +} + +MatchList Tracker::getMatches() const +{ + return matches_; +} + +int Tracker::getImgId() const +{ + return img_id_; +} + +void Tracker::track(TiledImage& current_img, + double timestamp, + unsigned int frame_number) +{ + // Increment the current image number + img_id_++; + + #ifdef TIMING + // Set up and start internal timing + clock_t clock1, clock2, clock3, clock4, clock5, clock6, clock7; + clock1 = clock(); + #endif + + // Build image pyramid + getImagePyramid(current_img, img_pyramid_); + + #ifdef TIMING + clock2 = clock(); + #endif + + //============================================================================ + // Feature detection and tracking + //============================================================================ + + FeatureList current_features; + MatchList matches; + + // If first image + if(img_id_ == 1) + { + // Detect features in first image + FeatureList old_features; + featureDetection(current_img, current_features, timestamp, frame_number, old_features); + + // Undistortion of current features + camera_.undistort(current_features); + + #ifdef TIMING + clock3 = clock(); + + // Deal with unused timers + clock4 = clock3; + clock5 = clock4; + clock6 = clock5; + #endif + } + else // not first image + { + #ifdef TIMING + // End detection timer even if it did not occur + clock3 = clock(); + #endif + + // Track features + featureTracking(previous_img_, current_img, + previous_features_, current_features, + timestamp, frame_number); + + #ifdef TIMING + clock4 = clock(); + #endif + + removeOverflowFeatures(previous_img_, current_img, previous_features_, current_features); + + // Refresh features if needed + if(current_features.size() < n_feat_min_) + { + #ifdef DEBUG + std::cout << "Number of tracked features reduced to " + << current_features.size() + << std::endl; + std::cout << "Triggering re-detection" + << std::endl; + #endif + + // Detect and track new features outside of the neighborhood of + // previously-tracked features + FeatureList previous_features_new, current_features_new; + featureDetection(previous_img_, + previous_features_new, + previous_img_.getTimestamp(), + previous_img_.getFrameNumber(), + previous_features_); + featureTracking(previous_img_, + current_img, + previous_features_new, + current_features_new, + timestamp, + frame_number); + + // Concatenate previously-tracked and newly-tracked features + previous_features_.insert(previous_features_.end(), + previous_features_new.begin(), + previous_features_new.end()); + + current_features.insert(current_features.end(), + current_features_new.begin(), + current_features_new.end()); + } + + #ifdef TIMING + clock5 = clock(); + #endif + + // Undistortion of current features + camera_.undistort(current_features); + + // Undistort features in the previous image + // TODO(jeff) only do it for new features newly detected + camera_.undistort(previous_features_); + + //========================================================================== + // Outlier removal + //========================================================================== + + unsigned int n_matches = previous_features_.size(); + if (n_matches) + { + //TODO(jeff) Store coordinates as cv::Point2f in Feature class to avoid + // theses conversions + // Convert features to OpenCV keypoints + std::vector pts1, pts2; + pts1.resize(n_matches); + pts2.resize(n_matches); + for(unsigned int i = 0; i < pts1.size(); i++) + { + pts1[i].x = previous_features_[i].getX(); + pts1[i].y = previous_features_[i].getY(); + pts2[i].x = current_features[i].getX(); + pts2[i].y = current_features[i].getY(); + } + + std::vector mask; + cv::findFundamentalMat(pts1, + pts2, + outlier_method_, + outlier_param1_, + outlier_param2_, + mask); + /*cv::findHomography(pts1, + current_features, + outlier_method_, + outlier_param1_, + mask, + 2000, + 0.995);*/ + + FeatureList current_features_refined, previous_features_refined; + for(size_t i = 0; i < mask.size(); ++i) + { + if(mask[i] != 0) + { + previous_features_refined.push_back(previous_features_[i]); + current_features_refined.push_back(current_features[i]); + } + } + + std::swap(previous_features_refined, previous_features_); + std::swap(current_features_refined, current_features); + } + #ifdef TIMING + clock6 = clock(); + #endif + + //========================================================================== + // Match construction + //========================================================================== + + // Reset match list + n_matches = previous_features_.size(); + matches.resize(n_matches); + + // Store the message array into a match list + for(unsigned int i = 0; i < n_matches; ++i) + { + Match current_match; + current_match.previous = previous_features_[i]; + current_match.current = current_features[i]; + + // Add match to list + matches[i] = current_match; + } + } + + // Store results so it can queried outside the class + matches_ = matches; + + // Store info to match at next image + previous_features_ = current_features; + previous_timestamp_ = timestamp; + previous_img_ = current_img; + + // Exit the function if this is the first image + if(img_id_ < 2) + return; + + #ifdef TIMING + clock7 = clock(); + #endif + + // Print TIMING info +#ifdef TIMING + std::cout << "Tracking Info ====================================" << std::endl; + std::cout << "Number of matches: " << matches.size() << std::endl; + std::cout << "==================================================" << std::endl; + std::cout << + "Times ====================================" << std::endl << + "Build image pyramid: " << (double) (clock2 - clock1)/CLOCKS_PER_SEC*1000 << " ms" << std::endl << + "Feature detection: " << (double) (clock3 - clock2)/CLOCKS_PER_SEC*1000 << " ms" << std::endl << + "Feature tracking: " << (double) (clock4 - clock3)/CLOCKS_PER_SEC*1000 << " ms" << std::endl << + "Feature re-detection and tracking: " << (double) (clock5 - clock4)/CLOCKS_PER_SEC*1000 << " ms" << std::endl << + "Outlier check: " << (double) (clock6 - clock5)/CLOCKS_PER_SEC*1000 << " ms" << std::endl << + "Feature conversion: " << (double) (clock7 - clock6)/CLOCKS_PER_SEC*1000 << " ms" << std::endl << + "------------------------------------------" << std::endl << + "TOTAL: " << (double) (clock7 - clock1)/CLOCKS_PER_SEC*1000 << " ms" << std::endl << + "==========================================" << std::endl; +#endif + + // Plot matches in the image + plotMatches(matches_, current_img); +} + +/** \brief True if matches are chronological order + * @todo Make Match struct a class with this function in it. + */ +bool Tracker::checkMatches() +{ + if (matches_.size()) + { + Match match(matches_[0]); + if (match.previous.getTimestamp() < match.current.getTimestamp()) + { + return true; + } + } + return false; +} + +void Tracker::plotMatches(MatchList matches, TiledImage& img) +{ + // Convert grayscale image to color image +#if CV_MAJOR_VERSION == 4 + cv::cvtColor(img, img, cv::COLOR_GRAY2BGR); +#else + cv::cvtColor(img, img, CV_GRAY2BGR); +#endif + + // Plot tiles + img.plotTiles(); + + // Plot matches + const cv::Scalar green(100,255,100); + const unsigned int n = matches.size(); + for(unsigned int ii = 0; ii < n; ii++) + img.plotFeature(matches[ii].current, green); + + std::string str = std::string("Matches: ") + std::to_string(n); + + cv::putText(img, str, cv::Point((int) 10, (int) img.rows-10), cv::FONT_HERSHEY_PLAIN, + 1.0, green, 1.5, 8, false); +} + +void Tracker::featureDetection(const TiledImage& img, + FeatureList& new_feature_list, + double timestamp, + unsigned int frame_number, + FeatureList& old_feature_list) +{ + // Pyramidal OpenCV FAST + ImagePyramid pyr; + getImagePyramid(img, pyr); + + // Extract new features + FeatureList feature_list; + getFASTFeaturesPyramid(pyr, timestamp, frame_number, new_feature_list, old_feature_list); +} + +void Tracker::getImagePyramid (const TiledImage& img, + ImagePyramid& pyramid) +{ + pyramid.clear(); // clear the vector of images + pyramid.push_back(img); // the first pyramid level is the highest resolution image + for(unsigned int i = 1; i < pyramid_depth_; i++) // for all pyramid depths + { + TiledImage downsampledImage; // create a down-sampled image + downsampledImage.setTileSize(img.getTileHeight(), img.getTileWidth()); + cv::pyrDown(pyramid[i-1],downsampledImage); // reduce the image size by a factor of 2 + pyramid.push_back(downsampledImage); // add the down-sampled image to the pyramid + } +} + +void Tracker::getFASTFeaturesPyramid(ImagePyramid& pyramid, + double timestamp, + unsigned int frame_number, + FeatureList& features, + FeatureList& old_features) +{ + // Loop through the image pyramid levels and get the fast features + for(unsigned int l = 0; l < pyramid.size(); l++) + getFASTFeaturesImage(pyramid[l], timestamp, frame_number, l, features, old_features); +} + +void Tracker::getFASTFeaturesImage(TiledImage& img, + double timestamp, + unsigned int frame_number, + unsigned int pyramid_level, + FeatureList& features, + FeatureList& old_features) +{ + Keypoints keypoints; // vector of cv::Keypoint + // scaling for points dependent on pyramid level to get their position + // (x, y) = (x*2^pyramid_level,y*2^pyramid_level) + double scale_factor = pow(2,(double) pyramid_level); + + // Get FAST features + cv::FAST(img, keypoints, fast_detection_delta_, non_max_supp_); + + // Create a blocked mask which is 1 in the neighborhood of old features, and 0 + // elsewhere. + cv::Mat blocked_img = cv::Mat::zeros(img.rows, img.cols, CV_8U); + computeNeighborhoodMask(old_features, img, blocked_img); + + // For all keypoints, extract the relevant information and put it into the + // vector of Feature structs + FeatureList candidate_features; + for(unsigned int i = 0; i < keypoints.size(); i++) + { + Feature feature(timestamp, + frame_number, + ((double) keypoints[i].pt.x) * scale_factor, + ((double) keypoints[i].pt.y) * scale_factor, + pyramid_level, + keypoints[i].response); + + // Check whether features fall in the bounds of allowed features; if yes, + // add it to candidate features and store its designated bucket information + if(isFeatureInsideBorder(feature, + img, + margin_, + feature.getPyramidLevel())) + { + candidate_features.push_back(feature); + } + } + + // Sort the candidate features by FAST score (highest first) + std::sort(candidate_features.begin(), candidate_features.end(), + boost::bind(&Tracker::compareFASTFeaturesScore, this, _1, _2)); + + // Append candidate features which are not neighbors of tracked features. + // Note: old features are not supposed to be in here, see. updateTrailLists() + appendNonNeighborFeatures(img, features, candidate_features, blocked_img); +} + +void Tracker::computeNeighborhoodMask(const FeatureList& features, + const cv::Mat& img, + cv::Mat& mask) +{ + // Loop through candidate features. + // Create submask for each feature + for(int i = 0; i < (int) features.size(); i++) + { + // Rounded feature coordinates + double x = std::round(features[i].getXDist()); + double y = std::round(features[i].getYDist()); + + // Submask top-left corner and width/height determination. + int x_tl, y_tl, w, h; + // Width + if(x - block_half_length_ < 0) + { + x_tl = 0; + w = block_half_length_ + 1 + x; + } + else if(x + block_half_length_ > img.cols - 1) + { + x_tl = x - block_half_length_; + w = block_half_length_ + img.cols - x; + } + else + { + x_tl = x - block_half_length_; + w = 2*block_half_length_ + 1; + } + // Height + if(y - block_half_length_ < 0) + { + y_tl = 0; + h = block_half_length_ + 1 + y; + } + else if(y + block_half_length_ > img.rows - 1) + { + y_tl = y - block_half_length_; + h = block_half_length_ + img.rows - y; + } + else + { + y_tl = y - block_half_length_; + h = 2*block_half_length_ + 1; + } + + // Submask application + cv::Mat blocked_box = mask(cv::Rect(x_tl, y_tl, w, h)); // box to block off in the image + blocked_box.setTo(cv::Scalar(1)); // block out the surrounding area by setting the mask to 1 + } +} + +bool Tracker::isFeatureInsideBorder(Feature& feature, + const TiledImage& img, + unsigned int margin, + unsigned int pyramid_level) const +{ + // Return true if the feature falls within the border at the specified pyramid level + int xMin = (int) margin; + int xMax = (int) floor(img.cols/pow(2,(double) pyramid_level) - margin) - 1; + int yMin = (int) margin; + int yMax = (int) floor(img.rows/pow(2,(double) pyramid_level) - margin) - 1; + PixelCoor feature_coord = {0, 0}; + getScaledPixelCoordinate(feature, feature_coord); + return ( (feature_coord.x >= xMin) && (feature_coord.x <= xMax) && + (feature_coord.y >= yMin) && (feature_coord.y <= yMax)); +} + +void Tracker::getScaledPixelCoordinate(const Feature& feature, + PixelCoor& scaled_coord) const +{ + // Transform the feature into scaled coordinates + scaled_coord.x = (int) std::round(feature.getXDist() / pow(2, (double) feature.getPyramidLevel())); + scaled_coord.y = (int) std::round(feature.getYDist() / pow(2, (double) feature.getPyramidLevel())); +} + +bool Tracker::compareFASTFeaturesScore(const Feature& feature1, + const Feature& feature2) const +{ + return feature1.getFastScore() > feature2.getFastScore(); +} + +void Tracker::appendNonNeighborFeatures(TiledImage& img, + FeatureList& features, + FeatureList& candidate_features, + cv::Mat& mask) +{ + // Loop through candidate features. + // If the patch is not blocked and there is room in the bucket, add it to features + for(unsigned int i = 0; i < candidate_features.size(); i++) + { + double x = std::round(candidate_features[i].getXDist()); // approximate pixel coords (integer) + double y = std::round(candidate_features[i].getYDist()); // approximate pixel coords (integer) + + // if the point is not blocked out + if(mask.at(y, x) == 0) + { + // if there is room in the bucket + Feature& candidate_feature = candidate_features[i]; + img.setTileForFeature(candidate_feature); + + // add it to the features and update the bucket count + features.push_back(candidate_features[i]); + + //block out the surrounding area by setting the mask to 1 + cv::Mat blocked_box = mask(cv::Rect(x - block_half_length_, y - block_half_length_, + 2*block_half_length_ + 1, 2*block_half_length_ + 1)); + blocked_box.setTo(cv::Scalar(1)); + } + } +} + +void Tracker::removeOverflowFeatures(TiledImage& img1, + TiledImage& img2, + FeatureList& features1, + FeatureList& features2) const +{ + img1.resetFeatureCounts(); + + // Loop through current features to update their tile location + for(int i = features2.size(); i >= 1; i--) + { + img1.setTileForFeature(features1[i-1]); + img2.setTileForFeature(features2[i-1]); + + img1.incrementFeatureCountAtTile(features1[i-1].getTileRow(), features1[i-1].getTileCol()); + img2.incrementFeatureCountAtTile(features2[i-1].getTileRow(), features2[i-1].getTileCol()); + } + // Loop through current features to check if there are more than the max + // allowed per tile and delete all features from this tile if so. + // They are sorted by score so start at the bottom. + for(int i = features2.size()-1; i >= 1; i--) + { + const unsigned int feat2_row = features2[i-1].getTileRow(); + const unsigned int feat2_col = features2[i-1].getTileCol(); + const unsigned int count = img2.getFeatureCountAtTile(feat2_row, feat2_col); + if(count > img1.getMaxFeatPerTile()) + { + //if the bucket is overflowing, remove the feature. + features1.erase(features1.begin() + i-1); + features2.erase(features2.begin() + i-1); + } + } +} + +//this function automatically gets rid of points for which tracking fails +void Tracker::featureTracking(const cv::Mat& img1, + const cv::Mat& img2, + FeatureList& features1, + FeatureList& features2, + const double timestamp2, + const unsigned int frame_number2) const +{ + // Convert features to OpenCV keypoints + //TODO(jeff) Store coordinates as cv::Point2f in Feature class to avoid + // theses conversions + std::vector pts1; + pts1.resize(features1.size()); + for(unsigned int i = 0; i < pts1.size(); i++) + { + pts1[i].x = features1[i].getXDist(); + pts1[i].y = features1[i].getYDist(); + } + + std::vector status; + std::vector err; + cv::Size win_size = cv::Size(31,31); + cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01); + + // Prevents calcOpticalFlowPyrLK crash + std::vector pts2; + if(pts1.size() > 0) + { + cv::calcOpticalFlowPyrLK(img1, img2, + pts1, pts2, + status, err, win_size, 2, term_crit, + cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 0.0); //0.001 + } + + // Output features which KLT tracked and stayed inside the frame + int index_correction = 0; + for(unsigned int i = 0; i < status.size(); i++) + { + const cv::Point2f pt = pts2.at(i); + if(status.at(i) != 0 + && pt.x >= -0.5 && pt.y >= -0.5 + && pt.x <= img2.cols-0.5 && pt.y <= img2.rows-0.5) + { + Feature new_feature(timestamp2, + frame_number2, + pt.x, + pt.y, + features1[i].getPyramidLevel(), + features1[i].getFastScore()); + + features2.push_back(new_feature); + } + else + { + features1.erase(features1.begin() + (i - index_correction)); + index_correction++; + } + } + + // Sanity check + assert(features1.size() == features2.size()); +} diff --git a/src/x/vision/triangulation.cpp b/src/x/vision/triangulation.cpp new file mode 100644 index 0000000..f4e3aac --- /dev/null +++ b/src/x/vision/triangulation.cpp @@ -0,0 +1,234 @@ +/* + * Copyright 2020 California Institute of Technology (“Caltech”) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +using namespace x; + +Triangulation::Triangulation(const x::QuaternionArray& quat_l, + const x::Vector3Array& pos_l, + const unsigned int max_iter, + const double term) + : n_poses_( quat_l.size() ) + , positions_( pos_l ) + , max_iter_(max_iter) + , term_(term) +{ + // Initialize pose vectors + rotations_ = std::vector(n_poses_, Eigen::Matrix3d()); + projs_ = std::vector(n_poses_, cv::Mat()); + + // Attitudes and translations must have the same length + // TODO(jeff): Create pose class and remove this + assert(pos_l.size() == n_poses_); + + for (size_t i = 0; i < n_poses_; ++i) + { + // Compute rotation matrix + rotations_[i] = quat_l[i].toRotationMatrix().transpose(); + + // Compute projection matrix + pose2proj(rotations_[i], positions_[i], projs_[i]); + } +} + +Triangulation::Triangulation(const x::AttitudeList& attitudes, + const x::TranslationList& translations, + const unsigned int max_iter, + const double term) + : n_poses_( attitudes.size() ) + , max_iter_(max_iter) + , term_(term) +{ + // Initialize pose vectors + rotations_ = std::vector(n_poses_, Eigen::Matrix3d()); + positions_ = std::vector(n_poses_, Eigen::Vector3d()); + projs_ = std::vector(n_poses_, cv::Mat()); + + // Attitudes and translations must have the same length + // TODO(jeff): Create pose class and remove this + assert(translations.size() == n_poses_); + + for (size_t i = 0; i < n_poses_; ++i) + { + // Convert xVIO types to Eigen + // TODO(jeff): sync all types so this can be removed + x::Quaternion quat; + quat.x() = attitudes[i].ax; + quat.y() = attitudes[i].ay; + quat.z() = attitudes[i].az; + quat.w() = attitudes[i].aw; + quat = quat.normalized(); + rotations_[i] = quat.toRotationMatrix().transpose(); + + Eigen::Vector3d pos(translations[i].tx, + translations[i].ty, + translations[i].tz); + positions_[i] = pos; + + // Compute projection matrices + pose2proj(rotations_[i], positions_[i], projs_[i]); + } +} + +void Triangulation::triangulateDlt(const x::Feature& obs1, + const x::Feature& obs2, + const int i1, + const int i2, + Eigen::Vector3d& pt_xyz) const +{ + // Feature image coordinates + cv::Mat obs1_cv = (cv::Mat_(2, 1) << obs1.getX(), obs1.getY()); + cv::Mat obs2_cv = (cv::Mat_(2, 1) << obs2.getX(), obs2.getY()); + + // Feature triangulation with OpenCV Direct Linear Transform + cv::Mat pt_h; // homogeneous coordinates + cv::triangulatePoints(projs_[i1], projs_[i2], obs1_cv, obs2_cv, pt_h); + + // Feature cartesian coordinates (from OpenCV's homogeneous output) + const double x = pt_h.at(0) / pt_h.at(3); + const double y = pt_h.at(1) / pt_h.at(3); + const double z = pt_h.at(2) / pt_h.at(3); + pt_xyz = Eigen::Vector3d(x,y,z); +} + +void Triangulation::triangulateGN(const x::Track& track, + Eigen::Vector3d& pt_ivd) const +{ + /********************** Linear triangulation *********************/ + + // Indices of the first and last observations of that feature in + // the pose vectors + const int n_obs = track.size(); + const int i2 = n_poses_ - 1; + const int i1 = i2 - n_obs + 1; + + // First and last feature observations + const x::Feature& obs1 = track[i1]; + const x::Feature& obs2 = track[i2]; + + // Initial 2-view triangulation + Eigen::Vector3d pt_xyz; + triangulateDlt(obs1, obs2, i1, i2, pt_xyz); + + /*************************** Initialization ***********************/ + + // Homogenous coordinates in world frame + const cv::Mat pt_w_h = + (cv::Mat_(4, 1) << pt_xyz(0), + pt_xyz(1), + pt_xyz(2), + 1); + + // Homogenous coordinates in last camera frame + const cv::Mat pt_c2 = projs_[i2] * pt_w_h; + + // Inverse-depth coordinates initialization + double alpha = pt_c2.at(0) / pt_c2.at(2); + double beta = pt_c2.at(1) / pt_c2.at(2); + double rho = 1.0 / pt_c2.at(2); + + // Anchor frame for inverse-depth: last frame + const Eigen::Matrix3d rot_a = rotations_[i2]; + const Eigen::VectorXd p_a = positions_[i2]; + + // Iteration variables + const size_t n_meas = 2 * n_poses_; + double r_norm_last = 1000.0; + double r_norm = 100.0; + unsigned int iter = 0; + const Eigen::Vector3d u_x(1.0, 0.0, 0.0); // x axis + const Eigen::Vector3d u_y(0.0, 1.0, 0.0); // y axis + + /************************* Iterations ****************************/ + + // While the termination criterion is not reached + while (r_norm_last - r_norm > term_) + { + // Check nb of iterations <= max + iter++; + if (iter > max_iter_) + break; + + Eigen::VectorXd r = Eigen::VectorXd::Zero(n_meas); + Eigen::MatrixXd j = Eigen::MatrixXd::Zero(n_meas, 3); + + // Loop over camera frames + for (int i = i1; i <= i2; i++) + { + const Eigen::Matrix3d rot = rotations_[i]; + const Eigen::Matrix3d delta_rot_i2 = rot * rot_a.transpose(); + + const Eigen::VectorXd p = positions_[i]; + const Eigen::Vector3d delta_pos_i2 = rot * p_a - rot * p; + + // Measurement + const int i_trk = i - i1; + Eigen::Vector2d h_meas(track[i_trk].getX(), + track[i_trk].getY()); + + // Predicted measurement + Eigen::Vector3d h_i = + delta_rot_i2 * Eigen::Vector3d(alpha, beta, 1) + + rho * delta_pos_i2; + Eigen::Vector2d h( h_i(0)/h_i(2), h_i(1)/h_i(2)) ; + + // Residual + r.segment(i_trk * 2, 2) = h_meas - h; + + // Jacobians + Eigen::Vector3d j_alpha = delta_rot_i2 * u_x; + Eigen::Vector3d j_beta = delta_rot_i2 * u_y; + Eigen::Vector3d j_rho = delta_pos_i2; + + Eigen::Matrix3d j0; + j0 << j_alpha, j_beta, j_rho; + + Eigen::MatrixXd j1(2,3); + j1 << -1.0 / h_i(2), 0.0 , h_i(0) / std::pow(h_i(2), 2), + 0.0 , -1 / h_i(2), h_i(1) / std::pow(h_i(2), 2); + + j.block(i_trk * 2, 0, 2, 3) = j1 * j0; + } + + // Delta correction + const Eigen::Vector3d delta = (j.transpose() * j).inverse() * j.transpose() * r; + alpha = alpha - delta(0); + beta = beta - delta(1); + rho = rho - delta(2); + + // Save residuals for termination criterion + r_norm_last = r_norm; + r_norm = r.norm(); + } + + // Output + pt_ivd = Eigen::Vector3d(alpha, beta, rho); +} + +void Triangulation::pose2proj(const Eigen::Matrix3d& rot, + const Eigen::Vector3d& pos, + cv::Mat& proj) const +{ + // Construct projection matrix with Eigen + Eigen::MatrixXd proj_tmp(3,4); + proj_tmp << rot, -rot * pos; + + // Convert to cv::Mat + cv::eigen2cv(proj_tmp, proj); +}