From e6620a4ed45ff49960e9572365743a5bc4a08f66 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 10 Apr 2019 18:35:26 +0200 Subject: [PATCH 01/86] edited launch files to support euroc and mynt --- launch/image_processor_euroc.launch | 3 +- launch/image_processor_mynt.launch | 33 ++++++++++++++++ launch/msckf_vio_euroc.launch | 3 ++ launch/msckf_vio_mynt.launch | 61 +++++++++++++++++++++++++++++ 4 files changed, 99 insertions(+), 1 deletion(-) create mode 100644 launch/image_processor_mynt.launch create mode 100644 launch/msckf_vio_mynt.launch diff --git a/launch/image_processor_euroc.launch b/launch/image_processor_euroc.launch index f728112..f49fc6c 100644 --- a/launch/image_processor_euroc.launch +++ b/launch/image_processor_euroc.launch @@ -8,7 +8,8 @@ + output="screen" + > diff --git a/launch/image_processor_mynt.launch b/launch/image_processor_mynt.launch new file mode 100644 index 0000000..63b058e --- /dev/null +++ b/launch/image_processor_mynt.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index bfbf7eb..896feff 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -53,6 +53,9 @@ + + + diff --git a/launch/msckf_vio_mynt.launch b/launch/msckf_vio_mynt.launch new file mode 100644 index 0000000..678787e --- /dev/null +++ b/launch/msckf_vio_mynt.launch @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 79cce26dad1dd4be6a632b6ec27c138db4479e4c Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 10 Apr 2019 18:36:11 +0200 Subject: [PATCH 02/86] added moving window structure, not yet done. added timestame sync for images and features detected --- include/msckf_vio/feature.hpp | 2 + include/msckf_vio/image_processor.h | 11 +-- include/msckf_vio/msckf_vio.h | 37 +++++++- include/msckf_vio/utils.h | 7 -- msg/CameraMeasurement.msg | 2 +- src/image_processor.cpp | 38 +------- src/msckf_vio.cpp | 137 ++++++++++++++++++++++++++-- src/utils.cpp | 14 --- 8 files changed, 174 insertions(+), 74 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index f9f08c5..e43c9df 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -144,6 +144,8 @@ struct Feature { Eigen::aligned_allocator< std::pair > > observations; + // NxN Patch of Anchor Image + std::vector patch; // 3d postion of the feature in the world frame. Eigen::Vector3d position; diff --git a/include/msckf_vio/image_processor.h b/include/msckf_vio/image_processor.h index 1004086..086c5a9 100644 --- a/include/msckf_vio/image_processor.h +++ b/include/msckf_vio/image_processor.h @@ -14,10 +14,6 @@ #include #include -#include -#include -#include - #include #include #include @@ -312,7 +308,7 @@ private: const std::vector& markers, std::vector& refined_vec) { if (raw_vec.size() != markers.size()) { - ROS_WARN("The input size of raw_vec(%i) and markers(%i) does not match...", + ROS_WARN("The input size of raw_vec(%lu) and markers(%lu) does not match...", raw_vec.size(), markers.size()); } for (int i = 0; i < markers.size(); ++i) { @@ -367,11 +363,6 @@ private: boost::shared_ptr prev_features_ptr; boost::shared_ptr curr_features_ptr; - cv::cuda::GpuMat cam0_curr_img; - cv::cuda::GpuMat cam1_curr_img; - cv::cuda::GpuMat cam0_points_gpu; - cv::cuda::GpuMat cam1_points_gpu; - // Number of features after each outlier removal step. int before_tracking; int after_tracking; diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 8077acf..c497d0f 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -14,10 +14,14 @@ #include #include #include + #include +#include +#include #include #include +#include #include #include #include @@ -27,6 +31,11 @@ #include "feature.hpp" #include +#include +#include +#include +#include + namespace msckf_vio { /* * @brief MsckfVio Implements the algorithm in @@ -103,6 +112,12 @@ class MsckfVio { */ void featureCallback(const CameraMeasurementConstPtr& msg); + + void imageCallback ( + const sensor_msgs::ImageConstPtr& cam0_img, + const sensor_msgs::ImageConstPtr& cam1_img, + const CameraMeasurementConstPtr& feature_msg); + /* * @brief publish Publish the results of VIO. * @param time The time stamp of output msgs. @@ -126,6 +141,11 @@ class MsckfVio { bool resetCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); + void manageMovingWindow( + const cv_bridge::CvImageConstPtr& cam0_image_ptr, + const cv_bridge::CvImageConstPtr& cam1_image_ptr, + const CameraMeasurementConstPtr& feature_msg); + // Filter related functions // Propogate the state void batchImuProcessing( @@ -179,6 +199,15 @@ class MsckfVio { // transfer delay between IMU and Image messages. std::vector imu_msg_buffer; + // Moving Window buffer + std::map, + Eigen::aligned_allocator< + std::pair > > cam0_moving_window; + + std::map, + Eigen::aligned_allocator< + std::pair > > cam1_moving_window; + // Indicate if the gravity vector is set. bool is_gravity_set; @@ -206,12 +235,18 @@ class MsckfVio { // Subscribers and publishers ros::Subscriber imu_sub; - ros::Subscriber feature_sub; ros::Publisher odom_pub; ros::Publisher feature_pub; tf::TransformBroadcaster tf_pub; ros::ServiceServer reset_srv; + + message_filters::Subscriber cam0_img_sub; + message_filters::Subscriber cam1_img_sub; + message_filters::Subscriber feature_sub; + + message_filters::TimeSynchronizer image_sub; + // Frame id std::string fixed_frame_id; std::string child_frame_id; diff --git a/include/msckf_vio/utils.h b/include/msckf_vio/utils.h index cf0d39a..97f0dac 100644 --- a/include/msckf_vio/utils.h +++ b/include/msckf_vio/utils.h @@ -11,9 +11,6 @@ #include #include #include -#include -#include -#include #include namespace msckf_vio { @@ -21,10 +18,6 @@ namespace msckf_vio { * @brief utilities for msckf_vio */ namespace utils { - -void download(const cv::cuda::GpuMat& d_mat, std::vector& vec); -void download(const cv::cuda::GpuMat& d_mat, std::vector& vec); - Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh, const std::string &field); diff --git a/msg/CameraMeasurement.msg b/msg/CameraMeasurement.msg index 29a69bf..84e448d 100644 --- a/msg/CameraMeasurement.msg +++ b/msg/CameraMeasurement.msg @@ -1,4 +1,4 @@ -std_msgs/Header header +Header header # All features on the current image, # including tracked ones and newly detected ones. FeatureMeasurement[] features diff --git a/src/image_processor.cpp b/src/image_processor.cpp index ba220b1..a5039f3 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -170,10 +170,6 @@ bool ImageProcessor::loadParameters() { processor_config.ransac_threshold); ROS_INFO("stereo_threshold: %f", processor_config.stereo_threshold); - ROS_INFO("OpenCV Major Version: %d", - CV_MAJOR_VERSION); - ROS_INFO("OpenCV Minor Version: %d", - CV_MINOR_VERSION); ROS_INFO("==========================================="); return true; } @@ -223,9 +219,7 @@ void ImageProcessor::stereoCallback( sensor_msgs::image_encodings::MONO8); // Build the image pyramids once since they're used at multiple places - - // removed due to alternate cuda construct - //createImagePyramids(); + createImagePyramids(); // Detect features in the first frame. if (is_first_img) { @@ -302,7 +296,6 @@ void ImageProcessor::imuCallback( void ImageProcessor::createImagePyramids() { const Mat& curr_cam0_img = cam0_curr_img_ptr->image; - // TODO: build cuda optical flow buildOpticalFlowPyramid( curr_cam0_img, curr_cam0_pyramid_, Size(processor_config.patch_size, processor_config.patch_size), @@ -310,7 +303,6 @@ void ImageProcessor::createImagePyramids() { BORDER_CONSTANT, false); const Mat& curr_cam1_img = cam1_curr_img_ptr->image; - // TODO: build cuda optical flow buildOpticalFlowPyramid( curr_cam1_img, curr_cam1_pyramid_, Size(processor_config.patch_size, processor_config.patch_size), @@ -464,7 +456,6 @@ void ImageProcessor::trackFeatures() { predictFeatureTracking(prev_cam0_points, cam0_R_p_c, cam0_intrinsics, curr_cam0_points); - //TODO: change to GPU calcOpticalFlowPyrLK( prev_cam0_pyramid_, curr_cam0_pyramid_, prev_cam0_points, curr_cam0_points, @@ -634,30 +625,7 @@ void ImageProcessor::stereoMatch( cam1_distortion_model, cam1_distortion_coeffs); } - auto d_pyrLK_sparse = cuda::SparsePyrLKOpticalFlow::create( - Size(processor_config.patch_size, processor_config.patch_size), - processor_config.pyramid_levels, - processor_config.max_iteration, - true); - - cam0_curr_img = cv::cuda::GpuMat(cam0_curr_img_ptr->image); - cam1_curr_img = cv::cuda::GpuMat(cam1_curr_img_ptr->image); - cam0_points_gpu = cv::cuda::GpuMat(cam0_points); - cam1_points_gpu = cv::cuda::GpuMat(cam1_points); - - cv::cuda::GpuMat inlier_markers_gpu; - d_pyrLK_sparse->calc(cam0_curr_img, - cam1_curr_img, - cam0_points_gpu, - cam1_points_gpu, - inlier_markers_gpu, - noArray()); - - utils::download(cam1_points_gpu, cam1_points); - utils::download(inlier_markers_gpu, inlier_markers); - // Track features using LK optical flow method. - /* calcOpticalFlowPyrLK(curr_cam0_pyramid_, curr_cam1_pyramid_, cam0_points, cam1_points, inlier_markers, noArray(), @@ -667,7 +635,7 @@ void ImageProcessor::stereoMatch( processor_config.max_iteration, processor_config.track_precision), cv::OPTFLOW_USE_INITIAL_FLOW); - */ + // Mark those tracked points out of the image region // as untracked. for (int i = 0; i < cam1_points.size(); ++i) { @@ -1027,7 +995,7 @@ void ImageProcessor::twoPointRansac( // Check the size of input point size. if (pts1.size() != pts2.size()) - ROS_ERROR("Sets of different size (%i and %i) are used...", + ROS_ERROR("Sets of different size (%lu and %lu) are used...", pts1.size(), pts2.size()); double norm_pixel_unit = 2.0 / (intrinsics[0]+intrinsics[1]); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index d9c1b8d..c045040 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -53,6 +53,7 @@ map MsckfVio::chi_squared_test_table; MsckfVio::MsckfVio(ros::NodeHandle& pnh): is_gravity_set(false), is_first_img(true), + image_sub(10), nh(pnh) { return; } @@ -186,8 +187,16 @@ bool MsckfVio::createRosIO() { imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this); - feature_sub = nh.subscribe("features", 40, - &MsckfVio::featureCallback, this); + //feature_sub = nh.subscribe("features", 40, + // &MsckfVio::featureCallback, this); + + cam0_img_sub.subscribe(nh, "cam0_image", 10); + cam1_img_sub.subscribe(nh, "cam1_image", 10); + feature_sub.subscribe(nh, "features", 10); + + image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub); + image_sub.registerCallback(&MsckfVio::imageCallback, this); + mocap_odom_sub = nh.subscribe("mocap_odom", 10, &MsckfVio::mocapOdomCallback, this); @@ -245,6 +254,119 @@ void MsckfVio::imuCallback( return; } +void MsckfVio::imageCallback ( + const sensor_msgs::ImageConstPtr& cam0_img, + const sensor_msgs::ImageConstPtr& cam1_img, + const CameraMeasurementConstPtr& feature_msg) +{ + ROS_INFO("In Callback"); + + // Return if the gravity vector has not been set. + if (!is_gravity_set) return; + + // Get the current image. + cv_bridge::CvImageConstPtr cam0_image_ptr = cv_bridge::toCvShare(cam0_img, + sensor_msgs::image_encodings::MONO8); + cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img, + sensor_msgs::image_encodings::MONO8); + + // Start the system if the first image is received. + // The frame where the first image is received will be + // the origin. + if (is_first_img) { + is_first_img = false; + state_server.imu_state.time = feature_msg->header.stamp.toSec(); + } + + static double max_processing_time = 0.0; + static int critical_time_cntr = 0; + double processing_start_time = ros::Time::now().toSec(); + + // Propogate the IMU state. + // that are received before the image feature_msg. + ros::Time start_time = ros::Time::now(); + batchImuProcessing(feature_msg->header.stamp.toSec()); + double imu_processing_time = ( + ros::Time::now()-start_time).toSec(); + + // Augment the state vector. + start_time = ros::Time::now(); + stateAugmentation(feature_msg->header.stamp.toSec()); + double state_augmentation_time = ( + ros::Time::now()-start_time).toSec(); + + // Add new observations for existing features or new + // features in the map server. + start_time = ros::Time::now(); + addFeatureObservations(feature_msg); + double add_observations_time = ( + ros::Time::now()-start_time).toSec(); + + // Add new images to moving window + start_time = ros::Time::now(); + manageMovingWindow(cam0_image_ptr, cam1_img_ptr, feature_msg); + double manage_moving_window_time = ( + ros::Time::now()-start_time).toSec(); + + // Perform measurement update if necessary. + start_time = ros::Time::now(); + removeLostFeatures(); + double remove_lost_features_time = ( + ros::Time::now()-start_time).toSec(); + + start_time = ros::Time::now(); + pruneCamStateBuffer(); + double prune_cam_states_time = ( + ros::Time::now()-start_time).toSec(); + + // Publish the odometry. + start_time = ros::Time::now(); + publish(feature_msg->header.stamp); + double publish_time = ( + ros::Time::now()-start_time).toSec(); + + // Reset the system if necessary. + onlineReset(); + + double processing_end_time = ros::Time::now().toSec(); + double processing_time = + processing_end_time - processing_start_time; + if (processing_time > 1.0/frame_rate) { + ++critical_time_cntr; + //ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", + // processing_time, critical_time_cntr); + //printf("IMU processing time: %f/%f\n", + // imu_processing_time, imu_processing_time/processing_time); + //printf("State augmentation time: %f/%f\n", + // state_augmentation_time, state_augmentation_time/processing_time); + //printf("Add observations time: %f/%f\n", + // add_observations_time, add_observations_time/processing_time); + //printf("Remove lost features time: %f/%f\n", + // remove_lost_features_time, remove_lost_features_time/processing_time); + //printf("Remove camera states time: %f/%f\n", + // prune_cam_states_time, prune_cam_states_time/processing_time); + //printf("Publish time: %f/%f\n", + // publish_time, publish_time/processing_time); + } + + return; +} + +void MsckfVio::manageMovingWindow( + const cv_bridge::CvImageConstPtr& cam0_image_ptr, + const cv_bridge::CvImageConstPtr& cam1_image_ptr, + const CameraMeasurementConstPtr& feature_msg) { + + cam0_moving_window[state_server.imu_state.id] = cam0_image_ptr->image; + cam1_moving_window[state_server.imu_state.id] = cam1_image_ptr->image; + + while(cam0_moving_window.size() > 100) + { + cam1_moving_window.erase(cam1_moving_window.begin()); + cam0_moving_window.erase(cam0_moving_window.begin()); + } +} + void MsckfVio::initializeGravityAndBias() { // Initialize gravity and gyro bias. @@ -290,7 +412,6 @@ bool MsckfVio::resetCallback( ROS_WARN("Start resetting msckf vio..."); // Temporarily shutdown the subscribers to prevent the // state from updating. - feature_sub.shutdown(); imu_sub.shutdown(); // Reset the IMU state. @@ -348,10 +469,11 @@ bool MsckfVio::resetCallback( // Restart the subscribers. imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this); - feature_sub = nh.subscribe("features", 40, - &MsckfVio::featureCallback, this); - // TODO: When can the reset fail? +// feature_sub = nh.subscribe("features", 40, +// &MsckfVio::featureCallback, this); + +// TODO: When can the reset fail? res.success = true; ROS_WARN("Resetting msckf vio completed..."); return true; @@ -754,6 +876,7 @@ void MsckfVio::stateAugmentation(const double& time) { return; } + void MsckfVio::addFeatureObservations( const CameraMeasurementConstPtr& msg) { @@ -1298,6 +1421,8 @@ void MsckfVio::pruneCamStateBuffer() { // Remove this camera state in the state vector. state_server.cam_states.erase(cam_id); + cam0_moving_window.erase(cam_id); + cam1_moving_window.erase(cam_id); } return; diff --git a/src/utils.cpp b/src/utils.cpp index 830d84f..2af7278 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -11,20 +11,6 @@ namespace msckf_vio { namespace utils { -void download(const cv::cuda::GpuMat& d_mat, std::vector& vec) -{ - vec.resize(d_mat.cols); - cv::Mat mat(1, d_mat.cols, CV_32FC2, (void*)&vec[0]); - d_mat.download(mat); -} - -void download(const cv::cuda::GpuMat& d_mat, std::vector& vec) -{ - vec.resize(d_mat.cols); - cv::Mat mat(1, d_mat.cols, CV_8UC1, (void*)&vec[0]); - d_mat.download(mat); -} - Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh, const std::string &field) { Eigen::Isometry3d T; From b0dca3b15c4a0a320cae155a412ca98512d321df Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 10 Apr 2019 18:43:30 +0200 Subject: [PATCH 03/86] added pseudocode of original msckf --- CMakeLists.txt | 2 +- package.xml | 7 +- pseudocode/pseudocode_image_processing | 97 ++++++++++++++++++++++++++ pseudocode/pseudocode_msckf | 82 ++++++++++++++++++++++ 4 files changed, 183 insertions(+), 5 deletions(-) create mode 100644 pseudocode/pseudocode_image_processing create mode 100644 pseudocode/pseudocode_msckf diff --git a/CMakeLists.txt b/CMakeLists.txt index d81aed6..1a77616 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.12) -project(msckf_vio) +project(msckf) add_compile_options(-std=c++11) diff --git a/package.xml b/package.xml index 66d6b26..df884ce 100644 --- a/package.xml +++ b/package.xml @@ -3,13 +3,12 @@ msckf_vio 0.0.1 - Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation + Multi-State Constraint Kalman Filter - Photometric expansion - Ke Sun + Raphael Maenle Penn Software License - Ke Sun - Kartik Mohta + Raphael Maenle catkin diff --git a/pseudocode/pseudocode_image_processing b/pseudocode/pseudocode_image_processing new file mode 100644 index 0000000..d1472f0 --- /dev/null +++ b/pseudocode/pseudocode_image_processing @@ -0,0 +1,97 @@ + + +stereo callback() + + create image pyramids + _Constructs the image pyramid which can be passed to calcOpticalFlowPyrLK._ +. + if first Frame: + *initialize first Frame () + + else: + *track Features () + + *addnewFeatures () + + *pruneGridFeatures() + _removes worst features from any overflowing grid_ + + publish features (u1, v1, u2, v2) + _undistorts them beforehand_ + +addnewFeatures() + *mask existing features + *detect new fast features + *collect in a grid, keep only best n per grid + *stereomatch() + *save inliers into a new feature with u,v on cam0 and cam1 + + +track Features() + *integrateIMUData () + _uses existing IMU data between two frames to calc. rotation between the two frames_ + + *predictFeatureTracking() + _compensates the rotation between consecutive frames - rotates previous camera frame features to current camera rotation_ + + *calcOpticalFlowPyrLK() + _measures the change between the features in the previous frame and in the current frame (using the predicted features)_ + + *remove points outside of image region + _how does this even happen?_ + + *stereo match() + _find tracked features from optical flow in the camera 1 image_ + _remove all features that could not be matched_ + + *twoPointRansac(cam0) + *twoPointRansac(cam1) + _remove any features outside best found ransac model_ + + + + +twoPointRansac() + *mark all points as inliers + *compensate rotation between frames + *normalize points + *calculate difference bewteen previous and current points + *mark large distances (over 50 pixels currently) + *calculate mean points distance + *return if inliers (non marked) < 3 + *return if motion smaller than norm pixel unit + *ransac + *optimize with found inlier after random sample + + *set inlier markers + +initialize first Frame() + + features = FastFeatureDetector detect () + + *stereo match () + + group features into grid + - according to position in the image + - sorting them by response + - save the top responses + - save the top responses + + +stereo match () + + *undistort cam0 Points + *project cam0 Points to cam1 to initialize points in cam1 + + *calculate lk optical flow + _used because camera calibrations not perfect enough_ + _also, calculation more efficient, because LK calculated anyway_ + + *compute relative trans/rot between cam0 and cam1* + + *remove outliers based on essential matrix + _essential matrix relates points in stereo image (pinhole model)_ + for every point: + - calculate epipolar line of point in cam0 + - calculate error of cam1 to epipolar line + - remove if to big diff --git a/pseudocode/pseudocode_msckf b/pseudocode/pseudocode_msckf new file mode 100644 index 0000000..9dc2ecd --- /dev/null +++ b/pseudocode/pseudocode_msckf @@ -0,0 +1,82 @@ +featureCallback + propagate IMU state() + state Augmentation() + add Feature Observations() + + #the following possibly trigger ekf update step: + remove Lost Features () + prune Camera State Buffer () + + +remove Lost Features() + every feature that does not have a current observation: + *just delete if not enough features + + check Motion of Feature () + _calculation here makes no sense - he uses pixel position as direction vector for feature?_ + *initialize Position () + + caculate feature Jakobian and Residual() + *for every observation in this feature + - calculate u and v in camera frames, based on estimated feature position + - input results into jakobi d(measurement)/d(camera 0/1) + - input results into jakobi d(camera 0/1)/d(state) and jakobi d(camera 0/1)/d(feature position) + - project both jakobis to nullspace of feature position jakobi + - calculate residual: measurement - u and v of camera frames + - project residual onto nullspace of feature position jakobi + + - stack residual and jakobians + + gating Test() + + *measurementUpdate() + _use calculated residuals and jakobians to calculate change in error_ +measurementUpdate(): + - QR reduce the stacked Measurment Jakobis + - calcualte Kalman Gain based on Measurement Jakobian, Error-State Covariance and Noise + _does some fancy shit here_ + - calculate estimated error after observation: delta_x = KalmanGain * residual + - add estimated error to state (imu states and cam states) + +initialize Position (): + * create initial guess for global feature position () + _uses first feature measurement on left camera and last feature measurement of right camera_ + - transform first measurement to plane of last measurement + - calcualte least square point between rays + * get position approximation using measured feature positions + _using Levenberg Marqhart iterative search_ + + + +add Feature Observations() + * if feature not in map, add feature to map + _and add u0, v0, u1, v1 as first observation + * if feature in map, add new observation u0,v0,u1,v1 + + + +state Augmentation() + * Add estimated cam position to state + * Update P with Jakobian of cam Position + + +propagate IMU state () + _uses IMU process model for every saved IMU state_ + + for every buffered imu state: + + *remove bias + + *Compute F and G matrix (continuous transition and noise cov.) + _using current orientation, gyro and acc. reading_ + * approximate phi: phi = 1 + Fdt + ... + + * calculate new state propagating through runge kutta + * modify transition matrix to have a propper null space? + + * calculate Q = Phi*G*Q_noise*GT*PhiT + * calculate P = Phi*P*PhiT + Q + + + stateAugmentation () + _save current IMU state as camera position_ From a6af82a269e1022be12a64fc882892ee68aa7e74 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 10 Apr 2019 19:10:31 +0200 Subject: [PATCH 04/86] manage moving window saves copies of images --- CMakeLists.txt | 2 +- src/msckf_vio.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a77616..d81aed6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.12) -project(msckf) +project(msckf_vio) add_compile_options(-std=c++11) diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index c045040..bff5d38 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -259,7 +259,6 @@ void MsckfVio::imageCallback ( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - ROS_INFO("In Callback"); // Return if the gravity vector has not been set. if (!is_gravity_set) return; @@ -357,9 +356,11 @@ void MsckfVio::manageMovingWindow( const cv_bridge::CvImageConstPtr& cam1_image_ptr, const CameraMeasurementConstPtr& feature_msg) { - cam0_moving_window[state_server.imu_state.id] = cam0_image_ptr->image; - cam1_moving_window[state_server.imu_state.id] = cam1_image_ptr->image; + cam0_moving_window[state_server.imu_state.id] = cam0_image_ptr->image.clone(); + cam1_moving_window[state_server.imu_state.id] = cam1_image_ptr->image.clone(); + std::cout << cam0_moving_window.begin()->second << std::endl; + //TODO handle massive overflow correctly (should be pruned, before this ever triggers) while(cam0_moving_window.size() > 100) { cam1_moving_window.erase(cam1_moving_window.begin()); From a85a4745f2b5cb2e30b3d56d0a8c900d8718ac8a Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 12 Apr 2019 11:02:58 +0200 Subject: [PATCH 05/86] added anchor information generation --- include/msckf_vio/cam_state.h | 4 ++ include/msckf_vio/feature.hpp | 38 +++++++++++++- include/msckf_vio/msckf_vio.h | 23 ++++---- src/msckf_vio.cpp | 98 ++++------------------------------- 4 files changed, 61 insertions(+), 102 deletions(-) diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index 8d528a1..c6d2f7c 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -62,6 +62,10 @@ struct CAMState { typedef std::map, Eigen::aligned_allocator< std::pair > > CamStateServer; + +typedef std::map, + Eigen::aligned_allocator< + std::pair > > movingWindow; } // namespace msckf_vio #endif // MSCKF_VIO_CAM_STATE_H diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index e43c9df..0ff2137 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -114,6 +114,19 @@ struct Feature { inline bool checkMotion( const CamStateServer& cam_states) const; + /* + * @brief InitializeAnchor generates the NxN patch around the + * feature in the Anchor image + * @param cam_states: A map containing all recorded images + * currently presented in the camera state vector + * @return the irradiance of the Anchor NxN Patch + * @return True if the Anchor can be estimated + */ + + inline bool initializeAnchor( + const movingWindow& cam0_moving_window); + + /* * @brief InitializePosition Intialize the feature position * based on all current available measurements. @@ -145,7 +158,7 @@ struct Feature { std::pair > > observations; // NxN Patch of Anchor Image - std::vector patch; + std::vector anchorPatch; // 3d postion of the feature in the world frame. Eigen::Vector3d position; @@ -292,6 +305,29 @@ bool Feature::checkMotion( else return false; } +bool Feature::initializeAnchor( + const movingWindow& cam0_moving_window) +{ + + int N = 5; + int n = (int)(N-1)/2; + + auto anchor = observations.begin(); + if(cam0_moving_window.find(anchor->first) == cam0_moving_window.end()) + return false; + + cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second; + auto u = anchor->second(0)*anchorImage.rows/2 + anchorImage.rows/2; + auto v = anchor->second(1)*anchorImage.cols/2 + anchorImage.cols/2; + int count = 0; + + for(int u_run = (int)u - n; u_run <= (int)u + n; u_run++) + for(int v_run = v - n; v_run <= v + n; v_run++) + anchorPatch.push_back(anchorImage.at(u_run,v_run)); + + return true; +} + bool Feature::initializePosition( const CamStateServer& cam_states) { // Organize camera poses and feature observations properly. diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index c497d0f..7c2dd73 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -106,13 +107,14 @@ class MsckfVio { void imuCallback(const sensor_msgs::ImuConstPtr& msg); /* - * @brief featureCallback - * Callback function for feature measurements. - * @param msg Stereo feature measurements. + * @brief imageCallback + * Callback function for feature measurements + * Triggers measurement update + * @param msg + * Camera 0 Image + * Camera 1 Image + * Stereo feature measurements. */ - void featureCallback(const CameraMeasurementConstPtr& msg); - - void imageCallback ( const sensor_msgs::ImageConstPtr& cam0_img, const sensor_msgs::ImageConstPtr& cam1_img, @@ -200,14 +202,9 @@ class MsckfVio { std::vector imu_msg_buffer; // Moving Window buffer - std::map, - Eigen::aligned_allocator< - std::pair > > cam0_moving_window; + movingWindow cam0_moving_window; + movingWindow cam1_moving_window; - std::map, - Eigen::aligned_allocator< - std::pair > > cam1_moving_window; - // Indicate if the gravity vector is set. bool is_gravity_set; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index bff5d38..dd5e110 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -187,8 +187,6 @@ bool MsckfVio::createRosIO() { imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this); - //feature_sub = nh.subscribe("features", 40, - // &MsckfVio::featureCallback, this); cam0_img_sub.subscribe(nh, "cam0_image", 10); cam1_img_sub.subscribe(nh, "cam1_image", 10); @@ -236,7 +234,8 @@ bool MsckfVio::initialize() { } void MsckfVio::imuCallback( - const sensor_msgs::ImuConstPtr& msg) { + const sensor_msgs::ImuConstPtr& msg) +{ // IMU msgs are pushed backed into a buffer instead of // being processed immediately. The IMU msgs are processed @@ -254,7 +253,7 @@ void MsckfVio::imuCallback( return; } -void MsckfVio::imageCallback ( +void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam0_img, const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) @@ -358,9 +357,8 @@ void MsckfVio::manageMovingWindow( cam0_moving_window[state_server.imu_state.id] = cam0_image_ptr->image.clone(); cam1_moving_window[state_server.imu_state.id] = cam1_image_ptr->image.clone(); - std::cout << cam0_moving_window.begin()->second << std::endl; - //TODO handle massive overflow correctly (should be pruned, before this ever triggers) + //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) while(cam0_moving_window.size() > 100) { cam1_moving_window.erase(cam1_moving_window.begin()); @@ -480,88 +478,6 @@ bool MsckfVio::resetCallback( return true; } -void MsckfVio::featureCallback( - const CameraMeasurementConstPtr& msg) { - - // Return if the gravity vector has not been set. - if (!is_gravity_set) return; - - // Start the system if the first image is received. - // The frame where the first image is received will be - // the origin. - if (is_first_img) { - is_first_img = false; - state_server.imu_state.time = msg->header.stamp.toSec(); - } - - static double max_processing_time = 0.0; - static int critical_time_cntr = 0; - double processing_start_time = ros::Time::now().toSec(); - - // Propogate the IMU state. - // that are received before the image msg. - ros::Time start_time = ros::Time::now(); - batchImuProcessing(msg->header.stamp.toSec()); - double imu_processing_time = ( - ros::Time::now()-start_time).toSec(); - - // Augment the state vector. - start_time = ros::Time::now(); - stateAugmentation(msg->header.stamp.toSec()); - double state_augmentation_time = ( - ros::Time::now()-start_time).toSec(); - - // Add new observations for existing features or new - // features in the map server. - start_time = ros::Time::now(); - addFeatureObservations(msg); - double add_observations_time = ( - ros::Time::now()-start_time).toSec(); - - // Perform measurement update if necessary. - start_time = ros::Time::now(); - removeLostFeatures(); - double remove_lost_features_time = ( - ros::Time::now()-start_time).toSec(); - - start_time = ros::Time::now(); - pruneCamStateBuffer(); - double prune_cam_states_time = ( - ros::Time::now()-start_time).toSec(); - - // Publish the odometry. - start_time = ros::Time::now(); - publish(msg->header.stamp); - double publish_time = ( - ros::Time::now()-start_time).toSec(); - - // Reset the system if necessary. - onlineReset(); - - double processing_end_time = ros::Time::now().toSec(); - double processing_time = - processing_end_time - processing_start_time; - if (processing_time > 1.0/frame_rate) { - ++critical_time_cntr; - ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", - processing_time, critical_time_cntr); - //printf("IMU processing time: %f/%f\n", - // imu_processing_time, imu_processing_time/processing_time); - //printf("State augmentation time: %f/%f\n", - // state_augmentation_time, state_augmentation_time/processing_time); - //printf("Add observations time: %f/%f\n", - // add_observations_time, add_observations_time/processing_time); - printf("Remove lost features time: %f/%f\n", - remove_lost_features_time, remove_lost_features_time/processing_time); - printf("Remove camera states time: %f/%f\n", - prune_cam_states_time, prune_cam_states_time/processing_time); - //printf("Publish time: %f/%f\n", - // publish_time, publish_time/processing_time); - } - - return; -} - void MsckfVio::mocapOdomCallback( const nav_msgs::OdometryConstPtr& msg) { static bool first_mocap_odom_msg = true; @@ -1198,6 +1114,12 @@ void MsckfVio::removeLostFeatures() { } } + if(!feature.initializeAnchor(cam0_moving_window)) + { + invalid_feature_ids.push_back(feature.id); + continue; + } + jacobian_row_size += 4*feature.observations.size() - 3; processed_feature_ids.push_back(feature.id); } From 8227a8e48deb6209201313e1bd773322422ce9b8 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 12 Apr 2019 17:37:01 +0200 Subject: [PATCH 06/86] added position calculation --- CMakeLists.txt | 2 + include/msckf_vio/feature.hpp | 70 ++++++++++++++++++++++++----- include/msckf_vio/image_handler.h | 34 ++++++++++++++ include/msckf_vio/msckf_vio.h | 11 +++++ package.xml | 2 +- src/image_handler.cpp | 75 +++++++++++++++++++++++++++++++ src/image_processor.cpp | 17 +++---- src/msckf_vio.cpp | 50 ++++++++++++++++++++- 8 files changed, 241 insertions(+), 20 deletions(-) create mode 100644 include/msckf_vio/image_handler.h create mode 100644 src/image_handler.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d81aed6..717982f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -79,6 +79,7 @@ include_directories( add_library(msckf_vio src/msckf_vio.cpp src/utils.cpp + src/image_handler.cpp ) add_dependencies(msckf_vio ${${PROJECT_NAME}_EXPORTED_TARGETS} @@ -106,6 +107,7 @@ target_link_libraries(msckf_vio_nodelet add_library(image_processor src/image_processor.cpp src/utils.cpp + src/image_handler.cpp ) add_dependencies(image_processor ${${PROJECT_NAME}_EXPORTED_TARGETS} diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 0ff2137..32b60b6 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -16,6 +16,8 @@ #include #include +#include "image_handler.h" + #include "math_utils.hpp" #include "imu_state.h" #include "cam_state.h" @@ -123,8 +125,11 @@ struct Feature { * @return True if the Anchor can be estimated */ - inline bool initializeAnchor( - const movingWindow& cam0_moving_window); + bool initializeAnchor( + const movingWindow& cam0_moving_window, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs); /* @@ -141,6 +146,15 @@ struct Feature { inline bool initializePosition( const CamStateServer& cam_states); + /* + * @brief projectPixelToPosition uses the calcualted pixels + * of the anchor patch to generate 3D positions of all of em + */ + bool projectPixelToPosition(cv::Point2f in_p, + Eigen::Vector3d& out_p, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs); // An unique identifier for the feature. // In case of long time running, the variable @@ -159,9 +173,16 @@ struct Feature { // NxN Patch of Anchor Image std::vector anchorPatch; + + // Position of NxN Patch in 3D space + std::vector anchorPatch_3d; + // 3d postion of the feature in the world frame. Eigen::Vector3d position; + // inverse depth representation + double rho; + // A indicator to show if the 3d postion of the feature // has been initialized or not. bool is_initialized; @@ -305,8 +326,24 @@ bool Feature::checkMotion( else return false; } +bool Feature::projectPixelToPosition(cv::Point2f in_p, + Eigen::Vector3d& out_p, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs) +{ + // use undistorted position of point of interest + // project it back into 3D space using pinhole model + // save resulting NxN positions for this feature + anchorPatch_3d.push_back(Eigen::Vector3d(in_p.x/rho, in_p.y/rho, 1/rho)); + printf("%f, %f, %f\n",in_p.x/rho, in_p.y/rho, 1/rho); +} + bool Feature::initializeAnchor( - const movingWindow& cam0_moving_window) + const movingWindow& cam0_moving_window, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs) { int N = 5; @@ -317,15 +354,25 @@ bool Feature::initializeAnchor( return false; cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second; - auto u = anchor->second(0)*anchorImage.rows/2 + anchorImage.rows/2; - auto v = anchor->second(1)*anchorImage.cols/2 + anchorImage.cols/2; + auto u = anchor->second(0)*intrinsics[0] + intrinsics[2]; + auto v = anchor->second(1)*intrinsics[1] + intrinsics[3]; int count = 0; - for(int u_run = (int)u - n; u_run <= (int)u + n; u_run++) - for(int v_run = v - n; v_run <= v + n; v_run++) - anchorPatch.push_back(anchorImage.at(u_run,v_run)); - - return true; + printf("estimated NxN position: \n"); + for(double u_run = u - n; u_run <= u + n; u_run = u_run + 1) + { + for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1) + { + anchorPatch.push_back(anchorImage.at((int)u_run,(int)v_run)); + Eigen::Vector3d Npose; + projectPixelToPosition(cv::Point2f((u_run-intrinsics[2])/intrinsics[0], (v_run-intrinsics[1])/intrinsics[3]), + Npose, + intrinsics, + distortion_model, + distortion_coeffs); + } + } + return true; } bool Feature::initializePosition( @@ -465,6 +512,9 @@ bool Feature::initializePosition( } } + //save inverse depth distance from camera + rho = solution(2); + // Convert the feature position to the world frame. position = T_c0_w.linear()*final_position + T_c0_w.translation(); diff --git a/include/msckf_vio/image_handler.h b/include/msckf_vio/image_handler.h new file mode 100644 index 0000000..8fd9ff9 --- /dev/null +++ b/include/msckf_vio/image_handler.h @@ -0,0 +1,34 @@ +#ifndef MSCKF_VIO_IMAGE_HANDLER_H +#define MSCKF_VIO_IMAGE_HANDLER_H + +#include +#include +#include +#include +#include +#include +#include + +namespace msckf_vio { +/* + * @brief utilities for msckf_vio + */ +namespace image_handler { + +void undistortPoints( + const std::vector& pts_in, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + std::vector& pts_out, + const cv::Matx33d &rectification_matrix = cv::Matx33d::eye(), + const cv::Vec4d &new_intrinsics = cv::Vec4d(1,1,0,0)); + +std::vector distortPoints( + const std::vector& pts_in, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs); +} +} +#endif diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 7c2dd73..b3a37bc 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -205,6 +205,17 @@ class MsckfVio { movingWindow cam0_moving_window; movingWindow cam1_moving_window; + // Camera calibration parameters + std::string cam0_distortion_model; + cv::Vec2i cam0_resolution; + cv::Vec4d cam0_intrinsics; + cv::Vec4d cam0_distortion_coeffs; + + std::string cam1_distortion_model; + cv::Vec2i cam1_resolution; + cv::Vec4d cam1_intrinsics; + cv::Vec4d cam1_distortion_coeffs; + // Indicate if the gravity vector is set. bool is_gravity_set; diff --git a/package.xml b/package.xml index df884ce..86a6e20 100644 --- a/package.xml +++ b/package.xml @@ -5,7 +5,7 @@ 0.0.1 Multi-State Constraint Kalman Filter - Photometric expansion - Raphael Maenle + Raphael Maenle Penn Software License Raphael Maenle diff --git a/src/image_handler.cpp b/src/image_handler.cpp new file mode 100644 index 0000000..6f5bad7 --- /dev/null +++ b/src/image_handler.cpp @@ -0,0 +1,75 @@ +#include + +namespace msckf_vio { +namespace image_handler { + + +void undistortPoints( + const std::vector& pts_in, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + std::vector& pts_out, + const cv::Matx33d &rectification_matrix, + const cv::Vec4d &new_intrinsics) { + + if (pts_in.size() == 0) return; + + const cv::Matx33d K( + intrinsics[0], 0.0, intrinsics[2], + 0.0, intrinsics[1], intrinsics[3], + 0.0, 0.0, 1.0); + + const cv::Matx33d K_new( + new_intrinsics[0], 0.0, new_intrinsics[2], + 0.0, new_intrinsics[1], new_intrinsics[3], + 0.0, 0.0, 1.0); + + if (distortion_model == "radtan") { + cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, + rectification_matrix, K_new); + } else if (distortion_model == "equidistant") { + cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs, + rectification_matrix, K_new); + } else { + ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", + distortion_model.c_str()); + cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, + rectification_matrix, K_new); + } + + return; +} + +std::vector distortPoints( + const std::vector& pts_in, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs) { + + const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2], + 0.0, intrinsics[1], intrinsics[3], + 0.0, 0.0, 1.0); + + std::vector pts_out; + if (distortion_model == "radtan") { + std::vector homogenous_pts; + cv::convertPointsToHomogeneous(pts_in, homogenous_pts); + cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K, + distortion_coeffs, pts_out); + } else if (distortion_model == "equidistant") { + cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs); + } else { + ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", + distortion_model.c_str()); + std::vector homogenous_pts; + cv::convertPointsToHomogeneous(pts_in, homogenous_pts); + cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K, + distortion_coeffs, pts_out); + } + + return pts_out; +} + + } // namespace image_handler +} // namespace msckf_vio \ No newline at end of file diff --git a/src/image_processor.cpp b/src/image_processor.cpp index a5039f3..6bd9030 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -17,6 +17,7 @@ #include #include #include +#include using namespace std; using namespace cv; @@ -618,10 +619,10 @@ void ImageProcessor::stereoMatch( // rotation from stereo extrinsics const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu; vector cam0_points_undistorted; - undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model, + image_handler::undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs, cam0_points_undistorted, R_cam0_cam1); - cam1_points = distortPoints(cam0_points_undistorted, cam1_intrinsics, + cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1_intrinsics, cam1_distortion_model, cam1_distortion_coeffs); } @@ -662,10 +663,10 @@ void ImageProcessor::stereoMatch( // essential matrix. vector cam0_points_undistorted(0); vector cam1_points_undistorted(0); - undistortPoints( + image_handler::undistortPoints( cam0_points, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs, cam0_points_undistorted); - undistortPoints( + image_handler::undistortPoints( cam1_points, cam1_intrinsics, cam1_distortion_model, cam1_distortion_coeffs, cam1_points_undistorted); @@ -1009,10 +1010,10 @@ void ImageProcessor::twoPointRansac( // Undistort all the points. vector pts1_undistorted(pts1.size()); vector pts2_undistorted(pts2.size()); - undistortPoints( + image_handler::undistortPoints( pts1, intrinsics, distortion_model, distortion_coeffs, pts1_undistorted); - undistortPoints( + image_handler::undistortPoints( pts2, intrinsics, distortion_model, distortion_coeffs, pts2_undistorted); @@ -1250,10 +1251,10 @@ void ImageProcessor::publish() { vector curr_cam0_points_undistorted(0); vector curr_cam1_points_undistorted(0); - undistortPoints( + image_handler::undistortPoints( curr_cam0_points, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs, curr_cam0_points_undistorted); - undistortPoints( + image_handler::undistortPoints( curr_cam1_points, cam1_intrinsics, cam1_distortion_model, cam1_distortion_coeffs, curr_cam1_points_undistorted); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index dd5e110..37b751e 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -117,6 +117,54 @@ bool MsckfVio::loadParameters() { nh.param("initial_covariance/extrinsic_translation_cov", extrinsic_translation_cov, 1e-4); + // get camera information (used for back projection) + nh.param("cam0/distortion_model", + cam0_distortion_model, string("radtan")); + nh.param("cam1/distortion_model", + cam1_distortion_model, string("radtan")); + + vector cam0_resolution_temp(2); + nh.getParam("cam0/resolution", cam0_resolution_temp); + cam0_resolution[0] = cam0_resolution_temp[0]; + cam0_resolution[1] = cam0_resolution_temp[1]; + + vector cam1_resolution_temp(2); + nh.getParam("cam1/resolution", cam1_resolution_temp); + cam1_resolution[0] = cam1_resolution_temp[0]; + cam1_resolution[1] = cam1_resolution_temp[1]; + + vector cam0_intrinsics_temp(4); + nh.getParam("cam0/intrinsics", cam0_intrinsics_temp); + cam0_intrinsics[0] = cam0_intrinsics_temp[0]; + cam0_intrinsics[1] = cam0_intrinsics_temp[1]; + cam0_intrinsics[2] = cam0_intrinsics_temp[2]; + cam0_intrinsics[3] = cam0_intrinsics_temp[3]; + + vector cam1_intrinsics_temp(4); + nh.getParam("cam1/intrinsics", cam1_intrinsics_temp); + cam1_intrinsics[0] = cam1_intrinsics_temp[0]; + cam1_intrinsics[1] = cam1_intrinsics_temp[1]; + cam1_intrinsics[2] = cam1_intrinsics_temp[2]; + cam1_intrinsics[3] = cam1_intrinsics_temp[3]; + + vector cam0_distortion_coeffs_temp(4); + nh.getParam("cam0/distortion_coeffs", + cam0_distortion_coeffs_temp); + cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0]; + cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1]; + cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2]; + cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3]; + + vector cam1_distortion_coeffs_temp(4); + nh.getParam("cam1/distortion_coeffs", + cam1_distortion_coeffs_temp); + cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0]; + cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1]; + cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; + cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; + + + state_server.state_cov = MatrixXd::Zero(21, 21); for (int i = 3; i < 6; ++i) state_server.state_cov(i, i) = gyro_bias_cov; @@ -1114,7 +1162,7 @@ void MsckfVio::removeLostFeatures() { } } - if(!feature.initializeAnchor(cam0_moving_window)) + if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs)) { invalid_feature_ids.push_back(feature.id); continue; From abd343f57633f81f172a99646c567bb0c678c7c8 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 12 Apr 2019 19:04:45 +0200 Subject: [PATCH 07/86] corrected position calculation for NxN points --- CMakeLists.txt | 1 + include/msckf_vio/feature.hpp | 11 +++++++++-- include/msckf_vio/image_handler.h | 9 +++++---- src/image_handler.cpp | 12 +++++++++++- 4 files changed, 26 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 717982f..3f5770a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -88,6 +88,7 @@ add_dependencies(msckf_vio target_link_libraries(msckf_vio ${catkin_LIBRARIES} ${SUITESPARSE_LIBRARIES} + ${OpenCV_LIBRARIES} ) # Msckf Vio nodelet diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 32b60b6..45abbaf 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -177,6 +177,9 @@ struct Feature { // Position of NxN Patch in 3D space std::vector anchorPatch_3d; + // Anchor Isometry + Eigen::Isometry3d T_anchor_w; + // 3d postion of the feature in the world frame. Eigen::Vector3d position; @@ -335,8 +338,11 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p, // use undistorted position of point of interest // project it back into 3D space using pinhole model // save resulting NxN positions for this feature - anchorPatch_3d.push_back(Eigen::Vector3d(in_p.x/rho, in_p.y/rho, 1/rho)); - printf("%f, %f, %f\n",in_p.x/rho, in_p.y/rho, 1/rho); + + Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho); + Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); + anchorPatch_3d.push_back(PositionInWorld); + printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); } bool Feature::initializeAnchor( @@ -412,6 +418,7 @@ bool Feature::initializePosition( // vector from the first camera frame in the buffer to this // camera frame. Eigen::Isometry3d T_c0_w = cam_poses[0]; + T_anchor_w = T_c0_w; for (auto& pose : cam_poses) pose = pose.inverse() * T_c0_w; diff --git a/include/msckf_vio/image_handler.h b/include/msckf_vio/image_handler.h index 8fd9ff9..f35e3a7 100644 --- a/include/msckf_vio/image_handler.h +++ b/include/msckf_vio/image_handler.h @@ -1,13 +1,14 @@ #ifndef MSCKF_VIO_IMAGE_HANDLER_H #define MSCKF_VIO_IMAGE_HANDLER_H -#include -#include +#include +#include #include #include + +#include #include -#include -#include + namespace msckf_vio { /* diff --git a/src/image_handler.cpp b/src/image_handler.cpp index 6f5bad7..3db65b5 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -1,4 +1,14 @@ -#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include namespace msckf_vio { namespace image_handler { From 010d36d21693a9ebe4e08bbbedb8fd16f2a7e5b8 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 16 Apr 2019 17:40:33 +0200 Subject: [PATCH 08/86] added projection into feature observations camera states --- include/msckf_vio/cam_state.h | 2 + include/msckf_vio/feature.hpp | 100 ++++++++++++++++++-- include/msckf_vio/msckf_vio.h | 15 +++ src/image_processor.cpp | 1 + src/msckf_vio.cpp | 169 +++++++++++++++++++++++++++++++++- 5 files changed, 277 insertions(+), 10 deletions(-) diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index c6d2f7c..3cfa49c 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -35,6 +35,8 @@ struct CAMState { // Position of the camera frame in the world frame. Eigen::Vector3d position; + //Illumination illumination; + // These two variables should have the same physical // interpretation with `orientation` and `position`. // There two variables are used to modify the measurement diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 45abbaf..5a756c2 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -146,16 +146,46 @@ struct Feature { inline bool initializePosition( const CamStateServer& cam_states); + + inline cv::Point2f projectPositionToCamera( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + Eigen::Vector3d& in_p) const; + + /* + * @brief IrradianceAnchorPatch_Camera returns irradiance values + * of the Anchor Patch position in a camera frame + * + */ + + bool IrradianceOfAnchorPatch( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + const movingWindow& cam0_moving_window, + std::vector& anchorPatch_measurement) const; + /* * @brief projectPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em */ - bool projectPixelToPosition(cv::Point2f in_p, + inline bool projectPixelToPosition(cv::Point2f in_p, Eigen::Vector3d& out_p, const cv::Vec4d& intrinsics, const std::string& distortion_model, const cv::Vec4d& distortion_coeffs); + /* + * @brief Irradiance returns irradiance value of a pixel + */ + + inline uint8_t Irradiance(cv::Point2f pose, cv::Mat image) const; + // An unique identifier for the feature. // In case of long time running, the variable // type of id is set to FeatureIDType in order @@ -329,6 +359,61 @@ bool Feature::checkMotion( else return false; } +bool Feature::IrradianceOfAnchorPatch( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + const movingWindow& cam0_moving_window, + std::vector& anchorPatch_measurement) const +{ + //project every point in anchorPatch_3d. + for (auto point : anchorPatch_3d) + { + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, intrinsics, distortion_model, distortion_coeffs, point); + uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second); + anchorPatch_measurement.push_back(irradiance); + } +} + +uint8_t Feature::Irradiance(cv::Point2f pose, cv::Mat image) const +{ + return image.at(pose.x, pose.y); +} + +cv::Point2f Feature::projectPositionToCamera( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + Eigen::Vector3d& in_p) const +{ + Eigen::Isometry3d T_c0_w; + + cv::Point2f out_p; + + // transfrom position to camera frame + Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); + const Eigen::Vector3d& t_c0_w = cam_state.position; + Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); + + out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); + std::vector out_v; + out_v.push_back(out_p); + std::vector my_p = image_handler::distortPoints( out_v, + intrinsics, + distortion_model, + distortion_coeffs); + + // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); + // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); + // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); + + return out_p; +} + bool Feature::projectPixelToPosition(cv::Point2f in_p, Eigen::Vector3d& out_p, const cv::Vec4d& intrinsics, @@ -342,9 +427,11 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p, Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho); Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); anchorPatch_3d.push_back(PositionInWorld); - printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); + //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); } + +//@test center projection must always be initial feature projection bool Feature::initializeAnchor( const movingWindow& cam0_moving_window, const cv::Vec4d& intrinsics, @@ -352,7 +439,7 @@ bool Feature::initializeAnchor( const cv::Vec4d& distortion_coeffs) { - int N = 5; + int N = 3; int n = (int)(N-1)/2; auto anchor = observations.begin(); @@ -364,21 +451,22 @@ bool Feature::initializeAnchor( auto v = anchor->second(1)*intrinsics[1] + intrinsics[3]; int count = 0; - printf("estimated NxN position: \n"); + //go through surrounding pixels for(double u_run = u - n; u_run <= u + n; u_run = u_run + 1) { for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1) { anchorPatch.push_back(anchorImage.at((int)u_run,(int)v_run)); Eigen::Vector3d Npose; - projectPixelToPosition(cv::Point2f((u_run-intrinsics[2])/intrinsics[0], (v_run-intrinsics[1])/intrinsics[3]), + projectPixelToPosition(cv::Point2f((u_run-intrinsics[2])/intrinsics[0], (v_run-intrinsics[3])/intrinsics[1]), Npose, intrinsics, distortion_model, distortion_coeffs); } } - return true; + + return true; } bool Feature::initializePosition( diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index b3a37bc..f857a52 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -174,6 +175,20 @@ class MsckfVio { void featureJacobian(const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + + + void PhotometricMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::Matrix& H_x, + Eigen::Matrix& H_f, + Eigen::Vector4d& r); + + void PhotometricFeatureJacobian( + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + void measurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); bool gatingTest(const Eigen::MatrixXd& H, diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 6bd9030..cea396d 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -622,6 +622,7 @@ void ImageProcessor::stereoMatch( image_handler::undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs, cam0_points_undistorted, R_cam0_cam1); + cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1_intrinsics, cam1_distortion_model, cam1_distortion_coeffs); } diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 37b751e..f5391dd 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -443,12 +443,14 @@ void MsckfVio::initializeGravityAndBias() { // is consistent with the inertial frame. double gravity_norm = gravity_imu.norm(); IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm); - + Quaterniond q0_i_w = Quaterniond::FromTwoVectors( gravity_imu, -IMUState::gravity); state_server.imu_state.orientation = rotationToQuaternion(q0_i_w.toRotationMatrix().transpose()); - + // printf("gravity Norm %f\n", gravity_norm); + // printf("linear_acc %f, %f, %f\n", gravity_imu(0), gravity_imu(1), gravity_imu(2)); + // printf("quaterniond: %f, %f, %f, %f\n", q0_i_w.w(), q0_i_w.x(), q0_i_w.y(), q0_i_w.z()); return; } @@ -803,6 +805,7 @@ void MsckfVio::stateAugmentation(const double& time) { cam_state.orientation_null = cam_state.orientation; cam_state.position_null = cam_state.position; + // Update the covariance matrix of the state. // To simplify computation, the matrix J below is the nontrivial block // in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision @@ -874,6 +877,157 @@ void MsckfVio::addFeatureObservations( return; } +void MsckfVio::PhotometricMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Matrix& H_x, Matrix& H_f, Vector4d& r) { + + // Prepare all the required data. + const CAMState& cam_state = state_server.cam_states[cam_state_id]; + const Feature& feature = map_server[feature_id]; + + // Cam0 pose. + Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); + const Vector3d& t_c0_w = cam_state.position; + + // Cam1 pose. + Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); + Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0; + Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); + + // 3d feature position in the world frame. + // And its observation with the stereo cameras. + const Vector3d& p_w = feature.position; + + //observation + const Vector4d& z = feature.observations.find(cam_state_id)->second; + + //photometric observation + std::vector photo_z; + feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs, cam0_moving_window, photo_z); + + + // Convert the feature position from the world frame to + // the cam0 and cam1 frame. + Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w); + Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w); + + + //compute resulting esimtated position in image + cv::Point2f out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); + std::vector out_v; + out_v.push_back(out_p); + + /* + //prints the feature projection in pixel space + std::vector my_p = image_handler::distortPoints( out_v, + cam0_intrinsics, + cam0_distortion_model, + cam0_distortion_coeffs); + printf("projection: %f, %f\n", my_p[0].x, my_p[0].y); + */ + + // Compute the Jacobians. + Matrix dz_dpc0 = Matrix::Zero(); + dz_dpc0(0, 0) = 1 / p_c0(2); + dz_dpc0(1, 1) = 1 / p_c0(2); + dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2)); + dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2)); + + Matrix dz_dpc1 = Matrix::Zero(); + dz_dpc1(2, 0) = 1 / p_c1(2); + dz_dpc1(3, 1) = 1 / p_c1(2); + dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2)); + dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2)); + + Matrix dpc0_dxc = Matrix::Zero(); + dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); + dpc0_dxc.rightCols(3) = -R_w_c0; + + Matrix dpc1_dxc = Matrix::Zero(); + dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0); + dpc1_dxc.rightCols(3) = -R_w_c1; + + Matrix3d dpc0_dpg = R_w_c0; + Matrix3d dpc1_dpg = R_w_c1; + + H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc; + H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg; + + // Modifty the measurement Jacobian to ensure + // observability constrain. + Matrix A = H_x; + Matrix u = Matrix::Zero(); + u.block<3, 1>(0, 0) = quaternionToRotation( + cam_state.orientation_null) * IMUState::gravity; + u.block<3, 1>(3, 0) = skewSymmetric( + p_w-cam_state.position_null) * IMUState::gravity; + H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose(); + H_f = -H_x.block<4, 3>(0, 3); + + // Compute the residual. + r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), + p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); + + return; +} + +void MsckfVio::PhotometricFeatureJacobian( + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + MatrixXd& H_x, VectorXd& r) { + + const auto& feature = map_server[feature_id]; + + // Check how many camera states in the provided camera + // id camera has actually seen this feature. + vector valid_cam_state_ids(0); + for (const auto& cam_id : cam_state_ids) { + if (feature.observations.find(cam_id) == + feature.observations.end()) continue; + + valid_cam_state_ids.push_back(cam_id); + } + + int jacobian_row_size = 0; + jacobian_row_size = 4 * valid_cam_state_ids.size(); + + MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size, + 21+state_server.cam_states.size()*6); + MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3); + VectorXd r_j = VectorXd::Zero(jacobian_row_size); + int stack_cntr = 0; + + for (const auto& cam_id : valid_cam_state_ids) { + + Matrix H_xi = Matrix::Zero(); + Matrix H_fi = Matrix::Zero(); + Vector4d r_i = Vector4d::Zero(); + PhotometricMeasurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i); + + auto cam_state_iter = state_server.cam_states.find(cam_id); + int cam_state_cntr = std::distance( + state_server.cam_states.begin(), cam_state_iter); + + // Stack the Jacobians. + H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi; + H_fj.block<4, 3>(stack_cntr, 0) = H_fi; + r_j.segment<4>(stack_cntr) = r_i; + stack_cntr += 4; + } + + // Project the residual and Jacobians onto the nullspace + // of H_fj. + JacobiSVD svd_helper(H_fj, ComputeFullU | ComputeThinV); + MatrixXd A = svd_helper.matrixU().rightCols( + jacobian_row_size - 3); + + H_x = A.transpose() * H_xj; + r = A.transpose() * r_j; + + return; +} + void MsckfVio::measurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, @@ -1199,7 +1353,7 @@ void MsckfVio::removeLostFeatures() { MatrixXd H_xj; VectorXd r_j; - featureJacobian(feature.id, cam_state_ids, H_xj, r_j); + PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; @@ -1316,6 +1470,13 @@ void MsckfVio::pruneCamStateBuffer() { } } + if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs)) + { + for (const auto& cam_id : involved_cam_state_ids) + feature.observations.erase(cam_id); + continue; + } + jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -1342,7 +1503,7 @@ void MsckfVio::pruneCamStateBuffer() { MatrixXd H_xj; VectorXd r_j; - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; From 7f2140ae882e744bd2e1be9c7fd31f4c91e3165d Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 16 Apr 2019 19:05:11 +0200 Subject: [PATCH 09/86] moved camera calibration information into a struct to make data handling smoother --- include/msckf_vio/cam_state.h | 17 ++++- include/msckf_vio/image_processor.h | 13 +--- include/msckf_vio/msckf_vio.h | 10 +-- src/image_processor.cpp | 114 ++++++++++++++-------------- src/msckf_vio.cpp | 56 +++++++------- 5 files changed, 107 insertions(+), 103 deletions(-) diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index 3cfa49c..e56b6c9 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -15,6 +15,21 @@ #include "imu_state.h" namespace msckf_vio { + +struct IlluminationParameter{ + double frame_bias; + double frame_gain; + double feature_bias; + double feature_gain; +}; + +struct CameraCalibration{ + std::string distortion_model; + cv::Vec2i resolution; + cv::Vec4d intrinsics; + cv::Vec4d distortion_coeffs; +}; + /* * @brief CAMState Stored camera state in order to * form measurement model. @@ -35,7 +50,7 @@ struct CAMState { // Position of the camera frame in the world frame. Eigen::Vector3d position; - //Illumination illumination; + IlluminationParameter illumination; // These two variables should have the same physical // interpretation with `orientation` and `position`. diff --git a/include/msckf_vio/image_processor.h b/include/msckf_vio/image_processor.h index 086c5a9..638fbcd 100644 --- a/include/msckf_vio/image_processor.h +++ b/include/msckf_vio/image_processor.h @@ -22,6 +22,8 @@ #include #include +#include "cam_state.h" + namespace msckf_vio { /* @@ -332,15 +334,8 @@ private: std::vector imu_msg_buffer; // Camera calibration parameters - std::string cam0_distortion_model; - cv::Vec2i cam0_resolution; - cv::Vec4d cam0_intrinsics; - cv::Vec4d cam0_distortion_coeffs; - - std::string cam1_distortion_model; - cv::Vec2i cam1_resolution; - cv::Vec4d cam1_intrinsics; - cv::Vec4d cam1_distortion_coeffs; + CameraCalibration cam0; + CameraCalibration cam1; // Take a vector from cam0 frame to the IMU frame. cv::Matx33d R_cam0_imu; diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index f857a52..b87349b 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -221,15 +221,9 @@ class MsckfVio { movingWindow cam1_moving_window; // Camera calibration parameters - std::string cam0_distortion_model; - cv::Vec2i cam0_resolution; - cv::Vec4d cam0_intrinsics; - cv::Vec4d cam0_distortion_coeffs; + CameraCalibration cam0; + CameraCalibration cam1; - std::string cam1_distortion_model; - cv::Vec2i cam1_resolution; - cv::Vec4d cam1_intrinsics; - cv::Vec4d cam1_distortion_coeffs; // Indicate if the gravity vector is set. bool is_gravity_set; diff --git a/src/image_processor.cpp b/src/image_processor.cpp index cea396d..4e26618 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -44,49 +44,49 @@ ImageProcessor::~ImageProcessor() { bool ImageProcessor::loadParameters() { // Camera calibration parameters nh.param("cam0/distortion_model", - cam0_distortion_model, string("radtan")); + cam0.distortion_model, string("radtan")); nh.param("cam1/distortion_model", - cam1_distortion_model, string("radtan")); + cam1.distortion_model, string("radtan")); vector cam0_resolution_temp(2); nh.getParam("cam0/resolution", cam0_resolution_temp); - cam0_resolution[0] = cam0_resolution_temp[0]; - cam0_resolution[1] = cam0_resolution_temp[1]; + cam0.resolution[0] = cam0_resolution_temp[0]; + cam0.resolution[1] = cam0_resolution_temp[1]; vector cam1_resolution_temp(2); nh.getParam("cam1/resolution", cam1_resolution_temp); - cam1_resolution[0] = cam1_resolution_temp[0]; - cam1_resolution[1] = cam1_resolution_temp[1]; + cam1.resolution[0] = cam1_resolution_temp[0]; + cam1.resolution[1] = cam1_resolution_temp[1]; vector cam0_intrinsics_temp(4); nh.getParam("cam0/intrinsics", cam0_intrinsics_temp); - cam0_intrinsics[0] = cam0_intrinsics_temp[0]; - cam0_intrinsics[1] = cam0_intrinsics_temp[1]; - cam0_intrinsics[2] = cam0_intrinsics_temp[2]; - cam0_intrinsics[3] = cam0_intrinsics_temp[3]; + cam0.intrinsics[0] = cam0_intrinsics_temp[0]; + cam0.intrinsics[1] = cam0_intrinsics_temp[1]; + cam0.intrinsics[2] = cam0_intrinsics_temp[2]; + cam0.intrinsics[3] = cam0_intrinsics_temp[3]; vector cam1_intrinsics_temp(4); nh.getParam("cam1/intrinsics", cam1_intrinsics_temp); - cam1_intrinsics[0] = cam1_intrinsics_temp[0]; - cam1_intrinsics[1] = cam1_intrinsics_temp[1]; - cam1_intrinsics[2] = cam1_intrinsics_temp[2]; - cam1_intrinsics[3] = cam1_intrinsics_temp[3]; + cam1.intrinsics[0] = cam1_intrinsics_temp[0]; + cam1.intrinsics[1] = cam1_intrinsics_temp[1]; + cam1.intrinsics[2] = cam1_intrinsics_temp[2]; + cam1.intrinsics[3] = cam1_intrinsics_temp[3]; vector cam0_distortion_coeffs_temp(4); nh.getParam("cam0/distortion_coeffs", cam0_distortion_coeffs_temp); - cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0]; - cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1]; - cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2]; - cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3]; + cam0.distortion_coeffs[0] = cam0_distortion_coeffs_temp[0]; + cam0.distortion_coeffs[1] = cam0_distortion_coeffs_temp[1]; + cam0.distortion_coeffs[2] = cam0_distortion_coeffs_temp[2]; + cam0.distortion_coeffs[3] = cam0_distortion_coeffs_temp[3]; vector cam1_distortion_coeffs_temp(4); nh.getParam("cam1/distortion_coeffs", cam1_distortion_coeffs_temp); - cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0]; - cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1]; - cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; - cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; + cam1.distortion_coeffs[0] = cam1_distortion_coeffs_temp[0]; + cam1.distortion_coeffs[1] = cam1_distortion_coeffs_temp[1]; + cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; + cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; cv::Mat T_imu_cam0 = utils::getTransformCV(nh, "cam0/T_cam_imu"); cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3))); @@ -124,27 +124,27 @@ bool ImageProcessor::loadParameters() { processor_config.stereo_threshold, 3); ROS_INFO("==========================================="); - ROS_INFO("cam0_resolution: %d, %d", - cam0_resolution[0], cam0_resolution[1]); + ROS_INFO("cam0.resolution: %d, %d", + cam0.resolution[0], cam0.resolution[1]); ROS_INFO("cam0_intrinscs: %f, %f, %f, %f", - cam0_intrinsics[0], cam0_intrinsics[1], - cam0_intrinsics[2], cam0_intrinsics[3]); - ROS_INFO("cam0_distortion_model: %s", - cam0_distortion_model.c_str()); + cam0.intrinsics[0], cam0.intrinsics[1], + cam0.intrinsics[2], cam0.intrinsics[3]); + ROS_INFO("cam0.distortion_model: %s", + cam0.distortion_model.c_str()); ROS_INFO("cam0_distortion_coefficients: %f, %f, %f, %f", - cam0_distortion_coeffs[0], cam0_distortion_coeffs[1], - cam0_distortion_coeffs[2], cam0_distortion_coeffs[3]); + cam0.distortion_coeffs[0], cam0.distortion_coeffs[1], + cam0.distortion_coeffs[2], cam0.distortion_coeffs[3]); - ROS_INFO("cam1_resolution: %d, %d", - cam1_resolution[0], cam1_resolution[1]); + ROS_INFO("cam1.resolution: %d, %d", + cam1.resolution[0], cam1.resolution[1]); ROS_INFO("cam1_intrinscs: %f, %f, %f, %f", - cam1_intrinsics[0], cam1_intrinsics[1], - cam1_intrinsics[2], cam1_intrinsics[3]); - ROS_INFO("cam1_distortion_model: %s", - cam1_distortion_model.c_str()); + cam1.intrinsics[0], cam1.intrinsics[1], + cam1.intrinsics[2], cam1.intrinsics[3]); + ROS_INFO("cam1.distortion_model: %s", + cam1.distortion_model.c_str()); ROS_INFO("cam1_distortion_coefficients: %f, %f, %f, %f", - cam1_distortion_coeffs[0], cam1_distortion_coeffs[1], - cam1_distortion_coeffs[2], cam1_distortion_coeffs[3]); + cam1.distortion_coeffs[0], cam1.distortion_coeffs[1], + cam1.distortion_coeffs[2], cam1.distortion_coeffs[3]); cout << R_imu_cam0 << endl; cout << t_imu_cam0.t() << endl; @@ -455,7 +455,7 @@ void ImageProcessor::trackFeatures() { vector track_inliers(0); predictFeatureTracking(prev_cam0_points, - cam0_R_p_c, cam0_intrinsics, curr_cam0_points); + cam0_R_p_c, cam0.intrinsics, curr_cam0_points); calcOpticalFlowPyrLK( prev_cam0_pyramid_, curr_cam0_pyramid_, @@ -550,14 +550,14 @@ void ImageProcessor::trackFeatures() { // Step 2 and 3: RANSAC on temporal image pairs of cam0 and cam1. vector cam0_ransac_inliers(0); twoPointRansac(prev_matched_cam0_points, curr_matched_cam0_points, - cam0_R_p_c, cam0_intrinsics, cam0_distortion_model, - cam0_distortion_coeffs, processor_config.ransac_threshold, + cam0_R_p_c, cam0.intrinsics, cam0.distortion_model, + cam0.distortion_coeffs, processor_config.ransac_threshold, 0.99, cam0_ransac_inliers); vector cam1_ransac_inliers(0); twoPointRansac(prev_matched_cam1_points, curr_matched_cam1_points, - cam1_R_p_c, cam1_intrinsics, cam1_distortion_model, - cam1_distortion_coeffs, processor_config.ransac_threshold, + cam1_R_p_c, cam1.intrinsics, cam1.distortion_model, + cam1.distortion_coeffs, processor_config.ransac_threshold, 0.99, cam1_ransac_inliers); // Number of features after ransac. @@ -619,12 +619,12 @@ void ImageProcessor::stereoMatch( // rotation from stereo extrinsics const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu; vector cam0_points_undistorted; - image_handler::undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model, - cam0_distortion_coeffs, cam0_points_undistorted, + image_handler::undistortPoints(cam0_points, cam0.intrinsics, cam0.distortion_model, + cam0.distortion_coeffs, cam0_points_undistorted, R_cam0_cam1); - cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1_intrinsics, - cam1_distortion_model, cam1_distortion_coeffs); + cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1.intrinsics, + cam1.distortion_model, cam1.distortion_coeffs); } // Track features using LK optical flow method. @@ -665,15 +665,15 @@ void ImageProcessor::stereoMatch( vector cam0_points_undistorted(0); vector cam1_points_undistorted(0); image_handler::undistortPoints( - cam0_points, cam0_intrinsics, cam0_distortion_model, - cam0_distortion_coeffs, cam0_points_undistorted); + cam0_points, cam0.intrinsics, cam0.distortion_model, + cam0.distortion_coeffs, cam0_points_undistorted); image_handler::undistortPoints( - cam1_points, cam1_intrinsics, cam1_distortion_model, - cam1_distortion_coeffs, cam1_points_undistorted); + cam1_points, cam1.intrinsics, cam1.distortion_model, + cam1.distortion_coeffs, cam1_points_undistorted); double norm_pixel_unit = 4.0 / ( - cam0_intrinsics[0]+cam0_intrinsics[1]+ - cam1_intrinsics[0]+cam1_intrinsics[1]); + cam0.intrinsics[0]+cam0.intrinsics[1]+ + cam1.intrinsics[0]+cam1.intrinsics[1]); for (int i = 0; i < cam0_points_undistorted.size(); ++i) { if (inlier_markers[i] == 0) continue; @@ -1253,11 +1253,11 @@ void ImageProcessor::publish() { vector curr_cam1_points_undistorted(0); image_handler::undistortPoints( - curr_cam0_points, cam0_intrinsics, cam0_distortion_model, - cam0_distortion_coeffs, curr_cam0_points_undistorted); + curr_cam0_points, cam0.intrinsics, cam0.distortion_model, + cam0.distortion_coeffs, curr_cam0_points_undistorted); image_handler::undistortPoints( - curr_cam1_points, cam1_intrinsics, cam1_distortion_model, - cam1_distortion_coeffs, curr_cam1_points_undistorted); + curr_cam1_points, cam1.intrinsics, cam1.distortion_model, + cam1.distortion_coeffs, curr_cam1_points_undistorted); for (int i = 0; i < curr_ids.size(); ++i) { feature_msg_ptr->features.push_back(FeatureMeasurement()); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index f5391dd..589c2bc 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -119,49 +119,49 @@ bool MsckfVio::loadParameters() { // get camera information (used for back projection) nh.param("cam0/distortion_model", - cam0_distortion_model, string("radtan")); + cam0.distortion_model, string("radtan")); nh.param("cam1/distortion_model", - cam1_distortion_model, string("radtan")); + cam1.distortion_model, string("radtan")); vector cam0_resolution_temp(2); nh.getParam("cam0/resolution", cam0_resolution_temp); - cam0_resolution[0] = cam0_resolution_temp[0]; - cam0_resolution[1] = cam0_resolution_temp[1]; + cam0.resolution[0] = cam0_resolution_temp[0]; + cam0.resolution[1] = cam0_resolution_temp[1]; vector cam1_resolution_temp(2); nh.getParam("cam1/resolution", cam1_resolution_temp); - cam1_resolution[0] = cam1_resolution_temp[0]; - cam1_resolution[1] = cam1_resolution_temp[1]; + cam1.resolution[0] = cam1_resolution_temp[0]; + cam1.resolution[1] = cam1_resolution_temp[1]; vector cam0_intrinsics_temp(4); nh.getParam("cam0/intrinsics", cam0_intrinsics_temp); - cam0_intrinsics[0] = cam0_intrinsics_temp[0]; - cam0_intrinsics[1] = cam0_intrinsics_temp[1]; - cam0_intrinsics[2] = cam0_intrinsics_temp[2]; - cam0_intrinsics[3] = cam0_intrinsics_temp[3]; + cam0.intrinsics[0] = cam0_intrinsics_temp[0]; + cam0.intrinsics[1] = cam0_intrinsics_temp[1]; + cam0.intrinsics[2] = cam0_intrinsics_temp[2]; + cam0.intrinsics[3] = cam0_intrinsics_temp[3]; vector cam1_intrinsics_temp(4); nh.getParam("cam1/intrinsics", cam1_intrinsics_temp); - cam1_intrinsics[0] = cam1_intrinsics_temp[0]; - cam1_intrinsics[1] = cam1_intrinsics_temp[1]; - cam1_intrinsics[2] = cam1_intrinsics_temp[2]; - cam1_intrinsics[3] = cam1_intrinsics_temp[3]; + cam1.intrinsics[0] = cam1_intrinsics_temp[0]; + cam1.intrinsics[1] = cam1_intrinsics_temp[1]; + cam1.intrinsics[2] = cam1_intrinsics_temp[2]; + cam1.intrinsics[3] = cam1_intrinsics_temp[3]; vector cam0_distortion_coeffs_temp(4); nh.getParam("cam0/distortion_coeffs", cam0_distortion_coeffs_temp); - cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0]; - cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1]; - cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2]; - cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3]; + cam0.distortion_coeffs[0] = cam0_distortion_coeffs_temp[0]; + cam0.distortion_coeffs[1] = cam0_distortion_coeffs_temp[1]; + cam0.distortion_coeffs[2] = cam0_distortion_coeffs_temp[2]; + cam0.distortion_coeffs[3] = cam0_distortion_coeffs_temp[3]; vector cam1_distortion_coeffs_temp(4); nh.getParam("cam1/distortion_coeffs", cam1_distortion_coeffs_temp); - cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0]; - cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1]; - cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; - cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; + cam1.distortion_coeffs[0] = cam1_distortion_coeffs_temp[0]; + cam1.distortion_coeffs[1] = cam1_distortion_coeffs_temp[1]; + cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; + cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; @@ -904,7 +904,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //photometric observation std::vector photo_z; - feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs, cam0_moving_window, photo_z); + feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs, cam0_moving_window, photo_z); // Convert the feature position from the world frame to @@ -921,9 +921,9 @@ void MsckfVio::PhotometricMeasurementJacobian( /* //prints the feature projection in pixel space std::vector my_p = image_handler::distortPoints( out_v, - cam0_intrinsics, - cam0_distortion_model, - cam0_distortion_coeffs); + cam0.intrinsics, + cam0.distortion_model, + cam0.distortion_coeffs); printf("projection: %f, %f\n", my_p[0].x, my_p[0].y); */ @@ -1316,7 +1316,7 @@ void MsckfVio::removeLostFeatures() { } } - if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs)) + if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs)) { invalid_feature_ids.push_back(feature.id); continue; @@ -1470,7 +1470,7 @@ void MsckfVio::pruneCamStateBuffer() { } } - if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs)) + if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); From 819e43bb3b34325cabf1506375797866bfcef47d Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 17 Apr 2019 09:03:27 +0200 Subject: [PATCH 10/86] fixed pixel position return value --- include/msckf_vio/feature.hpp | 62 +++++++++++------------------------ src/msckf_vio.cpp | 7 ++-- 2 files changed, 22 insertions(+), 47 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 5a756c2..8215cad 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -127,9 +127,7 @@ struct Feature { bool initializeAnchor( const movingWindow& cam0_moving_window, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs); + const CameraCalibration& cam); /* @@ -150,9 +148,7 @@ struct Feature { inline cv::Point2f projectPositionToCamera( const CAMState& cam_state, const StateIDType& cam_state_id, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs, + const CameraCalibration& cam, Eigen::Vector3d& in_p) const; /* @@ -164,9 +160,7 @@ struct Feature { bool IrradianceOfAnchorPatch( const CAMState& cam_state, const StateIDType& cam_state_id, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs, + const CameraCalibration& cam, const movingWindow& cam0_moving_window, std::vector& anchorPatch_measurement) const; @@ -174,11 +168,8 @@ struct Feature { * @brief projectPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em */ - inline bool projectPixelToPosition(cv::Point2f in_p, - Eigen::Vector3d& out_p, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs); +inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, + const CameraCalibration& cam); /* * @brief Irradiance returns irradiance value of a pixel @@ -362,16 +353,14 @@ bool Feature::checkMotion( bool Feature::IrradianceOfAnchorPatch( const CAMState& cam_state, const StateIDType& cam_state_id, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs, + const CameraCalibration& cam, const movingWindow& cam0_moving_window, std::vector& anchorPatch_measurement) const { //project every point in anchorPatch_3d. for (auto point : anchorPatch_3d) { - cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, intrinsics, distortion_model, distortion_coeffs, point); + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point); uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second); anchorPatch_measurement.push_back(irradiance); } @@ -385,9 +374,7 @@ uint8_t Feature::Irradiance(cv::Point2f pose, cv::Mat image) const cv::Point2f Feature::projectPositionToCamera( const CAMState& cam_state, const StateIDType& cam_state_id, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs, + const CameraCalibration& cam, Eigen::Vector3d& in_p) const { Eigen::Isometry3d T_c0_w; @@ -402,23 +389,17 @@ cv::Point2f Feature::projectPositionToCamera( out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); std::vector out_v; out_v.push_back(out_p); - std::vector my_p = image_handler::distortPoints( out_v, - intrinsics, - distortion_model, - distortion_coeffs); + std::vector my_p = image_handler::distortPoints(out_v, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); - return out_p; + return my_p[0]; } -bool Feature::projectPixelToPosition(cv::Point2f in_p, - Eigen::Vector3d& out_p, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs) +Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, + const CameraCalibration& cam) { // use undistorted position of point of interest // project it back into 3D space using pinhole model @@ -426,7 +407,7 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p, Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho); Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); - anchorPatch_3d.push_back(PositionInWorld); + return PositionInWorld; //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); } @@ -434,9 +415,7 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p, //@test center projection must always be initial feature projection bool Feature::initializeAnchor( const movingWindow& cam0_moving_window, - const cv::Vec4d& intrinsics, - const std::string& distortion_model, - const cv::Vec4d& distortion_coeffs) + const CameraCalibration& cam) { int N = 3; @@ -447,8 +426,8 @@ bool Feature::initializeAnchor( return false; cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second; - auto u = anchor->second(0)*intrinsics[0] + intrinsics[2]; - auto v = anchor->second(1)*intrinsics[1] + intrinsics[3]; + auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2]; + auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3]; int count = 0; //go through surrounding pixels @@ -457,12 +436,9 @@ bool Feature::initializeAnchor( for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1) { anchorPatch.push_back(anchorImage.at((int)u_run,(int)v_run)); - Eigen::Vector3d Npose; - projectPixelToPosition(cv::Point2f((u_run-intrinsics[2])/intrinsics[0], (v_run-intrinsics[3])/intrinsics[1]), - Npose, - intrinsics, - distortion_model, - distortion_coeffs); + cv::Point2f currentPoint((u_run-cam.intrinsics[2])/cam.intrinsics[0], (v_run-cam.intrinsics[3])/cam.intrinsics[1]); + Eigen::Vector3d Npose = projectPixelToPosition(currentPoint, cam); + anchorPatch_3d.push_back(Npose); } } diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 589c2bc..095b71b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -904,8 +904,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //photometric observation std::vector photo_z; - feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs, cam0_moving_window, photo_z); - + feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z); // Convert the feature position from the world frame to // the cam0 and cam1 frame. @@ -1316,7 +1315,7 @@ void MsckfVio::removeLostFeatures() { } } - if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs)) + if(!feature.initializeAnchor(cam0_moving_window, cam0)) { invalid_feature_ids.push_back(feature.id); continue; @@ -1470,7 +1469,7 @@ void MsckfVio::pruneCamStateBuffer() { } } - if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs)) + if(!feature.initializeAnchor(cam0_moving_window, cam0)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); From 6ae83f0db786c989932c7513189d5ed98745edcb Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 17 Apr 2019 10:54:54 +0200 Subject: [PATCH 11/86] added saving exposure time from the frame ID, where the TUM dataset saves it --- include/msckf_vio/cam_state.h | 10 ++++++++-- include/msckf_vio/feature.hpp | 4 ++-- include/msckf_vio/msckf_vio.h | 4 ++-- src/msckf_vio.cpp | 27 ++++++++++++++++----------- 4 files changed, 28 insertions(+), 17 deletions(-) diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index e56b6c9..a58da92 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -30,6 +30,11 @@ struct CameraCalibration{ cv::Vec4d distortion_coeffs; }; +struct Frame{ + cv::Mat image; + double exposureTime_ms; +}; + /* * @brief CAMState Stored camera state in order to * form measurement model. @@ -50,6 +55,7 @@ struct CAMState { // Position of the camera frame in the world frame. Eigen::Vector3d position; + // Illumination Information of the frame IlluminationParameter illumination; // These two variables should have the same physical @@ -80,9 +86,9 @@ typedef std::map, Eigen::aligned_allocator< std::pair > > CamStateServer; -typedef std::map, +typedef std::map, Eigen::aligned_allocator< - std::pair > > movingWindow; + std::pair > > movingWindow; } // namespace msckf_vio #endif // MSCKF_VIO_CAM_STATE_H diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 8215cad..5026c3d 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -361,7 +361,7 @@ bool Feature::IrradianceOfAnchorPatch( for (auto point : anchorPatch_3d) { cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point); - uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second); + uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second.image); anchorPatch_measurement.push_back(irradiance); } } @@ -425,7 +425,7 @@ bool Feature::initializeAnchor( if(cam0_moving_window.find(anchor->first) == cam0_moving_window.end()) return false; - cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second; + cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second.image; auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2]; auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3]; int count = 0; diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index b87349b..07da6bd 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -145,8 +145,8 @@ class MsckfVio { std_srvs::Trigger::Response& res); void manageMovingWindow( - const cv_bridge::CvImageConstPtr& cam0_image_ptr, - const cv_bridge::CvImageConstPtr& cam1_image_ptr, + const sensor_msgs::ImageConstPtr& cam0_img, + const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg); // Filter related functions diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 095b71b..3625541 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -310,12 +310,6 @@ void MsckfVio::imageCallback( // Return if the gravity vector has not been set. if (!is_gravity_set) return; - // Get the current image. - cv_bridge::CvImageConstPtr cam0_image_ptr = cv_bridge::toCvShare(cam0_img, - sensor_msgs::image_encodings::MONO8); - cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img, - sensor_msgs::image_encodings::MONO8); - // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -350,7 +344,7 @@ void MsckfVio::imageCallback( // Add new images to moving window start_time = ros::Time::now(); - manageMovingWindow(cam0_image_ptr, cam1_img_ptr, feature_msg); + manageMovingWindow(cam0_img, cam1_img, feature_msg); double manage_moving_window_time = ( ros::Time::now()-start_time).toSec(); @@ -399,12 +393,23 @@ void MsckfVio::imageCallback( } void MsckfVio::manageMovingWindow( - const cv_bridge::CvImageConstPtr& cam0_image_ptr, - const cv_bridge::CvImageConstPtr& cam1_image_ptr, + const sensor_msgs::ImageConstPtr& cam0_img, + const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - cam0_moving_window[state_server.imu_state.id] = cam0_image_ptr->image.clone(); - cam1_moving_window[state_server.imu_state.id] = cam1_image_ptr->image.clone(); + //save exposure Time into moving window + cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000; + cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000; + printf("exposure: %f\n", cam0_moving_window[state_server.imu_state.id].exposureTime_ms); + // Get the current image. + cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img, + sensor_msgs::image_encodings::MONO8); + cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img, + sensor_msgs::image_encodings::MONO8); + + // save image information into moving window + cam0_moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone(); + cam1_moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) while(cam0_moving_window.size() > 100) From f4a17f85125618ae6b2c40a34d77dce0e452380c Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 17 Apr 2019 16:16:45 +0200 Subject: [PATCH 12/86] deactivated most to find reason for slowdown --- include/msckf_vio/feature.hpp | 82 +++++++++++++++++++++++++------ src/msckf_vio.cpp | 91 ++++++++++++++++++++++------------- 2 files changed, 123 insertions(+), 50 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 5026c3d..3793d49 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -157,12 +157,19 @@ struct Feature { * */ - bool IrradianceOfAnchorPatch( + bool estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - const CameraCalibration& cam, + const CameraCalibration& cam0, const movingWindow& cam0_moving_window, - std::vector& anchorPatch_measurement) const; + std::vector& anchorPatch_estimate) const; + + bool FrameIrradiance( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const CameraCalibration& cam0, + const movingWindow& cam0_moving_window, + std::vector& anchorPatch_measurement) const; /* * @brief projectPixelToPosition uses the calcualted pixels @@ -175,7 +182,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, * @brief Irradiance returns irradiance value of a pixel */ - inline uint8_t Irradiance(cv::Point2f pose, cv::Mat image) const; + inline float PixelIrradiance(cv::Point2f pose, cv::Mat image) const; // An unique identifier for the feature. // In case of long time running, the variable @@ -350,25 +357,56 @@ bool Feature::checkMotion( else return false; } -bool Feature::IrradianceOfAnchorPatch( +bool Feature::estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - const CameraCalibration& cam, + const CameraCalibration& cam0, const movingWindow& cam0_moving_window, - std::vector& anchorPatch_measurement) const + std::vector& anchorPatch_estimate) const +{ + // get irradiance of patch in anchor frame + // subtract estimated b and divide by a of anchor frame + // muliply by a and add b of this frame + + auto anchor = observations.begin(); + if(cam0_moving_window.find(anchor->first) == cam0_moving_window.end()) + return false; + + double anchorExposureTime_ms = cam0_moving_window.find(anchor->first)->second.exposureTime_ms; + double frameExposureTime_ms = cam0_moving_window.find(cam_state_id)->second.exposureTime_ms; + + + double a_A = anchorExposureTime_ms; + double b_A = 0; + double a_l =frameExposureTime_ms; + double b_l = 0; + for (double anchorPixel : anchorPatch) + { + float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l; + anchorPatch_estimate.push_back(irradiance); + } + +} + +bool Feature::FrameIrradiance( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const CameraCalibration& cam0, + const movingWindow& cam0_moving_window, + std::vector& anchorPatch_measurement) const { //project every point in anchorPatch_3d. for (auto point : anchorPatch_3d) { - cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point); - uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second.image); + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); + float irradiance = PixelIrradiance(p_in_c0, cam0_moving_window.find(cam_state_id)->second.image); anchorPatch_measurement.push_back(irradiance); } } -uint8_t Feature::Irradiance(cv::Point2f pose, cv::Mat image) const +float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const { - return image.at(pose.x, pose.y); + return (float)image.at(pose.x, pose.y); } cv::Point2f Feature::projectPositionToCamera( @@ -389,7 +427,10 @@ cv::Point2f Feature::projectPositionToCamera( out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); std::vector out_v; out_v.push_back(out_p); - std::vector my_p = image_handler::distortPoints(out_v, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); + std::vector my_p = image_handler::distortPoints(out_v, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); @@ -418,6 +459,8 @@ bool Feature::initializeAnchor( const CameraCalibration& cam) { + //initialize patch Size + //TODO make N size a ros parameter int N = 3; int n = (int)(N-1)/2; @@ -428,25 +471,32 @@ bool Feature::initializeAnchor( cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second.image; auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2]; auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3]; - int count = 0; - //go through surrounding pixels + //for NxN patch pixels around feature for(double u_run = u - n; u_run <= u + n; u_run = u_run + 1) { for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1) { - anchorPatch.push_back(anchorImage.at((int)u_run,(int)v_run)); - cv::Point2f currentPoint((u_run-cam.intrinsics[2])/cam.intrinsics[0], (v_run-cam.intrinsics[3])/cam.intrinsics[1]); + // add irradiance information + anchorPatch.push_back((double)anchorImage.at((int)u_run,(int)v_run)); + + // project patch pixel to 3D space + auto intr = cam.intrinsics; + cv::Point2f currentPoint((u_run-intr[2])/intr[0], (v_run-intr[3])/intr[1]); Eigen::Vector3d Npose = projectPixelToPosition(currentPoint, cam); + + //save position anchorPatch_3d.push_back(Npose); } } + //TODO test if NxN patch can be selected return true; } bool Feature::initializePosition( const CamStateServer& cam_states) { + // Organize camera poses and feature observations properly. std::vector > cam_poses(0); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 3625541..2a1f37a 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -344,7 +344,7 @@ void MsckfVio::imageCallback( // Add new images to moving window start_time = ros::Time::now(); - manageMovingWindow(cam0_img, cam1_img, feature_msg); + //manageMovingWindow(cam0_img, cam1_img, feature_msg); double manage_moving_window_time = ( ros::Time::now()-start_time).toSec(); @@ -373,20 +373,20 @@ void MsckfVio::imageCallback( processing_end_time - processing_start_time; if (processing_time > 1.0/frame_rate) { ++critical_time_cntr; - //ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", - // processing_time, critical_time_cntr); - //printf("IMU processing time: %f/%f\n", - // imu_processing_time, imu_processing_time/processing_time); - //printf("State augmentation time: %f/%f\n", - // state_augmentation_time, state_augmentation_time/processing_time); - //printf("Add observations time: %f/%f\n", - // add_observations_time, add_observations_time/processing_time); - //printf("Remove lost features time: %f/%f\n", - // remove_lost_features_time, remove_lost_features_time/processing_time); - //printf("Remove camera states time: %f/%f\n", - // prune_cam_states_time, prune_cam_states_time/processing_time); - //printf("Publish time: %f/%f\n", - // publish_time, publish_time/processing_time); + ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", + processing_time, critical_time_cntr); + printf("IMU processing time: %f/%f\n", + imu_processing_time, imu_processing_time/processing_time); + printf("State augmentation time: %f/%f\n", + state_augmentation_time, state_augmentation_time/processing_time); + printf("Add observations time: %f/%f\n", + add_observations_time, add_observations_time/processing_time); + printf("Remove lost features time: %f/%f\n", + remove_lost_features_time, remove_lost_features_time/processing_time); + printf("Remove camera states time: %f/%f\n", + prune_cam_states_time, prune_cam_states_time/processing_time); + printf("Publish time: %f/%f\n", + publish_time, publish_time/processing_time); } return; @@ -400,7 +400,15 @@ void MsckfVio::manageMovingWindow( //save exposure Time into moving window cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000; cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000; - printf("exposure: %f\n", cam0_moving_window[state_server.imu_state.id].exposureTime_ms); + if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms < 1) + cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 1; + if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms < 1) + cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 1; + if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 500) + cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 500; + if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 500) + cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 500; + // Get the current image. cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img, sensor_msgs::image_encodings::MONO8); @@ -908,8 +916,8 @@ void MsckfVio::PhotometricMeasurementJacobian( const Vector4d& z = feature.observations.find(cam_state_id)->second; //photometric observation - std::vector photo_z; - feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z); + std::vector photo_z; + //feature.FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z); // Convert the feature position from the world frame to // the cam0 and cam1 frame. @@ -973,6 +981,20 @@ void MsckfVio::PhotometricMeasurementJacobian( r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); + //estimate photometric measurement + std::vector estimate_photo_z; + //feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z); + std::vector photo_r; + + //calculate photom. residual + //for(int i = 0; i < photo_z.size(); i++) + // photo_r.push_back(photo_z[i] - estimate_photo_z[i]); + + // printf("-----\n"); + // for(int i = 0; i < photo_z.size(); i++) + // printf("%.4f - %.4f\n", photo_z[i], estimate_photo_z[i]); + + photo_z.clear(); return; } @@ -1002,6 +1024,7 @@ void MsckfVio::PhotometricFeatureJacobian( VectorXd r_j = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; + for (const auto& cam_id : valid_cam_state_ids) { Matrix H_xi = Matrix::Zero(); @@ -1318,12 +1341,11 @@ void MsckfVio::removeLostFeatures() { continue; } } - } - - if(!feature.initializeAnchor(cam0_moving_window, cam0)) - { - invalid_feature_ids.push_back(feature.id); - continue; + /*if(!feature.initializeAnchor(cam0_moving_window, cam0)) + { + invalid_feature_ids.push_back(feature.id); + continue; + }*/ } jacobian_row_size += 4*feature.observations.size() - 3; @@ -1472,13 +1494,12 @@ void MsckfVio::pruneCamStateBuffer() { continue; } } - } - - if(!feature.initializeAnchor(cam0_moving_window, cam0)) - { - for (const auto& cam_id : involved_cam_state_ids) - feature.observations.erase(cam_id); - continue; + /*if(!feature.initializeAnchor(cam0_moving_window, cam0)) + { + for (const auto& cam_id : involved_cam_state_ids) + feature.observations.erase(cam_id); + continue; + }*/ } jacobian_row_size += 4*involved_cam_state_ids.size() - 3; @@ -1491,7 +1512,7 @@ void MsckfVio::pruneCamStateBuffer() { 21+6*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; - + ros::Time start_time = ros::Time::now(); for (auto& item : map_server) { auto& feature = item.second; // Check how many camera states to be removed are associated @@ -1507,8 +1528,8 @@ void MsckfVio::pruneCamStateBuffer() { MatrixXd H_xj; VectorXd r_j; + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; @@ -1518,13 +1539,15 @@ void MsckfVio::pruneCamStateBuffer() { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } + double anchorPrune_processing_time = (ros::Time::now()-start_time).toSec(); + printf("FeatureJacobian Time: %f\n", anchorPrune_processing_time); H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); // Perform measurement update. measurementUpdate(H_x, r); - + for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_id)); From cfecefe29f4ef07aab236a831f912f5f43bebf26 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 17 Apr 2019 17:06:44 +0200 Subject: [PATCH 13/86] reinstantiated photometry removed slow-down problem --- src/msckf_vio.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 2a1f37a..bc38a4e 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -917,7 +917,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //photometric observation std::vector photo_z; - //feature.FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z); + feature.FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z); // Convert the feature position from the world frame to // the cam0 and cam1 frame. @@ -983,12 +983,12 @@ void MsckfVio::PhotometricMeasurementJacobian( //estimate photometric measurement std::vector estimate_photo_z; - //feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z); + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z); std::vector photo_r; //calculate photom. residual - //for(int i = 0; i < photo_z.size(); i++) - // photo_r.push_back(photo_z[i] - estimate_photo_z[i]); + for(int i = 0; i < photo_z.size(); i++) + photo_r.push_back(photo_z[i] - estimate_photo_z[i]); // printf("-----\n"); // for(int i = 0; i < photo_z.size(); i++) @@ -1341,11 +1341,11 @@ void MsckfVio::removeLostFeatures() { continue; } } - /*if(!feature.initializeAnchor(cam0_moving_window, cam0)) + if(!feature.initializeAnchor(cam0_moving_window, cam0)) { invalid_feature_ids.push_back(feature.id); continue; - }*/ + } } jacobian_row_size += 4*feature.observations.size() - 3; @@ -1494,12 +1494,12 @@ void MsckfVio::pruneCamStateBuffer() { continue; } } - /*if(!feature.initializeAnchor(cam0_moving_window, cam0)) + if(!feature.initializeAnchor(cam0_moving_window, cam0)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); continue; - }*/ + } } jacobian_row_size += 4*involved_cam_state_ids.size() - 3; From d91ff7ca9ddb59318d9cb4d9b35ca2ea03907e4d Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Thu, 18 Apr 2019 11:06:45 +0200 Subject: [PATCH 14/86] added tum launch files, removed anchor procedure being called multiple times through a flag --- config/camchain-imucam-tum.yaml | 36 ++++++++++++++ include/msckf_vio/feature.hpp | 23 ++++++--- launch/image_processor_tum.launch | 34 +++++++++++++ launch/msckf_vio_tum.launch | 64 +++++++++++++++++++++++++ src/image_processor.cpp | 79 +------------------------------ src/msckf_vio.cpp | 34 ++++++------- 6 files changed, 170 insertions(+), 100 deletions(-) create mode 100644 config/camchain-imucam-tum.yaml create mode 100644 launch/image_processor_tum.launch create mode 100644 launch/msckf_vio_tum.launch diff --git a/config/camchain-imucam-tum.yaml b/config/camchain-imucam-tum.yaml new file mode 100644 index 0000000..7edbd87 --- /dev/null +++ b/config/camchain-imucam-tum.yaml @@ -0,0 +1,36 @@ +cam0: + T_cam_imu: + [-0.9995378259923383, 0.02917807204183088, -0.008530798463872679, 0.047094247958417004, + 0.007526588843243184, -0.03435493139706542, -0.9993813532126198, -0.04788273017221637, + -0.029453096117288798, -0.9989836729399656, 0.034119442089241274, -0.0697294754693238, + 0.0, 0.0, 0.0, 1.0] + camera_model: pinhole + distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, + -0.001662284667857643] + distortion_model: equidistant + intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506] + resolution: [1024, 1024] + rostopic: /cam0/image_raw +cam1: + T_cam_imu: + [-0.9995240747493029, 0.02986739485347808, -0.007717688852024281, -0.05374086123613335, + 0.008095979457928231, 0.01256553460985914, -0.9998882749870535, -0.04648588412432889, + -0.02976708103202316, -0.9994748851595197, -0.0128013601698453, -0.07333210787623645, + 0.0, 0.0, 0.0, 1.0] + T_cn_cnm1: + [0.9999994317488622, -0.0008361847221513937, -0.0006612844045898121, -0.10092123225528335, + 0.0008042457277382264, 0.9988989443471681, -0.04690684567228134, -0.001964540595211977, + 0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572, + 0.0, 0.0, 0.0, 1.0] + camera_model: pinhole + distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, + -0.002347858896562788] + distortion_model: equidistant + intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407] + resolution: [1024, 1024] + rostopic: /cam1/image_raw +T_imu_body: + [1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000] \ No newline at end of file diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 3793d49..492cba3 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -59,11 +59,11 @@ struct Feature { // Constructors for the struct. Feature(): id(0), position(Eigen::Vector3d::Zero()), - is_initialized(false) {} + is_initialized(false), is_anchored(false) {} Feature(const FeatureIDType& new_id): id(new_id), position(Eigen::Vector3d::Zero()), - is_initialized(false) {} + is_initialized(false), is_anchored(false) {} /* * @brief cost Compute the cost of the camera observations @@ -217,7 +217,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, // A indicator to show if the 3d postion of the feature // has been initialized or not. bool is_initialized; - + bool is_anchored; // Noise for a normalized feature measurement. static double observation_noise; @@ -380,6 +380,8 @@ bool Feature::estimate_FrameIrradiance( double b_A = 0; double a_l =frameExposureTime_ms; double b_l = 0; + printf("frames: %lld, %lld\n", anchor->first, cam_state_id); + printf("exposure: %f, %f\n", a_A, a_l); for (double anchorPixel : anchorPatch) { float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l; @@ -396,6 +398,8 @@ bool Feature::FrameIrradiance( std::vector& anchorPatch_measurement) const { //project every point in anchorPatch_3d. + if(!is_anchored) + printf("not anchored!\n"); for (auto point : anchorPatch_3d) { cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); @@ -471,12 +475,19 @@ bool Feature::initializeAnchor( cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second.image; auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2]; auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3]; - + printf("initializing anchor\n"); + if(u - n < 0 || u + n >= cam.resolution(0) || v - n < 0 || v + n >= cam.resolution(1)) + { + printf("no good: \n"); + printf("%f, %f\n", u, v); + return false; + } //for NxN patch pixels around feature for(double u_run = u - n; u_run <= u + n; u_run = u_run + 1) { for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1) { + printf("ADDING\n"); // add irradiance information anchorPatch.push_back((double)anchorImage.at((int)u_run,(int)v_run)); @@ -489,8 +500,8 @@ bool Feature::initializeAnchor( anchorPatch_3d.push_back(Npose); } } - - //TODO test if NxN patch can be selected + printf("set to true!!!\n"); + is_anchored = true; return true; } diff --git a/launch/image_processor_tum.launch b/launch/image_processor_tum.launch new file mode 100644 index 0000000..d2a02e4 --- /dev/null +++ b/launch/image_processor_tum.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch new file mode 100644 index 0000000..247e824 --- /dev/null +++ b/launch/msckf_vio_tum.launch @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 4e26618..d35bb16 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -390,7 +390,6 @@ void ImageProcessor::predictFeatureTracking( const cv::Matx33f& R_p_c, const cv::Vec4d& intrinsics, vector& compensated_pts) { - // Return directly if there are no input features. if (input_pts.size() == 0) { compensated_pts.clear(); @@ -421,7 +420,6 @@ void ImageProcessor::trackFeatures() { cam0_curr_img_ptr->image.rows / processor_config.grid_row; static int grid_width = cam0_curr_img_ptr->image.cols / processor_config.grid_col; - // Compute a rough relative rotation which takes a vector // from the previous frame to the current frame. Matx33f cam0_R_p_c; @@ -611,7 +609,6 @@ void ImageProcessor::stereoMatch( const vector& cam0_points, vector& cam1_points, vector& inlier_markers) { - if (cam0_points.size() == 0) return; if(cam1_points.size() == 0) { @@ -700,8 +697,8 @@ void ImageProcessor::addNewFeatures() { cam0_curr_img_ptr->image.rows / processor_config.grid_row; static int grid_width = cam0_curr_img_ptr->image.cols / processor_config.grid_col; - // Create a mask to avoid redetecting existing features. + Mat mask(curr_img.rows, curr_img.cols, CV_8U, Scalar(1)); for (const auto& features : *curr_features_ptr) { @@ -721,7 +718,6 @@ void ImageProcessor::addNewFeatures() { mask(row_range, col_range) = 0; } } - // Detect new features. vector new_features(0); detector_ptr->detect(curr_img, new_features, mask); @@ -736,7 +732,6 @@ void ImageProcessor::addNewFeatures() { new_feature_sieve[ row*processor_config.grid_col+col].push_back(feature); } - new_features.clear(); for (auto& item : new_feature_sieve) { if (item.size() > processor_config.grid_max_feature_num) { @@ -749,7 +744,6 @@ void ImageProcessor::addNewFeatures() { } int detected_new_features = new_features.size(); - // Find the stereo matched points for the newly // detected features. vector cam0_points(new_features.size()); @@ -777,7 +771,6 @@ void ImageProcessor::addNewFeatures() { static_cast(detected_new_features) < 0.1) ROS_WARN("Images at [%f] seems unsynced...", cam0_curr_img_ptr->header.stamp.toSec()); - // Group the features into grids GridFeatures grid_new_features; for (int code = 0; code < @@ -799,7 +792,6 @@ void ImageProcessor::addNewFeatures() { new_feature.cam1_point = cam1_point; grid_new_features[code].push_back(new_feature); } - // Sort the new features in each grid based on its response. for (auto& item : grid_new_features) std::sort(item.second.begin(), item.second.end(), @@ -849,73 +841,6 @@ void ImageProcessor::pruneGridFeatures() { return; } -void ImageProcessor::undistortPoints( - const vector& pts_in, - const cv::Vec4d& intrinsics, - const string& distortion_model, - const cv::Vec4d& distortion_coeffs, - vector& pts_out, - const cv::Matx33d &rectification_matrix, - const cv::Vec4d &new_intrinsics) { - - if (pts_in.size() == 0) return; - - const cv::Matx33d K( - intrinsics[0], 0.0, intrinsics[2], - 0.0, intrinsics[1], intrinsics[3], - 0.0, 0.0, 1.0); - - const cv::Matx33d K_new( - new_intrinsics[0], 0.0, new_intrinsics[2], - 0.0, new_intrinsics[1], new_intrinsics[3], - 0.0, 0.0, 1.0); - - if (distortion_model == "radtan") { - cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, - rectification_matrix, K_new); - } else if (distortion_model == "equidistant") { - cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs, - rectification_matrix, K_new); - } else { - ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", - distortion_model.c_str()); - cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, - rectification_matrix, K_new); - } - - return; -} - -vector ImageProcessor::distortPoints( - const vector& pts_in, - const cv::Vec4d& intrinsics, - const string& distortion_model, - const cv::Vec4d& distortion_coeffs) { - - const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2], - 0.0, intrinsics[1], intrinsics[3], - 0.0, 0.0, 1.0); - - vector pts_out; - if (distortion_model == "radtan") { - vector homogenous_pts; - cv::convertPointsToHomogeneous(pts_in, homogenous_pts); - cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K, - distortion_coeffs, pts_out); - } else if (distortion_model == "equidistant") { - cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs); - } else { - ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", - distortion_model.c_str()); - vector homogenous_pts; - cv::convertPointsToHomogeneous(pts_in, homogenous_pts); - cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K, - distortion_coeffs, pts_out); - } - - return pts_out; -} - void ImageProcessor::integrateImuData( Matx33f& cam0_R_p_c, Matx33f& cam1_R_p_c) { // Find the start and the end limit within the imu msg buffer. @@ -967,7 +892,6 @@ void ImageProcessor::integrateImuData( void ImageProcessor::rescalePoints( vector& pts1, vector& pts2, float& scaling_factor) { - scaling_factor = 0.0f; for (int i = 0; i < pts1.size(); ++i) { @@ -1232,7 +1156,6 @@ void ImageProcessor::twoPointRansac( } void ImageProcessor::publish() { - // Publish features. CameraMeasurementPtr feature_msg_ptr(new CameraMeasurement); feature_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index bc38a4e..0d876d3 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -306,7 +306,6 @@ void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - // Return if the gravity vector has not been set. if (!is_gravity_set) return; @@ -344,7 +343,7 @@ void MsckfVio::imageCallback( // Add new images to moving window start_time = ros::Time::now(); - //manageMovingWindow(cam0_img, cam1_img, feature_msg); + manageMovingWindow(cam0_img, cam1_img, feature_msg); double manage_moving_window_time = ( ros::Time::now()-start_time).toSec(); @@ -398,16 +397,16 @@ void MsckfVio::manageMovingWindow( const CameraMeasurementConstPtr& feature_msg) { //save exposure Time into moving window - cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000; - cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000; + cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000000; + cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000000; if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms < 1) cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 1; if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms < 1) cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 1; - if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 500) - cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 500; - if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 500) - cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 500; + if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 100) + cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 100; + if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 100) + cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 100; // Get the current image. cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img, @@ -981,6 +980,8 @@ void MsckfVio::PhotometricMeasurementJacobian( r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); + printf("-----\n"); + //estimate photometric measurement std::vector estimate_photo_z; feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z); @@ -990,9 +991,9 @@ void MsckfVio::PhotometricMeasurementJacobian( for(int i = 0; i < photo_z.size(); i++) photo_r.push_back(photo_z[i] - estimate_photo_z[i]); - // printf("-----\n"); - // for(int i = 0; i < photo_z.size(); i++) - // printf("%.4f - %.4f\n", photo_z[i], estimate_photo_z[i]); + + for(int i = 0; i < photo_z.size(); i++) + printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); photo_z.clear(); return; @@ -1341,6 +1342,9 @@ void MsckfVio::removeLostFeatures() { continue; } } + } + if(!feature.is_anchored) + { if(!feature.initializeAnchor(cam0_moving_window, cam0)) { invalid_feature_ids.push_back(feature.id); @@ -1477,7 +1481,6 @@ void MsckfVio::pruneCamStateBuffer() { feature.observations.erase(involved_cam_state_ids[0]); continue; } - if (!feature.is_initialized) { // Check if the feature can be initialize. if (!feature.checkMotion(state_server.cam_states)) { @@ -1494,6 +1497,9 @@ void MsckfVio::pruneCamStateBuffer() { continue; } } + } + if(!feature.is_anchored) + { if(!feature.initializeAnchor(cam0_moving_window, cam0)) { for (const auto& cam_id : involved_cam_state_ids) @@ -1501,7 +1507,6 @@ void MsckfVio::pruneCamStateBuffer() { continue; } } - jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -1512,7 +1517,6 @@ void MsckfVio::pruneCamStateBuffer() { 21+6*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; - ros::Time start_time = ros::Time::now(); for (auto& item : map_server) { auto& feature = item.second; // Check how many camera states to be removed are associated @@ -1539,8 +1543,6 @@ void MsckfVio::pruneCamStateBuffer() { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } - double anchorPrune_processing_time = (ros::Time::now()-start_time).toSec(); - printf("FeatureJacobian Time: %f\n", anchorPrune_processing_time); H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); From 1fa251821536ce69c53c237ae73d4d4482ac0ae6 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Thu, 18 Apr 2019 16:22:41 +0200 Subject: [PATCH 15/86] fixed incorrect undistortion/distortion. residual should now be calculated correctly --- include/msckf_vio/feature.hpp | 118 ++++++++++++++++++++---------- include/msckf_vio/image_handler.h | 6 ++ src/image_handler.cpp | 33 +++++++++ src/msckf_vio.cpp | 11 +-- 4 files changed, 120 insertions(+), 48 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 492cba3..4eca45c 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -144,7 +144,12 @@ struct Feature { inline bool initializePosition( const CamStateServer& cam_states); - +/* +* @brief project PositionToCamera Takes a 3d position in a world frame +* and projects it into the passed camera frame using pinhole projection +* then distorts it using camera information to get +* the resulting distorted pixel position +*/ inline cv::Point2f projectPositionToCamera( const CAMState& cam_state, const StateIDType& cam_state_id, @@ -212,12 +217,15 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, Eigen::Vector3d position; // inverse depth representation - double rho; + double anchor_rho; // A indicator to show if the 3d postion of the feature // has been initialized or not. bool is_initialized; bool is_anchored; + + cv::Point2f anchor_center_pos; + cv::Point2f undist_anchor_center_pos; // Noise for a normalized feature measurement. static double observation_noise; @@ -397,14 +405,24 @@ bool Feature::FrameIrradiance( const movingWindow& cam0_moving_window, std::vector& anchorPatch_measurement) const { - //project every point in anchorPatch_3d. - if(!is_anchored) - printf("not anchored!\n"); + // project every point in anchorPatch_3d. + // int count = 0; for (auto point : anchorPatch_3d) { + // testing + //if(cam_state_id == observations.begin()->first) + //if(count == 4) + //printf("\n\ncenter:\n"); + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); float irradiance = PixelIrradiance(p_in_c0, cam0_moving_window.find(cam_state_id)->second.image); anchorPatch_measurement.push_back(irradiance); + + // testing + //if(cam_state_id == observations.begin()->first) + //if(count++ == 4) + //printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y); + } } @@ -429,18 +447,20 @@ cv::Point2f Feature::projectPositionToCamera( Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); - std::vector out_v; - out_v.push_back(out_p); - std::vector my_p = image_handler::distortPoints(out_v, - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs); - + + // if(cam_state_id == observations.begin()->first) + //printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y); + + cv::Point2f my_p = image_handler::distortPoint(out_p, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); + // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); - return my_p[0]; + return my_p; } Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, @@ -450,7 +470,7 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, // project it back into 3D space using pinhole model // save resulting NxN positions for this feature - Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho); + Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho); Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); return PositionInWorld; //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); @@ -473,34 +493,56 @@ bool Feature::initializeAnchor( return false; cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second.image; - auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2]; - auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3]; - printf("initializing anchor\n"); - if(u - n < 0 || u + n >= cam.resolution(0) || v - n < 0 || v + n >= cam.resolution(1)) - { - printf("no good: \n"); - printf("%f, %f\n", u, v); - return false; - } + auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2]; + auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3]; + + //testing + undist_anchor_center_pos = cv::Point2f(u,v); + //for NxN patch pixels around feature - for(double u_run = u - n; u_run <= u + n; u_run = u_run + 1) + int count = 0; + + // get feature in undistorted pixel space + cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), + cam.intrinsics, + cam.distortion_model, + 0); + // create vector of patch in pixel plane + std::vector und_pix_v; + for(double u_run = -n; u_run <= n; u_run++) + for(double v_run = -n; v_run <= n; v_run++) + und_pix_v.push_back(cv::Point2f(und_pix_p.x-u_run, und_pix_p.y-v_run)); + + //create undistorted pure points + std::vector und_v; + image_handler::undistortPoints(und_pix_v, + cam.intrinsics, + cam.distortion_model, + 0, + und_v); +//create distorted pixel points + std::vector vec = image_handler::distortPoints(und_v, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); + + anchor_center_pos = vec[4]; + + for(auto point : vec) { - for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1) + if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) { - printf("ADDING\n"); - // add irradiance information - anchorPatch.push_back((double)anchorImage.at((int)u_run,(int)v_run)); - - // project patch pixel to 3D space - auto intr = cam.intrinsics; - cv::Point2f currentPoint((u_run-intr[2])/intr[0], (v_run-intr[3])/intr[1]); - Eigen::Vector3d Npose = projectPixelToPosition(currentPoint, cam); - - //save position - anchorPatch_3d.push_back(Npose); + printf("no good\n"); + return false; } } - printf("set to true!!!\n"); + for(auto point : vec) + anchorPatch.push_back((double)anchorImage.at((int)point.x,(int)point.y)); + + // project patch pixel to 3D space + for(auto point : und_v) + anchorPatch_3d.push_back(projectPixelToPosition(point, cam)); + is_anchored = true; return true; } @@ -645,7 +687,7 @@ bool Feature::initializePosition( } //save inverse depth distance from camera - rho = solution(2); + anchor_rho = solution(2); // Convert the feature position to the world frame. position = T_c0_w.linear()*final_position + T_c0_w.translation(); diff --git a/include/msckf_vio/image_handler.h b/include/msckf_vio/image_handler.h index f35e3a7..6e8286f 100644 --- a/include/msckf_vio/image_handler.h +++ b/include/msckf_vio/image_handler.h @@ -30,6 +30,12 @@ std::vector distortPoints( const cv::Vec4d& intrinsics, const std::string& distortion_model, const cv::Vec4d& distortion_coeffs); + +cv::Point2f distortPoint( + const cv::Point2f& pt_in, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs); } } #endif diff --git a/src/image_handler.cpp b/src/image_handler.cpp index 3db65b5..5d868cc 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -81,5 +81,38 @@ std::vector distortPoints( return pts_out; } +cv::Point2f distortPoint( + const cv::Point2f& pt_in, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs) { + + std::vector pts_in; + pts_in.push_back(pt_in); + + const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2], + 0.0, intrinsics[1], intrinsics[3], + 0.0, 0.0, 1.0); + + std::vector pts_out; + if (distortion_model == "radtan") { + std::vector homogenous_pts; + cv::convertPointsToHomogeneous(pts_in, homogenous_pts); + cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K, + distortion_coeffs, pts_out); + } else if (distortion_model == "equidistant") { + cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs); + } else { + ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", + distortion_model.c_str()); + std::vector homogenous_pts; + cv::convertPointsToHomogeneous(pts_in, homogenous_pts); + cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K, + distortion_coeffs, pts_out); + } + + return pts_out[0]; +} + } // namespace image_handler } // namespace msckf_vio \ No newline at end of file diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 0d876d3..28adc60 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -929,15 +929,6 @@ void MsckfVio::PhotometricMeasurementJacobian( std::vector out_v; out_v.push_back(out_p); - /* - //prints the feature projection in pixel space - std::vector my_p = image_handler::distortPoints( out_v, - cam0.intrinsics, - cam0.distortion_model, - cam0.distortion_coeffs); - printf("projection: %f, %f\n", my_p[0].x, my_p[0].y); - */ - // Compute the Jacobians. Matrix dz_dpc0 = Matrix::Zero(); dz_dpc0(0, 0) = 1 / p_c0(2); @@ -1025,7 +1016,7 @@ void MsckfVio::PhotometricFeatureJacobian( VectorXd r_j = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; - + printf("_____FEATURE:_____\n"); for (const auto& cam_id : valid_cam_state_ids) { Matrix H_xi = Matrix::Zero(); From 6f16f1b566070f211bf7b0a305c2f95cccfc7712 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 19 Apr 2019 13:11:19 +0200 Subject: [PATCH 16/86] image reprojection visualization in images --- include/msckf_vio/cam_state.h | 20 +++++---- include/msckf_vio/feature.hpp | 54 ++++++++++++++--------- include/msckf_vio/msckf_vio.h | 3 ++ src/msckf_vio.cpp | 80 +++++++++++++++++++++-------------- 4 files changed, 99 insertions(+), 58 deletions(-) diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index a58da92..723f252 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -16,6 +16,15 @@ namespace msckf_vio { +struct Frame{ + cv::Mat image; + double exposureTime_ms; +}; + +typedef std::map, + Eigen::aligned_allocator< + std::pair > > movingWindow; + struct IlluminationParameter{ double frame_bias; double frame_gain; @@ -28,12 +37,11 @@ struct CameraCalibration{ cv::Vec2i resolution; cv::Vec4d intrinsics; cv::Vec4d distortion_coeffs; + movingWindow moving_window; + cv::Mat featureVisu; }; -struct Frame{ - cv::Mat image; - double exposureTime_ms; -}; + /* * @brief CAMState Stored camera state in order to @@ -85,10 +93,6 @@ struct CAMState { typedef std::map, Eigen::aligned_allocator< std::pair > > CamStateServer; - -typedef std::map, - Eigen::aligned_allocator< - std::pair > > movingWindow; } // namespace msckf_vio #endif // MSCKF_VIO_CAM_STATE_H diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 4eca45c..7c8d803 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -126,7 +126,6 @@ struct Feature { */ bool initializeAnchor( - const movingWindow& cam0_moving_window, const CameraCalibration& cam); @@ -165,15 +164,13 @@ struct Feature { bool estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - const CameraCalibration& cam0, - const movingWindow& cam0_moving_window, + CameraCalibration& cam0, std::vector& anchorPatch_estimate) const; bool FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - const CameraCalibration& cam0, - const movingWindow& cam0_moving_window, + CameraCalibration& cam0, std::vector& anchorPatch_measurement) const; /* @@ -368,8 +365,7 @@ bool Feature::checkMotion( bool Feature::estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - const CameraCalibration& cam0, - const movingWindow& cam0_moving_window, + CameraCalibration& cam0, std::vector& anchorPatch_estimate) const { // get irradiance of patch in anchor frame @@ -377,11 +373,11 @@ bool Feature::estimate_FrameIrradiance( // muliply by a and add b of this frame auto anchor = observations.begin(); - if(cam0_moving_window.find(anchor->first) == cam0_moving_window.end()) + if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end()) return false; - double anchorExposureTime_ms = cam0_moving_window.find(anchor->first)->second.exposureTime_ms; - double frameExposureTime_ms = cam0_moving_window.find(cam_state_id)->second.exposureTime_ms; + double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms; + double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms; double a_A = anchorExposureTime_ms; @@ -401,12 +397,15 @@ bool Feature::estimate_FrameIrradiance( bool Feature::FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - const CameraCalibration& cam0, - const movingWindow& cam0_moving_window, + CameraCalibration& cam0, std::vector& anchorPatch_measurement) const { // project every point in anchorPatch_3d. // int count = 0; + cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; + cv::Mat dottedFrame(current_image.size(), CV_8UC3); + cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB); + for (auto point : anchorPatch_3d) { // testing @@ -414,8 +413,14 @@ bool Feature::FrameIrradiance( //if(count == 4) //printf("\n\ncenter:\n"); - cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); - float irradiance = PixelIrradiance(p_in_c0, cam0_moving_window.find(cam_state_id)->second.image); + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); + + //visualization of feature + cv::Point xs(p_in_c0.x, p_in_c0.y); + cv::Point ys(p_in_c0.x, p_in_c0.y); + cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); + + float irradiance = PixelIrradiance(p_in_c0, cam0.moving_window.find(cam_state_id)->second.image); anchorPatch_measurement.push_back(irradiance); // testing @@ -424,6 +429,15 @@ bool Feature::FrameIrradiance( //printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y); } + + //visu + //cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2)); + if(cam0.featureVisu.empty()) + cam0.featureVisu = dottedFrame.clone(); + else + cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); + + } float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const @@ -476,10 +490,8 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); } - //@test center projection must always be initial feature projection bool Feature::initializeAnchor( - const movingWindow& cam0_moving_window, const CameraCalibration& cam) { @@ -489,10 +501,11 @@ bool Feature::initializeAnchor( int n = (int)(N-1)/2; auto anchor = observations.begin(); - if(cam0_moving_window.find(anchor->first) == cam0_moving_window.end()) + if(cam.moving_window.find(anchor->first) == cam.moving_window.end()) return false; - cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second.image; + cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image; + auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2]; auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3]; @@ -513,6 +526,8 @@ bool Feature::initializeAnchor( for(double v_run = -n; v_run <= n; v_run++) und_pix_v.push_back(cv::Point2f(und_pix_p.x-u_run, und_pix_p.y-v_run)); + + //create undistorted pure points std::vector und_v; image_handler::undistortPoints(und_pix_v, @@ -526,8 +541,9 @@ bool Feature::initializeAnchor( cam.distortion_model, cam.distortion_coeffs); - anchor_center_pos = vec[4]; + // save anchor position for later visualisaztion + anchor_center_pos = vec[4]; for(auto point : vec) { if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 07da6bd..ac44d67 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -225,6 +225,9 @@ class MsckfVio { CameraCalibration cam1; + + ros::Time image_save_time; + // Indicate if the gravity vector is set. bool is_gravity_set; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 28adc60..49b4d08 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -220,6 +220,12 @@ bool MsckfVio::loadParameters() { cout << T_imu_cam0.linear() << endl; cout << T_imu_cam0.translation().transpose() << endl; + cout << "OpenCV version : " << CV_VERSION << endl; + cout << "Major version : " << CV_MAJOR_VERSION << endl; + cout << "Minor version : " << CV_MINOR_VERSION << endl; + cout << "Subminor version : " << CV_SUBMINOR_VERSION << endl; + + ROS_INFO("max camera state #: %d", max_cam_state_size); ROS_INFO("==========================================="); return true; @@ -392,37 +398,37 @@ void MsckfVio::imageCallback( } void MsckfVio::manageMovingWindow( - const sensor_msgs::ImageConstPtr& cam0_img, - const sensor_msgs::ImageConstPtr& cam1_img, - const CameraMeasurementConstPtr& feature_msg) { + const sensor_msgs::ImageConstPtr& cam0_img, + const sensor_msgs::ImageConstPtr& cam1_img, + const CameraMeasurementConstPtr& feature_msg) { - //save exposure Time into moving window - cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000000; - cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000000; - if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms < 1) - cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 1; - if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms < 1) - cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 1; - if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 100) - cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 100; - if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 100) - cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 100; + //save exposure Time into moving window + cam0.moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000000; + cam1.moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000000; + if(cam0.moving_window[state_server.imu_state.id].exposureTime_ms < 1) + cam0.moving_window[state_server.imu_state.id].exposureTime_ms = 1; + if(cam1.moving_window[state_server.imu_state.id].exposureTime_ms < 1) + cam1.moving_window[state_server.imu_state.id].exposureTime_ms = 1; + if(cam0.moving_window[state_server.imu_state.id].exposureTime_ms > 100) + cam0.moving_window[state_server.imu_state.id].exposureTime_ms = 100; + if(cam1.moving_window[state_server.imu_state.id].exposureTime_ms > 100) + cam1.moving_window[state_server.imu_state.id].exposureTime_ms = 100; - // Get the current image. - cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img, - sensor_msgs::image_encodings::MONO8); - cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img, - sensor_msgs::image_encodings::MONO8); + // Get the current image. + cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img, + sensor_msgs::image_encodings::MONO8); + cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img, + sensor_msgs::image_encodings::MONO8); - // save image information into moving window - cam0_moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone(); - cam1_moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); + // save image information into moving window + cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone(); + cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) - while(cam0_moving_window.size() > 100) + while(cam0.moving_window.size() > 100) { - cam1_moving_window.erase(cam1_moving_window.begin()); - cam0_moving_window.erase(cam0_moving_window.begin()); + cam1.moving_window.erase(cam1.moving_window.begin()); + cam0.moving_window.erase(cam0.moving_window.begin()); } } @@ -916,7 +922,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //photometric observation std::vector photo_z; - feature.FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z); + feature.FrameIrradiance(cam_state, cam_state_id, cam0, photo_z); // Convert the feature position from the world frame to // the cam0 and cam1 frame. @@ -975,7 +981,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //estimate photometric measurement std::vector estimate_photo_z; - feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z); + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_photo_z); std::vector photo_r; //calculate photom. residual @@ -1017,6 +1023,8 @@ void MsckfVio::PhotometricFeatureJacobian( int stack_cntr = 0; printf("_____FEATURE:_____\n"); + + cam0.featureVisu.release(); for (const auto& cam_id : valid_cam_state_ids) { Matrix H_xi = Matrix::Zero(); @@ -1034,7 +1042,17 @@ void MsckfVio::PhotometricFeatureJacobian( r_j.segment<4>(stack_cntr) = r_i; stack_cntr += 4; } + if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000) + imshow("feature", cam0.featureVisu); + cvWaitKey(1); + if((ros::Time::now() - image_save_time).toSec() > 1) + { + std::stringstream ss; + ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; + imwrite(ss.str(), cam0.featureVisu); + image_save_time = ros::Time::now(); + } // Project the residual and Jacobians onto the nullspace // of H_fj. JacobiSVD svd_helper(H_fj, ComputeFullU | ComputeThinV); @@ -1336,7 +1354,7 @@ void MsckfVio::removeLostFeatures() { } if(!feature.is_anchored) { - if(!feature.initializeAnchor(cam0_moving_window, cam0)) + if(!feature.initializeAnchor(cam0)) { invalid_feature_ids.push_back(feature.id); continue; @@ -1491,7 +1509,7 @@ void MsckfVio::pruneCamStateBuffer() { } if(!feature.is_anchored) { - if(!feature.initializeAnchor(cam0_moving_window, cam0)) + if(!feature.initializeAnchor(cam0)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); @@ -1573,8 +1591,8 @@ void MsckfVio::pruneCamStateBuffer() { // Remove this camera state in the state vector. state_server.cam_states.erase(cam_id); - cam0_moving_window.erase(cam_id); - cam1_moving_window.erase(cam_id); + cam0.moving_window.erase(cam_id); + cam1.moving_window.erase(cam_id); } return; From 1949e4c43d4a8824b1592ba8cf40b800385aa325 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 19 Apr 2019 13:30:30 +0200 Subject: [PATCH 17/86] removed visu as result works so to not clutter the output --- include/msckf_vio/feature.hpp | 31 ++++++++++++++----------------- src/msckf_vio.cpp | 18 +++++++++++++----- 2 files changed, 27 insertions(+), 22 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 7c8d803..7702ddc 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -384,8 +384,10 @@ bool Feature::estimate_FrameIrradiance( double b_A = 0; double a_l =frameExposureTime_ms; double b_l = 0; - printf("frames: %lld, %lld\n", anchor->first, cam_state_id); - printf("exposure: %f, %f\n", a_A, a_l); + + //printf("frames: %lld, %lld\n", anchor->first, cam_state_id); + //printf("exposure: %f, %f\n", a_A, a_l); + for (double anchorPixel : anchorPatch) { float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l; @@ -400,26 +402,24 @@ bool Feature::FrameIrradiance( CameraCalibration& cam0, std::vector& anchorPatch_measurement) const { - // project every point in anchorPatch_3d. - // int count = 0; - cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; + + // visu + /*cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; cv::Mat dottedFrame(current_image.size(), CV_8UC3); cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB); + */ + // project every point in anchorPatch_3d. for (auto point : anchorPatch_3d) { - // testing - //if(cam_state_id == observations.begin()->first) - //if(count == 4) - //printf("\n\ncenter:\n"); cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); - //visualization of feature - cv::Point xs(p_in_c0.x, p_in_c0.y); + // visu + /*cv::Point xs(p_in_c0.x, p_in_c0.y); cv::Point ys(p_in_c0.x, p_in_c0.y); cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); - + */ float irradiance = PixelIrradiance(p_in_c0, cam0.moving_window.find(cam_state_id)->second.image); anchorPatch_measurement.push_back(irradiance); @@ -432,11 +432,11 @@ bool Feature::FrameIrradiance( //visu //cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2)); - if(cam0.featureVisu.empty()) + /*if(cam0.featureVisu.empty()) cam0.featureVisu = dottedFrame.clone(); else cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); - + */ } @@ -547,10 +547,7 @@ bool Feature::initializeAnchor( for(auto point : vec) { if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) - { - printf("no good\n"); return false; - } } for(auto point : vec) anchorPatch.push_back((double)anchorImage.at((int)point.x,(int)point.y)); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 49b4d08..70e167c 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -977,7 +977,8 @@ void MsckfVio::PhotometricMeasurementJacobian( r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); - printf("-----\n"); + // visu + //printf("-----\n"); //estimate photometric measurement std::vector estimate_photo_z; @@ -988,9 +989,9 @@ void MsckfVio::PhotometricMeasurementJacobian( for(int i = 0; i < photo_z.size(); i++) photo_r.push_back(photo_z[i] - estimate_photo_z[i]); - - for(int i = 0; i < photo_z.size(); i++) - printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); + // visu + //for(int i = 0; i < photo_z.size(); i++) + // printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); photo_z.clear(); return; @@ -1022,9 +1023,12 @@ void MsckfVio::PhotometricFeatureJacobian( VectorXd r_j = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; - printf("_____FEATURE:_____\n"); + // visu + /* + printf("_____FEATURE:_____\n"); cam0.featureVisu.release(); + */ for (const auto& cam_id : valid_cam_state_ids) { Matrix H_xi = Matrix::Zero(); @@ -1042,6 +1046,8 @@ void MsckfVio::PhotometricFeatureJacobian( r_j.segment<4>(stack_cntr) = r_i; stack_cntr += 4; } + // visu + /* if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000) imshow("feature", cam0.featureVisu); cvWaitKey(1); @@ -1053,6 +1059,8 @@ void MsckfVio::PhotometricFeatureJacobian( imwrite(ss.str(), cam0.featureVisu); image_save_time = ros::Time::now(); } + */ + // Project the residual and Jacobians onto the nullspace // of H_fj. JacobiSVD svd_helper(H_fj, ComputeFullU | ComputeThinV); From 8defb20c8e08bd0696de1c63f78ddd7d5cf49cf6 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 19 Apr 2019 13:32:16 +0200 Subject: [PATCH 18/86] commented visu parts cleanly --- include/msckf_vio/feature.hpp | 6 +++--- src/msckf_vio.cpp | 16 ++++++++-------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 7702ddc..662caed 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -403,7 +403,7 @@ bool Feature::FrameIrradiance( std::vector& anchorPatch_measurement) const { - // visu + // visu - feature /*cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; cv::Mat dottedFrame(current_image.size(), CV_8UC3); cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB); @@ -415,7 +415,7 @@ bool Feature::FrameIrradiance( cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); - // visu + // visu - feature /*cv::Point xs(p_in_c0.x, p_in_c0.y); cv::Point ys(p_in_c0.x, p_in_c0.y); cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); @@ -430,7 +430,7 @@ bool Feature::FrameIrradiance( } - //visu + // visu - feature //cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2)); /*if(cam0.featureVisu.empty()) cam0.featureVisu = dottedFrame.clone(); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 70e167c..6e49438 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -977,7 +977,7 @@ void MsckfVio::PhotometricMeasurementJacobian( r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); - // visu + // visu -residual //printf("-----\n"); //estimate photometric measurement @@ -989,7 +989,7 @@ void MsckfVio::PhotometricMeasurementJacobian( for(int i = 0; i < photo_z.size(); i++) photo_r.push_back(photo_z[i] - estimate_photo_z[i]); - // visu + // visu- residual //for(int i = 0; i < photo_z.size(); i++) // printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); @@ -1024,11 +1024,11 @@ void MsckfVio::PhotometricFeatureJacobian( int stack_cntr = 0; - // visu - /* - printf("_____FEATURE:_____\n"); - cam0.featureVisu.release(); - */ + // visu - residual + //printf("_____FEATURE:_____\n"); + // visu - feature + //cam0.featureVisu.release(); + for (const auto& cam_id : valid_cam_state_ids) { Matrix H_xi = Matrix::Zero(); @@ -1046,7 +1046,7 @@ void MsckfVio::PhotometricFeatureJacobian( r_j.segment<4>(stack_cntr) = r_i; stack_cntr += 4; } - // visu + // visu - feature /* if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000) imshow("feature", cam0.featureVisu); From 5958adb57c37ee5f6081f9e9a22596f4d7e237ee Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 23 Apr 2019 19:16:46 +0200 Subject: [PATCH 19/86] added jakobi x calculation, y calculation (of photometric update) still missing --- include/msckf_vio/feature.hpp | 42 ++++----- include/msckf_vio/msckf_vio.h | 2 +- src/msckf_vio.cpp | 156 ++++++++++++++++++++-------------- 3 files changed, 109 insertions(+), 91 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 662caed..d727646 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -167,11 +167,10 @@ struct Feature { CameraCalibration& cam0, std::vector& anchorPatch_estimate) const; - bool FrameIrradiance( + bool VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, - std::vector& anchorPatch_measurement) const; + CameraCalibration& cam0) const; /* * @brief projectPixelToPosition uses the calcualted pixels @@ -396,48 +395,41 @@ bool Feature::estimate_FrameIrradiance( } -bool Feature::FrameIrradiance( + +bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, - std::vector& anchorPatch_measurement) const + CameraCalibration& cam0) const { // visu - feature - /*cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; + cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; cv::Mat dottedFrame(current_image.size(), CV_8UC3); cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB); - */ // project every point in anchorPatch_3d. - for (auto point : anchorPatch_3d) + auto frame = cam0.moving_window.find(cam_state_id)->second.image; + + for(auto point : anchorPatch_3d) { - cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); // visu - feature - /*cv::Point xs(p_in_c0.x, p_in_c0.y); + cv::Point xs(p_in_c0.x, p_in_c0.y); cv::Point ys(p_in_c0.x, p_in_c0.y); cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); - */ - float irradiance = PixelIrradiance(p_in_c0, cam0.moving_window.find(cam_state_id)->second.image); - anchorPatch_measurement.push_back(irradiance); - - // testing - //if(cam_state_id == observations.begin()->first) - //if(count++ == 4) - //printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y); - - } + } + // testing + //if(cam_state_id == observations.begin()->first) + //if(count++ == 4) + //printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y); // visu - feature - //cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2)); - /*if(cam0.featureVisu.empty()) + cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2)); + if(cam0.featureVisu.empty()) cam0.featureVisu = dottedFrame.clone(); else cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); - */ - } float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index ac44d67..fcc4290 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -207,7 +207,7 @@ class MsckfVio { StateServer state_server; // Maximum number of camera states int max_cam_state_size; - + // Features used MapServer map_server; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 6e49438..d7eaf0b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -190,7 +190,7 @@ bool MsckfVio::loadParameters() { // Maximum number of camera states to be stored nh.param("max_cam_state_size", max_cam_state_size, 30); - + //cam_state_size = max_cam_state_size; ROS_INFO("==========================================="); ROS_INFO("fixed frame id: %s", fixed_frame_id.c_str()); ROS_INFO("child frame id: %s", child_frame_id.c_str()); @@ -684,25 +684,6 @@ void MsckfVio::processModel(const double& time, // Propogate the state using 4th order Runge-Kutta predictNewState(dtime, gyro, acc); - // Modify the transition matrix - Matrix3d R_kk_1 = quaternionToRotation(imu_state.orientation_null); - Phi.block<3, 3>(0, 0) = - quaternionToRotation(imu_state.orientation) * R_kk_1.transpose(); - - Vector3d u = R_kk_1 * IMUState::gravity; - RowVector3d s = (u.transpose()*u).inverse() * u.transpose(); - - Matrix3d A1 = Phi.block<3, 3>(6, 0); - Vector3d w1 = skewSymmetric( - imu_state.velocity_null-imu_state.velocity) * IMUState::gravity; - Phi.block<3, 3>(6, 0) = A1 - (A1*u-w1)*s; - - Matrix3d A2 = Phi.block<3, 3>(12, 0); - Vector3d w2 = skewSymmetric( - dtime*imu_state.velocity_null+imu_state.position_null- - imu_state.position) * IMUState::gravity; - Phi.block<3, 3>(12, 0) = A2 - (A2*u-w2)*s; - // Propogate the state covariance matrix. Matrix Q = Phi*G*state_server.continuous_noise_cov* G.transpose()*Phi.transpose()*dtime; @@ -908,6 +889,9 @@ void MsckfVio::PhotometricMeasurementJacobian( Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Vector3d& t_c0_w = cam_state.position; + //temp N + const int N = 3; + // Cam1 pose. Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0; @@ -922,57 +906,99 @@ void MsckfVio::PhotometricMeasurementJacobian( //photometric observation std::vector photo_z; - feature.FrameIrradiance(cam_state, cam_state_id, cam0, photo_z); + + // individual Jacobians + Matrix dI_dhj = Matrix::Zero(); + Matrix dh_dCpij = Matrix::Zero(); + Matrix dh_dGpij = Matrix::Zero(); + Matrix dh_dXplj = Matrix::Zero(); + Matrix dGpi_drhoj = Matrix::Zero(); + Matrix dGpi_XpAj = Matrix::Zero(); + + // one line of the NxN Jacobians + Eigen::Matrix H_rhoj; + Eigen::Matrix H_plj; + Eigen::Matrix H_pAj; + + // combined Jacobians + Eigen::MatrixXd H_rho(N*N, 3); + Eigen::MatrixXd H_pl(N*N, 6); + Eigen::MatrixXd H_pA(N*N, 6); + + auto frame = cam0.moving_window.find(cam_state_id)->second.image; + + int count = 0; + float dx, dy; + for (auto point : feature.anchorPatch_3d) + { + + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + + //add observation + photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); + + //add jacobian + + // frame derivative calculated convoluting with kernel [-1, 0, 1] + dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame); + dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame); + dI_dhj(0, 0) = dx; + dI_dhj(1, 0) = dy; + + //dh / d{}^Cp_{ij} + dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix::Identity(); + dh_dCpij(0, 2) = -(point(0))/(point(2)*point(2)); + dh_dCpij(1, 2) = -(point(1))/(point(2)*point(2)); + dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose(); + + //dh / d X_{pl} + dh_dXplj.block<2, 3>(3, 0) = dh_dCpij * skewSymmetric(point); + dh_dXplj.block<2, 3>(3, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose(); + + //d{}^Gp_P{ij} / \rho_i + double rho = feature.anchor_rho; + dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(p_in_c0.x/(rho*rho), p_in_c0.y/(rho*rho), 1/(rho*rho)); + + dGpi_XpAj.block<3, 3>(3, 0) = skewSymmetric(Eigen::Vector3d(p_in_c0.x/(rho), p_in_c0.y/(rho), 1/(rho))); + dGpi_XpAj.block<3, 3>(3, 3) = Matrix::Identity(); + + // Intermediate Jakobians + H_rhoj = dI_dhj * dh_dGpij * dGpi_drhoj; // 1 x 3 + H_plj = dI_dhj * dh_dXplj; // 1 x 6 + H_pAj = dI_dhj * dh_dGpij * dGpi_XpAj; // 1 x 6 + + H_rho.block<1, 1>(count, 0) = H_rhoj; + H_pl.block<1, 6>(count, 0) = H_plj; + H_pA.block<1, 6>(count, 0) = H_pAj; + + count++; + } + + + + //Final Jakobians + MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); + MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+2); + auto cam_state_iter = state_server.cam_states.find(feature.observations.begin()->first); + int cam_state_cntr = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_state_id)); + + // set anchor Jakobi + H_xl.block(0,21+cam_state_cntr*7) = -H_pA; + //H_yl + + cam_state_iter = state_server.cam_states.find(cam_state_id); + cam_state_cntr = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_state_id)); + + // set frame Jakobi + H_xl.block(N*N, 6, 0, 21+cam_state_cntr*7) = -H_pl; + + H_xl.block(N*N, 1, 0, 21+cam_state_cntr*7) = Eigen::ArrayXd::Ones(N*N); // Convert the feature position from the world frame to // the cam0 and cam1 frame. Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w); Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w); - - //compute resulting esimtated position in image - cv::Point2f out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); - std::vector out_v; - out_v.push_back(out_p); - - // Compute the Jacobians. - Matrix dz_dpc0 = Matrix::Zero(); - dz_dpc0(0, 0) = 1 / p_c0(2); - dz_dpc0(1, 1) = 1 / p_c0(2); - dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2)); - dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2)); - - Matrix dz_dpc1 = Matrix::Zero(); - dz_dpc1(2, 0) = 1 / p_c1(2); - dz_dpc1(3, 1) = 1 / p_c1(2); - dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2)); - dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2)); - - Matrix dpc0_dxc = Matrix::Zero(); - dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); - dpc0_dxc.rightCols(3) = -R_w_c0; - - Matrix dpc1_dxc = Matrix::Zero(); - dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0); - dpc1_dxc.rightCols(3) = -R_w_c1; - - Matrix3d dpc0_dpg = R_w_c0; - Matrix3d dpc1_dpg = R_w_c1; - - H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc; - H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg; - - // Modifty the measurement Jacobian to ensure - // observability constrain. - Matrix A = H_x; - Matrix u = Matrix::Zero(); - u.block<3, 1>(0, 0) = quaternionToRotation( - cam_state.orientation_null) * IMUState::gravity; - u.block<3, 1>(3, 0) = skewSymmetric( - p_w-cam_state.position_null) * IMUState::gravity; - H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose(); - H_f = -H_x.block<4, 3>(0, 3); - // Compute the residual. r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); From 1ffc4fb37f789ebb056686fbc804b813a715a6df Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 24 Apr 2019 15:30:25 +0200 Subject: [PATCH 20/86] Jakobi Calculation done --- include/msckf_vio/feature.hpp | 23 +++++-- src/msckf_vio.cpp | 123 ++++++++++++++++++++++++---------- 2 files changed, 105 insertions(+), 41 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index d727646..2cf42fd 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -165,7 +165,8 @@ struct Feature { const CAMState& cam_state, const StateIDType& cam_state_id, CameraCalibration& cam0, - std::vector& anchorPatch_estimate) const; + std::vector& anchorPatch_estimate, + IlluminationParameter& estimatedIllumination) const; bool VisualizePatch( const CAMState& cam_state, @@ -202,6 +203,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, // NxN Patch of Anchor Image std::vector anchorPatch; + std::vector anchorPatch_ideal; // Position of NxN Patch in 3D space std::vector anchorPatch_3d; @@ -365,7 +367,8 @@ bool Feature::estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, CameraCalibration& cam0, - std::vector& anchorPatch_estimate) const + std::vector& anchorPatch_estimate, + IlluminationParameter& estimated_illumination) const { // get irradiance of patch in anchor frame // subtract estimated b and divide by a of anchor frame @@ -381,15 +384,19 @@ bool Feature::estimate_FrameIrradiance( double a_A = anchorExposureTime_ms; double b_A = 0; - double a_l =frameExposureTime_ms; + double a_l = frameExposureTime_ms; double b_l = 0; - + + estimated_illumination.frame_gain = a_l; + estimated_illumination.frame_bias = b_l; + estimated_illumination.feature_gain = 1; + estimated_illumination.feature_bias = 0; //printf("frames: %lld, %lld\n", anchor->first, cam_state_id); //printf("exposure: %f, %f\n", a_A, a_l); for (double anchorPixel : anchorPatch) { - float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l; + float irradiance = (anchorPixel - b_A) / a_A ; anchorPatch_estimate.push_back(irradiance); } @@ -536,6 +543,8 @@ bool Feature::initializeAnchor( // save anchor position for later visualisaztion anchor_center_pos = vec[4]; + + // save true pixel Patch position for(auto point : vec) { if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) @@ -546,8 +555,10 @@ bool Feature::initializeAnchor( // project patch pixel to 3D space for(auto point : und_v) + { + anchorPatch_ideal.push_back(point); anchorPatch_3d.push_back(projectPixelToPosition(point, cam)); - + } is_anchored = true; return true; } diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index d7eaf0b..de5e217 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -901,11 +901,8 @@ void MsckfVio::PhotometricMeasurementJacobian( // And its observation with the stereo cameras. const Vector3d& p_w = feature.position; - //observation - const Vector4d& z = feature.observations.find(cam_state_id)->second; - //photometric observation - std::vector photo_z; + std::vector photo_z; // individual Jacobians Matrix dI_dhj = Matrix::Zero(); @@ -921,17 +918,17 @@ void MsckfVio::PhotometricMeasurementJacobian( Eigen::Matrix H_pAj; // combined Jacobians - Eigen::MatrixXd H_rho(N*N, 3); + Eigen::MatrixXd H_rho(N*N, 1); Eigen::MatrixXd H_pl(N*N, 6); Eigen::MatrixXd H_pA(N*N, 6); auto frame = cam0.moving_window.find(cam_state_id)->second.image; int count = 0; - float dx, dy; + double dx, dy; for (auto point : feature.anchorPatch_3d) { - + Eigen::Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w); cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); //add observation @@ -947,8 +944,8 @@ void MsckfVio::PhotometricMeasurementJacobian( //dh / d{}^Cp_{ij} dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix::Identity(); - dh_dCpij(0, 2) = -(point(0))/(point(2)*point(2)); - dh_dCpij(1, 2) = -(point(1))/(point(2)*point(2)); + dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); + dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose(); //dh / d X_{pl} @@ -957,9 +954,9 @@ void MsckfVio::PhotometricMeasurementJacobian( //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; - dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(p_in_c0.x/(rho*rho), p_in_c0.y/(rho*rho), 1/(rho*rho)); + dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); - dGpi_XpAj.block<3, 3>(3, 0) = skewSymmetric(Eigen::Vector3d(p_in_c0.x/(rho), p_in_c0.y/(rho), 1/(rho))); + dGpi_XpAj.block<3, 3>(3, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho))); dGpi_XpAj.block<3, 3>(3, 3) = Matrix::Identity(); // Intermediate Jakobians @@ -974,25 +971,69 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } + // calculate residual + // visu -residual + //printf("-----\n"); + + //observation + const Vector4d& z = feature.observations.find(cam_state_id)->second; + + //estimate photometric measurement + std::vector estimate_irradiance; + std::vector estimate_photo_z; + IlluminationParameter estimated_illumination; + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); + for (auto& estimate_irradiance_j : estimate_irradiance) + estimate_photo_z.push_back(estimate_irradiance_j * + estimated_illumination.frame_gain * estimated_illumination.feature_gain + + estimated_illumination.frame_bias + estimated_illumination.feature_bias); + + std::vector photo_r; + //calculate photom. residual + for(int i = 0; i < photo_z.size(); i++) + photo_r.push_back(photo_z[i] - estimate_photo_z[i]); + + // visu- residual + //for(int i = 0; i < photo_z.size(); i++) + // printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); + //Final Jakobians + // cout << "------------------------" << endl; + // cout << "rho" << H_rho.rows() << "x" << H_rho.cols() << "\n" << H_rho << endl; + // cout << "l" << H_pl.rows() << "x" << H_pl.cols() << "\n" << H_pl << endl; + // cout << "A" << H_pA.rows() << "x" << H_pA.cols() << "\n" << H_pA << endl; + MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+2); - auto cam_state_iter = state_server.cam_states.find(feature.observations.begin()->first); - int cam_state_cntr = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_state_id)); + MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); // set anchor Jakobi - H_xl.block(0,21+cam_state_cntr*7) = -H_pA; - //H_yl - - cam_state_iter = state_server.cam_states.find(cam_state_id); - cam_state_cntr = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_state_id)); + // get position of anchor in cam states + auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); + int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); + H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; // set frame Jakobi - H_xl.block(N*N, 6, 0, 21+cam_state_cntr*7) = -H_pl; + //get position of current frame in cam states + auto cam_state_iter = state_server.cam_states.find(cam_state_id); + int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); + + // set jakobi of state + H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl; - H_xl.block(N*N, 1, 0, 21+cam_state_cntr*7) = Eigen::ArrayXd::Ones(N*N); + // set ones for irradiance bias + H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); + + // set irradiance error Block + H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N); + + // TODO make this calculation more fluent + for(int i = 0; i< N*N; i++) + H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; + H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho; + + // Original calculation // Convert the feature position from the world frame to // the cam0 and cam1 frame. @@ -1003,21 +1044,33 @@ void MsckfVio::PhotometricMeasurementJacobian( r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); - // visu -residual - //printf("-----\n"); - //estimate photometric measurement - std::vector estimate_photo_z; - feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_photo_z); - std::vector photo_r; - - //calculate photom. residual - for(int i = 0; i < photo_z.size(); i++) - photo_r.push_back(photo_z[i] - estimate_photo_z[i]); +// Compute the Jacobians. + Matrix dz_dpc0 = Matrix::Zero(); + dz_dpc0(0, 0) = 1 / p_c0(2); + dz_dpc0(1, 1) = 1 / p_c0(2); + dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2)); + dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2)); - // visu- residual - //for(int i = 0; i < photo_z.size(); i++) - // printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); + Matrix dz_dpc1 = Matrix::Zero(); + dz_dpc1(2, 0) = 1 / p_c1(2); + dz_dpc1(3, 1) = 1 / p_c1(2); + dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2)); + dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2)); + + Matrix dpc0_dxc = Matrix::Zero(); + dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); + dpc0_dxc.rightCols(3) = -R_w_c0; + + Matrix dpc1_dxc = Matrix::Zero(); + dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0); + dpc1_dxc.rightCols(3) = -R_w_c1; + + Matrix3d dpc0_dpg = R_w_c0; + Matrix3d dpc1_dpg = R_w_c1; + + H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc; + H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg; photo_z.clear(); return; @@ -1051,7 +1104,7 @@ void MsckfVio::PhotometricFeatureJacobian( // visu - residual - //printf("_____FEATURE:_____\n"); + printf("_____FEATURE:_____\n"); // visu - feature //cam0.featureVisu.release(); From 821d9d6f71ee719c0d5a23fd140a98c790ac616a Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 24 Apr 2019 19:36:38 +0200 Subject: [PATCH 21/86] added debug launch file, added state augmentation, added jakobi concat; resulting jakobis do not pass gating test --- include/msckf_vio/feature.hpp | 2 +- include/msckf_vio/msckf_vio.h | 11 +- launch/msckf_vio_debug_tum.launch | 66 +++++++++ launch/msckf_vio_tum.launch | 1 + src/msckf_vio.cpp | 223 +++++++++++++++++++----------- 5 files changed, 215 insertions(+), 88 deletions(-) create mode 100644 launch/msckf_vio_debug_tum.launch diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 2cf42fd..6e013ea 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -496,7 +496,7 @@ bool Feature::initializeAnchor( //initialize patch Size //TODO make N size a ros parameter - int N = 3; + int N = 13; int n = (int)(N-1)/2; auto anchor = observations.begin(); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index fcc4290..536c8ca 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -162,6 +162,7 @@ class MsckfVio { // Measurement update void stateAugmentation(const double& time); + void PhotometricStateAugmentation(const double& time); void addFeatureObservations(const CameraMeasurementConstPtr& msg); // This function is used to compute the measurement Jacobian // for a single feature observed at a single camera frame. @@ -180,9 +181,9 @@ class MsckfVio { void PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, - Eigen::Matrix& H_x, - Eigen::Matrix& H_f, - Eigen::Vector4d& r); + Eigen::MatrixXd& H_x, + Eigen::MatrixXd& H_y, + Eigen::VectorXd& r); void PhotometricFeatureJacobian( const FeatureIDType& feature_id, @@ -207,7 +208,7 @@ class MsckfVio { StateServer state_server; // Maximum number of camera states int max_cam_state_size; - + // Features used MapServer map_server; @@ -224,6 +225,8 @@ class MsckfVio { CameraCalibration cam0; CameraCalibration cam1; + // covariance data + double irradiance_frame_bias; ros::Time image_save_time; diff --git a/launch/msckf_vio_debug_tum.launch b/launch/msckf_vio_debug_tum.launch new file mode 100644 index 0000000..7597f79 --- /dev/null +++ b/launch/msckf_vio_debug_tum.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 247e824..18a5f4b 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -51,6 +51,7 @@ + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index de5e217..e7a696a 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -117,6 +117,10 @@ bool MsckfVio::loadParameters() { nh.param("initial_covariance/extrinsic_translation_cov", extrinsic_translation_cov, 1e-4); + // Get the initial irradiance covariance + nh.param("initial_covariance/irradiance_frame_bias", + irradiance_frame_bias, 0.1); + // get camera information (used for back projection) nh.param("cam0/distortion_model", cam0.distortion_model, string("radtan")); @@ -336,7 +340,7 @@ void MsckfVio::imageCallback( // Augment the state vector. start_time = ros::Time::now(); - stateAugmentation(feature_msg->header.stamp.toSec()); + PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); double state_augmentation_time = ( ros::Time::now()-start_time).toSec(); @@ -511,6 +515,10 @@ bool MsckfVio::resetCallback( nh.param("initial_covariance/extrinsic_translation_cov", extrinsic_translation_cov, 1e-4); + // Reset the irradiance covariance + nh.param("initial_covariance/irradiance_frame_bias", + irradiance_frame_bias, 0.1); + state_server.state_cov = MatrixXd::Zero(21, 21); for (int i = 3; i < 6; ++i) state_server.state_cov(i, i) = gyro_bias_cov; @@ -820,6 +828,8 @@ void MsckfVio::stateAugmentation(const double& time) { // Resize the state covariance matrix. size_t old_rows = state_server.state_cov.rows(); size_t old_cols = state_server.state_cov.cols(); + + // add 7 for camera state + irradiance bias eta = b_l state_server.state_cov.conservativeResize(old_rows+6, old_cols+6); // Rename some matrix blocks for convenience. @@ -839,10 +849,78 @@ void MsckfVio::stateAugmentation(const double& time) { MatrixXd state_cov_fixed = (state_server.state_cov + state_server.state_cov.transpose()) / 2.0; state_server.state_cov = state_cov_fixed; - return; } +void MsckfVio::PhotometricStateAugmentation(const double& time) { + + const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0; + const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu; + + // Add a new camera state to the state server. + Matrix3d R_w_i = quaternionToRotation( + state_server.imu_state.orientation); + Matrix3d R_w_c = R_i_c * R_w_i; + Vector3d t_c_w = state_server.imu_state.position + + R_w_i.transpose()*t_c_i; + + state_server.cam_states[state_server.imu_state.id] = + CAMState(state_server.imu_state.id); + CAMState& cam_state = state_server.cam_states[ + state_server.imu_state.id]; + + cam_state.time = time; + cam_state.orientation = rotationToQuaternion(R_w_c); + cam_state.position = t_c_w; + + cam_state.orientation_null = cam_state.orientation; + cam_state.position_null = cam_state.position; + + + // Update the covariance matrix of the state. + // To simplify computation, the matrix J below is the nontrivial block + // in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision + // -aided Inertial Navigation". + Matrix J = Matrix::Zero(); + J.block<3, 3>(0, 0) = R_i_c; + J.block<3, 3>(0, 15) = Matrix3d::Identity(); + J.block<3, 3>(3, 0) = skewSymmetric(R_w_i.transpose()*t_c_i); + //J.block<3, 3>(3, 0) = -R_w_i.transpose()*skewSymmetric(t_c_i); + J.block<3, 3>(3, 12) = Matrix3d::Identity(); + J.block<3, 3>(3, 18) = Matrix3d::Identity(); + + // Resize the state covariance matrix. + size_t old_rows = state_server.state_cov.rows(); + size_t old_cols = state_server.state_cov.cols(); + + // add 7 for camera state + irradiance bias eta = b_l + state_server.state_cov.conservativeResize(old_rows+7, old_cols+7); + + // Rename some matrix blocks for convenience. + const Matrix& P11 = + state_server.state_cov.block<21, 21>(0, 0); + const MatrixXd& P12 = + state_server.state_cov.block(0, 21, 21, old_cols-21); + + // Fill in the augmented state covariance. + state_server.state_cov.block(old_rows, 0, 6, old_cols) << J*P11, J*P12; + state_server.state_cov.block(0, old_cols, old_rows, 6) = + state_server.state_cov.block(old_rows, 0, 6, old_cols).transpose(); + state_server.state_cov.block<6, 6>(old_rows, old_cols) = + J * P11 * J.transpose(); + + // Add photometry P_eta and surrounding Zeros + state_server.state_cov.block<1, 12>(old_rows+6, 0) = Matrix::Zero(); + state_server.state_cov.block<12, 1>(0, old_cols+6) = Matrix::Zero(); + state_server.state_cov(old_rows+6, old_cols+6) = irradiance_frame_bias; + // Fix the covariance to be symmetric + MatrixXd state_cov_fixed = (state_server.state_cov + + state_server.state_cov.transpose()) / 2.0; + state_server.state_cov = state_cov_fixed; + return; +} + + void MsckfVio::addFeatureObservations( const CameraMeasurementConstPtr& msg) { @@ -879,7 +957,7 @@ void MsckfVio::addFeatureObservations( void MsckfVio::PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, - Matrix& H_x, Matrix& H_f, Vector4d& r) { + MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) { // Prepare all the required data. const CAMState& cam_state = state_server.cam_states[cam_state_id]; @@ -890,7 +968,7 @@ void MsckfVio::PhotometricMeasurementJacobian( const Vector3d& t_c0_w = cam_state.position; //temp N - const int N = 3; + const int N = 13; // Cam1 pose. Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); @@ -940,7 +1018,7 @@ void MsckfVio::PhotometricMeasurementJacobian( dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame); dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame); dI_dhj(0, 0) = dx; - dI_dhj(1, 0) = dy; + dI_dhj(0, 1) = dy; //dh / d{}^Cp_{ij} dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix::Identity(); @@ -949,15 +1027,15 @@ void MsckfVio::PhotometricMeasurementJacobian( dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose(); //dh / d X_{pl} - dh_dXplj.block<2, 3>(3, 0) = dh_dCpij * skewSymmetric(point); - dh_dXplj.block<2, 3>(3, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose(); + dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point); + dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose(); //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); - dGpi_XpAj.block<3, 3>(3, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho))); - dGpi_XpAj.block<3, 3>(3, 3) = Matrix::Identity(); + dGpi_XpAj.block<3, 3>(0, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho))); + dGpi_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); // Intermediate Jakobians H_rhoj = dI_dhj * dh_dGpij * dGpi_drhoj; // 1 x 3 @@ -973,9 +1051,6 @@ void MsckfVio::PhotometricMeasurementJacobian( // calculate residual - // visu -residual - //printf("-----\n"); - //observation const Vector4d& z = feature.observations.find(cam_state_id)->second; @@ -985,7 +1060,7 @@ void MsckfVio::PhotometricMeasurementJacobian( IlluminationParameter estimated_illumination; feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); for (auto& estimate_irradiance_j : estimate_irradiance) - estimate_photo_z.push_back(estimate_irradiance_j * + estimate_photo_z.push_back (estimate_irradiance_j * estimated_illumination.frame_gain * estimated_illumination.feature_gain + estimated_illumination.frame_bias + estimated_illumination.feature_bias); @@ -995,16 +1070,6 @@ void MsckfVio::PhotometricMeasurementJacobian( for(int i = 0; i < photo_z.size(); i++) photo_r.push_back(photo_z[i] - estimate_photo_z[i]); - // visu- residual - //for(int i = 0; i < photo_z.size(); i++) - // printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); - - //Final Jakobians - // cout << "------------------------" << endl; - // cout << "rho" << H_rho.rows() << "x" << H_rho.cols() << "\n" << H_rho << endl; - // cout << "l" << H_pl.rows() << "x" << H_pl.cols() << "\n" << H_pl << endl; - // cout << "A" << H_pA.rows() << "x" << H_pA.cols() << "\n" << H_pA << endl; - MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); @@ -1033,46 +1098,14 @@ void MsckfVio::PhotometricMeasurementJacobian( H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho; - // Original calculation + H_x = H_xl; + H_y = H_yl; - // Convert the feature position from the world frame to - // the cam0 and cam1 frame. - Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w); - Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w); + //TODO make this more fluent as well + count = 0; + for(auto data : photo_r) + r[count++] = data; - // Compute the residual. - r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), - p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); - - -// Compute the Jacobians. - Matrix dz_dpc0 = Matrix::Zero(); - dz_dpc0(0, 0) = 1 / p_c0(2); - dz_dpc0(1, 1) = 1 / p_c0(2); - dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2)); - dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2)); - - Matrix dz_dpc1 = Matrix::Zero(); - dz_dpc1(2, 0) = 1 / p_c1(2); - dz_dpc1(3, 1) = 1 / p_c1(2); - dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2)); - dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2)); - - Matrix dpc0_dxc = Matrix::Zero(); - dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); - dpc0_dxc.rightCols(3) = -R_w_c0; - - Matrix dpc1_dxc = Matrix::Zero(); - dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0); - dpc1_dxc.rightCols(3) = -R_w_c1; - - Matrix3d dpc0_dpg = R_w_c0; - Matrix3d dpc1_dpg = R_w_c1; - - H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc; - H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg; - - photo_z.clear(); return; } @@ -1083,6 +1116,7 @@ void MsckfVio::PhotometricFeatureJacobian( const auto& feature = map_server[feature_id]; + int N = 13; // Check how many camera states in the provided camera // id camera has actually seen this feature. vector valid_cam_state_ids(0); @@ -1094,36 +1128,38 @@ void MsckfVio::PhotometricFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = 4 * valid_cam_state_ids.size(); + jacobian_row_size = N * N * valid_cam_state_ids.size(); - MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size, - 21+state_server.cam_states.size()*6); - MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3); - VectorXd r_j = VectorXd::Zero(jacobian_row_size); + MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, + 21+state_server.cam_states.size()*7); + MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1); + VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; - // visu - residual - printf("_____FEATURE:_____\n"); + //printf("_____FEATURE:_____\n"); // visu - feature //cam0.featureVisu.release(); for (const auto& cam_id : valid_cam_state_ids) { - Matrix H_xi = Matrix::Zero(); - Matrix H_fi = Matrix::Zero(); - Vector4d r_i = Vector4d::Zero(); - PhotometricMeasurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i); + //Matrix H_xi = Matrix::Zero(); + //Matrix H_fi = Matrix::Zero(); + MatrixXd H_xl; + MatrixXd H_yl; + Eigen::VectorXd r_l = VectorXd::Zero(N*N); + + PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); auto cam_state_iter = state_server.cam_states.find(cam_id); int cam_state_cntr = std::distance( state_server.cam_states.begin(), cam_state_iter); // Stack the Jacobians. - H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi; - H_fj.block<4, 3>(stack_cntr, 0) = H_fi; - r_j.segment<4>(stack_cntr) = r_i; - stack_cntr += 4; + H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl; + H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl; + r_i.segment(stack_cntr, N*N) = r_l; + stack_cntr += N*N; } // visu - feature /* @@ -1141,13 +1177,14 @@ void MsckfVio::PhotometricFeatureJacobian( */ // Project the residual and Jacobians onto the nullspace - // of H_fj. - JacobiSVD svd_helper(H_fj, ComputeFullU | ComputeThinV); + // of H_yj. + + JacobiSVD svd_helper(H_yi, ComputeFullU | ComputeThinV); MatrixXd A = svd_helper.matrixU().rightCols( jacobian_row_size - 3); - H_x = A.transpose() * H_xj; - r = A.transpose() * r_j; + H_x = A.transpose() * H_xi; + r = A.transpose() * r_i; return; } @@ -1290,6 +1327,7 @@ void MsckfVio::measurementUpdate( // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; + cout << " measurement update ..." << endl; if (H.rows() > H.cols()) { // Convert H to a sparse matrix. @@ -1480,12 +1518,18 @@ void MsckfVio::removeLostFeatures() { MatrixXd H_xj; VectorXd r_j; PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); + cout << "made gating test" << endl; } + else + { + cout << "failed gating test" << endl; + } + + cout << " stacked features up" << endl; // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. @@ -1628,18 +1672,27 @@ void MsckfVio::pruneCamStateBuffer() { MatrixXd H_xj; VectorXd r_j; - + + cout << "getting featureJacobian..."; PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + cout << "done" << endl; + if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); + cout << "made gating test" << endl; + } + else + { + cout << "failed gating test" << endl; } - for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } + cout << " stacked features up" << endl; + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -1728,6 +1781,10 @@ void MsckfVio::onlineReset() { nh.param("initial_covariance/extrinsic_translation_cov", extrinsic_translation_cov, 1e-4); + // Reset the irradiance covariance + nh.param("initial_covariance/irradiance_frame_bias", + irradiance_frame_bias, 0.1); + state_server.state_cov = MatrixXd::Zero(21, 21); for (int i = 3; i < 6; ++i) state_server.state_cov(i, i) = gyro_bias_cov; From 6ba26d782d1c49da2bd9c22b2aa20ce8a5eef5c3 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Thu, 25 Apr 2019 11:16:44 +0200 Subject: [PATCH 22/86] added flag to switch to original, using right null space matrix for calculation now and existing eigen function, gating restult still way to high --- include/msckf_vio/feature.hpp | 4 +- include/msckf_vio/msckf_vio.h | 3 ++ launch/msckf_vio_debug_tum.launch | 3 ++ launch/msckf_vio_tum.launch | 3 ++ src/msckf_vio.cpp | 72 ++++++++++++++++++++----------- 5 files changed, 59 insertions(+), 26 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 6e013ea..4d1a9e0 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -441,7 +441,7 @@ bool Feature::VisualizePatch( float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const { - return (float)image.at(pose.x, pose.y); + return (float)(image.at(pose.x, pose.y)); } cv::Point2f Feature::projectPositionToCamera( @@ -496,7 +496,7 @@ bool Feature::initializeAnchor( //initialize patch Size //TODO make N size a ros parameter - int N = 13; + int N = 9; int n = (int)(N-1)/2; auto anchor = observations.begin(); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 536c8ca..50e78d1 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -201,6 +201,9 @@ class MsckfVio { // Reset the system online if the uncertainty is too large. void onlineReset(); + // Photometry flag + bool PHOTOMETRIC; + // Chi squared test table. static std::map chi_squared_test_table; diff --git a/launch/msckf_vio_debug_tum.launch b/launch/msckf_vio_debug_tum.launch index 7597f79..e80c5ef 100644 --- a/launch/msckf_vio_debug_tum.launch +++ b/launch/msckf_vio_debug_tum.launch @@ -18,6 +18,9 @@ output="screen" launch-prefix="xterm -e gdb --args"> + + + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 18a5f4b..edd0657 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -17,6 +17,9 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> + + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index e7a696a..5ed2eca 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -27,6 +27,7 @@ #include #include + using namespace std; using namespace Eigen; @@ -59,6 +60,10 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh): } bool MsckfVio::loadParameters() { + //Photometry Flag + nh.param("PHOTOMETRIC", PHOTOMETRIC, false); + + // Frame id nh.param("fixed_frame_id", fixed_frame_id, "world"); nh.param("child_frame_id", child_frame_id, "robot"); @@ -340,7 +345,7 @@ void MsckfVio::imageCallback( // Augment the state vector. start_time = ros::Time::now(); - PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); + stateAugmentation(feature_msg->header.stamp.toSec()); double state_augmentation_time = ( ros::Time::now()-start_time).toSec(); @@ -968,7 +973,7 @@ void MsckfVio::PhotometricMeasurementJacobian( const Vector3d& t_c0_w = cam_state.position; //temp N - const int N = 13; + const int N = 9; // Cam1 pose. Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); @@ -1116,7 +1121,7 @@ void MsckfVio::PhotometricFeatureJacobian( const auto& feature = map_server[feature_id]; - int N = 13; + int N = 9; // Check how many camera states in the provided camera // id camera has actually seen this feature. vector valid_cam_state_ids(0); @@ -1179,12 +1184,25 @@ void MsckfVio::PhotometricFeatureJacobian( // Project the residual and Jacobians onto the nullspace // of H_yj. - JacobiSVD svd_helper(H_yi, ComputeFullU | ComputeThinV); - MatrixXd A = svd_helper.matrixU().rightCols( - jacobian_row_size - 3); + // get Nullspace - H_x = A.transpose() * H_xi; - r = A.transpose() * r_i; + FullPivLU lu(H_yi); + MatrixXd A_right = lu.kernel(); + //FullPivLU lu2(A_right.transpose()); + //MatrixXd A = lu2.kernel().transpose(); + /* + JacobiSVD svd_helper(H_yi, ComputeFullU | ComputeThinV); + int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size(); + MatrixXd A = svd_helper.matrixU().rightCols( + jacobian_row_size-null_space_size); + */ + //H_x = A.transpose() * H_xi; + //r = A.transpose() * r_i; + H_x = H_xi * A_right; + r = r_i * A_right; + + cout << "r\n" << r << endl; + cout << "Hx\n" << H_x << endl; return; } @@ -1327,7 +1345,6 @@ void MsckfVio::measurementUpdate( // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; - cout << " measurement update ..." << endl; if (H.rows() > H.cols()) { // Convert H to a sparse matrix. @@ -1431,8 +1448,8 @@ bool MsckfVio::gatingTest( MatrixXd::Identity(H.rows(), H.rows()); double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - //cout << dof << " " << gamma << " " << - // chi_squared_test_table[dof] << " "; + cout << dof << " " << gamma << " " << + chi_squared_test_table[dof] << " "; if (gamma < chi_squared_test_table[dof]) { //cout << "passed" << endl; @@ -1517,7 +1534,12 @@ void MsckfVio::removeLostFeatures() { MatrixXd H_xj; VectorXd r_j; - PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j); + + if(PHOTOMETRIC) + PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j); + else + featureJacobian(feature.id, cam_state_ids, H_xj, r_j); + if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; @@ -1529,8 +1551,6 @@ void MsckfVio::removeLostFeatures() { cout << "failed gating test" << endl; } - cout << " stacked features up" << endl; - // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. if (stack_cntr > 1500) break; @@ -1673,9 +1693,10 @@ void MsckfVio::pruneCamStateBuffer() { MatrixXd H_xj; VectorXd r_j; - cout << "getting featureJacobian..."; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - cout << "done" << endl; + if(PHOTOMETRIC) + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + else + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; @@ -1691,19 +1712,22 @@ void MsckfVio::pruneCamStateBuffer() { feature.observations.erase(cam_id); } - cout << " stacked features up" << endl; - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); // Perform measurement update. measurementUpdate(H_x, r); - + + int augmentationSize = 6; + if(PHOTOMETRIC) + augmentationSize = 7; + for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_id)); - int cam_state_start = 21 + 6*cam_sequence; - int cam_state_end = cam_state_start + 6; + int cam_state_start = 21 + augmentationSize*cam_sequence; + int cam_state_end = cam_state_start + augmentationSize; + // Remove the corresponding rows and columns in the state // covariance matrix. @@ -1723,10 +1747,10 @@ void MsckfVio::pruneCamStateBuffer() { state_server.state_cov.cols()-cam_state_end); state_server.state_cov.conservativeResize( - state_server.state_cov.rows()-6, state_server.state_cov.cols()-6); + state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); } else { state_server.state_cov.conservativeResize( - state_server.state_cov.rows()-6, state_server.state_cov.cols()-6); + state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); } // Remove this camera state in the state vector. From de07296d31839b19d48370bd4e31964947775efd Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Thu, 25 Apr 2019 13:44:21 +0200 Subject: [PATCH 23/86] added minor changes to nullspace --- src/msckf_vio.cpp | 52 +++++++++++++++++++++++++---------------------- 1 file changed, 28 insertions(+), 24 deletions(-) diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 5ed2eca..a4a6874 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -345,7 +345,10 @@ void MsckfVio::imageCallback( // Augment the state vector. start_time = ros::Time::now(); - stateAugmentation(feature_msg->header.stamp.toSec()); + if(PHOTOMETRIC) + PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); + else + stateAugmentation(feature_msg->header.stamp.toSec()); double state_augmentation_time = ( ros::Time::now()-start_time).toSec(); @@ -1186,20 +1189,20 @@ void MsckfVio::PhotometricFeatureJacobian( // get Nullspace - FullPivLU lu(H_yi); - MatrixXd A_right = lu.kernel(); - //FullPivLU lu2(A_right.transpose()); - //MatrixXd A = lu2.kernel().transpose(); - /* + JacobiSVD svd_helper(H_yi, ComputeFullU | ComputeThinV); + int sv_size = 0; + Eigen::VectorXd singularValues = svd_helper.singularValues(); + for(int i = 0; i < singularValues.size(); i++) + if(singularValues[i] < 1e-3) + sv_size++; + int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size(); MatrixXd A = svd_helper.matrixU().rightCols( jacobian_row_size-null_space_size); - */ - //H_x = A.transpose() * H_xi; - //r = A.transpose() * r_i; - H_x = H_xi * A_right; - r = r_i * A_right; + + H_x = A.transpose() * H_xi; + r = A.transpose() * r_i; cout << "r\n" << r << endl; cout << "Hx\n" << H_x << endl; @@ -1262,17 +1265,6 @@ void MsckfVio::measurementJacobian( H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc; H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg; - // Modifty the measurement Jacobian to ensure - // observability constrain. - Matrix A = H_x; - Matrix u = Matrix::Zero(); - u.block<3, 1>(0, 0) = quaternionToRotation( - cam_state.orientation_null) * IMUState::gravity; - u.block<3, 1>(3, 0) = skewSymmetric( - p_w-cam_state.position_null) * IMUState::gravity; - H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose(); - H_f = -H_x.block<4, 3>(0, 3); - // Compute the residual. r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); @@ -1327,11 +1319,23 @@ void MsckfVio::featureJacobian( // Project the residual and Jacobians onto the nullspace // of H_fj. JacobiSVD svd_helper(H_fj, ComputeFullU | ComputeThinV); + int sv_size = 0; + Eigen::VectorXd singularValues = svd_helper.singularValues(); + for(int i = 0; i < singularValues.size(); i++) + if(singularValues[i] < 1e-3) + sv_size++; + + int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size(); + + //cout << "singular values: \n" << svd_helper.singularValues(); + cout << "null_space: " << null_space_size << endl; + MatrixXd A = svd_helper.matrixU().rightCols( jacobian_row_size - 3); + + H_x = A.transpose() * H_xj; + r = A.transpose() * r_j; - H_x = A.transpose() * H_xj; - r = A.transpose() * r_j; return; } From e2e936ff01c82b6baee4875061ecf66e36037fd7 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Thu, 25 Apr 2019 19:13:22 +0200 Subject: [PATCH 24/86] fixed non 0 filling of new state covariance --- include/msckf_vio/feature.hpp | 6 +-- include/msckf_vio/msckf_vio.h | 2 + src/msckf_vio.cpp | 92 +++++++++++++++++++++++------------ 3 files changed, 65 insertions(+), 35 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 4d1a9e0..1d9d92e 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -441,7 +441,7 @@ bool Feature::VisualizePatch( float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const { - return (float)(image.at(pose.x, pose.y)); + return ((float)image.at(pose.x, pose.y))/256; } cv::Point2f Feature::projectPositionToCamera( @@ -496,7 +496,7 @@ bool Feature::initializeAnchor( //initialize patch Size //TODO make N size a ros parameter - int N = 9; + int N = 3; int n = (int)(N-1)/2; auto anchor = observations.begin(); @@ -551,7 +551,7 @@ bool Feature::initializeAnchor( return false; } for(auto point : vec) - anchorPatch.push_back((double)anchorImage.at((int)point.x,(int)point.y)); + anchorPatch.push_back(PixelIrradiance(point, anchorImage)); // project patch pixel to 3D space for(auto point : und_v) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 50e78d1..04efc65 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -204,6 +204,8 @@ class MsckfVio { // Photometry flag bool PHOTOMETRIC; + bool nan_flag; + // Chi squared test table. static std::map chi_squared_test_table; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index a4a6874..c58dc27 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -55,6 +55,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh): is_gravity_set(false), is_first_img(true), image_sub(10), + nan_flag(false), nh(pnh) { return; } @@ -488,6 +489,7 @@ bool MsckfVio::resetCallback( std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { + cout << "reset" << endl; ROS_WARN("Start resetting msckf vio..."); // Temporarily shutdown the subscribers to prevent the // state from updating. @@ -901,8 +903,10 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) { size_t old_rows = state_server.state_cov.rows(); size_t old_cols = state_server.state_cov.cols(); + MatrixXd temp_cov = state_server.state_cov; + // add 7 for camera state + irradiance bias eta = b_l - state_server.state_cov.conservativeResize(old_rows+7, old_cols+7); + state_server.state_cov.conservativeResizeLike(Eigen::MatrixXd::Zero(old_rows+7, old_cols+7)); // Rename some matrix blocks for convenience. const Matrix& P11 = @@ -918,13 +922,13 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) { J * P11 * J.transpose(); // Add photometry P_eta and surrounding Zeros - state_server.state_cov.block<1, 12>(old_rows+6, 0) = Matrix::Zero(); - state_server.state_cov.block<12, 1>(0, old_cols+6) = Matrix::Zero(); state_server.state_cov(old_rows+6, old_cols+6) = irradiance_frame_bias; + // Fix the covariance to be symmetric MatrixXd state_cov_fixed = (state_server.state_cov + state_server.state_cov.transpose()) / 2.0; state_server.state_cov = state_cov_fixed; + return; } @@ -976,7 +980,7 @@ void MsckfVio::PhotometricMeasurementJacobian( const Vector3d& t_c0_w = cam_state.position; //temp N - const int N = 9; + const int N = 3; // Cam1 pose. Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); @@ -1112,7 +1116,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //TODO make this more fluent as well count = 0; for(auto data : photo_r) - r[count++] = data; + r[count++] = data; return; } @@ -1124,7 +1128,7 @@ void MsckfVio::PhotometricFeatureJacobian( const auto& feature = map_server[feature_id]; - int N = 9; + int N = 3; // Check how many camera states in the provided camera // id camera has actually seen this feature. vector valid_cam_state_ids(0); @@ -1194,26 +1198,24 @@ void MsckfVio::PhotometricFeatureJacobian( int sv_size = 0; Eigen::VectorXd singularValues = svd_helper.singularValues(); for(int i = 0; i < singularValues.size(); i++) - if(singularValues[i] < 1e-3) + if(singularValues[i] > 1e-5) sv_size++; int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size(); MatrixXd A = svd_helper.matrixU().rightCols( - jacobian_row_size-null_space_size); + jacobian_row_size-sv_size); H_x = A.transpose() * H_xi; r = A.transpose() * r_i; - cout << "r\n" << r << endl; - cout << "Hx\n" << H_x << endl; - return; } void MsckfVio::measurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, - Matrix& H_x, Matrix& H_f, Vector4d& r) { + Matrix& H_x, Matrix& H_f, Vector4d& r) +{ // Prepare all the required data. const CAMState& cam_state = state_server.cam_states[cam_state_id]; @@ -1275,7 +1277,8 @@ void MsckfVio::measurementJacobian( void MsckfVio::featureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, - MatrixXd& H_x, VectorXd& r) { + MatrixXd& H_x, VectorXd& r) +{ const auto& feature = map_server[feature_id]; @@ -1322,29 +1325,28 @@ void MsckfVio::featureJacobian( int sv_size = 0; Eigen::VectorXd singularValues = svd_helper.singularValues(); for(int i = 0; i < singularValues.size(); i++) - if(singularValues[i] < 1e-3) + if(singularValues[i] > 1e-5) sv_size++; - int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size(); - - //cout << "singular values: \n" << svd_helper.singularValues(); - cout << "null_space: " << null_space_size << endl; + int null_space_size = svd_helper.matrixU().cols() - sv_size; MatrixXd A = svd_helper.matrixU().rightCols( - jacobian_row_size - 3); + jacobian_row_size - sv_size); H_x = A.transpose() * H_xj; r = A.transpose() * r_j; + cout << "A: \n" << A.transpose() << endl; return; } -void MsckfVio::measurementUpdate( - const MatrixXd& H, const VectorXd& r) { +void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { if (H.rows() == 0 || r.rows() == 0) return; + + cout << "decomposition..."; // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -1377,7 +1379,8 @@ void MsckfVio::measurementUpdate( H_thin = H; r_thin = r; } - + cout << "done" << endl; + cout << "computing K..."; // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; MatrixXd S = H_thin*P*H_thin.transpose() + @@ -1387,6 +1390,8 @@ void MsckfVio::measurementUpdate( MatrixXd K_transpose = S.ldlt().solve(H_thin*P); MatrixXd K = K_transpose.transpose(); + cout << "done" << endl; + // Compute the error of the state. VectorXd delta_x = K * r_thin; @@ -1444,12 +1449,19 @@ void MsckfVio::measurementUpdate( return; } -bool MsckfVio::gatingTest( - const MatrixXd& H, const VectorXd& r, const int& dof) { +bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { MatrixXd P1 = H * state_server.state_cov * H.transpose(); + + MatrixXd P2 = Feature::observation_noise * MatrixXd::Identity(H.rows(), H.rows()); + + //cout << "H: \n" << H << endl; + //cout << "cov: \n" << state_server.state_cov << endl; + //cout << "P1: \n" << P1 << endl; + //cout << "solv: \n" << (P1+P2).ldlt().solve(r) << endl; + double gamma = r.transpose() * (P1+P2).ldlt().solve(r); cout << dof << " " << gamma << " " << @@ -1465,13 +1477,14 @@ bool MsckfVio::gatingTest( } void MsckfVio::removeLostFeatures() { - // Remove the features that lost track. // BTW, find the size the final Jacobian matrix and residual vector. int jacobian_row_size = 0; vector invalid_feature_ids(0); vector processed_feature_ids(0); + int N = 3; + for (auto iter = map_server.begin(); iter != map_server.end(); ++iter) { // Rename the feature to be checked. @@ -1507,7 +1520,11 @@ void MsckfVio::removeLostFeatures() { } } - jacobian_row_size += 4*feature.observations.size() - 3; + if(PHOTOMETRIC) + //just use max. size, as gets shrunken down after anyway + jacobian_row_size += N*N*feature.observations.size(); + else + jacobian_row_size += 4*feature.observations.size() - 3; processed_feature_ids.push_back(feature.id); } @@ -1523,8 +1540,12 @@ void MsckfVio::removeLostFeatures() { // Return if there is no lost feature to be processed. if (processed_feature_ids.size() == 0) return; + int augmentationSize = 6; + if(PHOTOMETRIC) + augmentationSize = 7; + MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, - 21+6*state_server.cam_states.size()); + 21+augmentationSize*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1548,11 +1569,11 @@ void MsckfVio::removeLostFeatures() { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); - cout << "made gating test" << endl; + cout << "approved chi" << endl; } else { - cout << "failed gating test" << endl; + cout << "rejected chi" << endl; } // Put an upper bound on the row size of measurement Jacobian, @@ -1573,8 +1594,7 @@ void MsckfVio::removeLostFeatures() { return; } -void MsckfVio::findRedundantCamStates( - vector& rm_cam_state_ids) { +void MsckfVio::findRedundantCamStates(vector& rm_cam_state_ids) { // Move the iterator to the key position. auto key_cam_state_iter = state_server.cam_states.end(); @@ -1627,6 +1647,8 @@ void MsckfVio::pruneCamStateBuffer() { vector rm_cam_state_ids(0); findRedundantCamStates(rm_cam_state_ids); + int N = 3; + // Find the size of the Jacobian matrix. int jacobian_row_size = 0; for (auto& item : map_server) { @@ -1671,7 +1693,10 @@ void MsckfVio::pruneCamStateBuffer() { continue; } } - jacobian_row_size += 4*involved_cam_state_ids.size() - 3; + if(PHOTOMETRIC) + jacobian_row_size += N*N*involved_cam_state_ids.size(); + else + jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } //cout << "jacobian row #: " << jacobian_row_size << endl; @@ -1716,9 +1741,12 @@ void MsckfVio::pruneCamStateBuffer() { feature.observations.erase(cam_id); } + cout << "resize" << endl; + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); + cout << "done" << endl; // Perform measurement update. measurementUpdate(H_x, r); From e8489dbd063629347c44ac0c6f4f9ddb14f33848 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 26 Apr 2019 09:44:19 +0200 Subject: [PATCH 25/86] removed resizing not correcting for photometric info, added N as global variable --- include/msckf_vio/feature.hpp | 6 +-- include/msckf_vio/msckf_vio.h | 3 ++ launch/msckf_vio_tum.launch | 1 + src/msckf_vio.cpp | 69 ++++++++++++++--------------------- 4 files changed, 33 insertions(+), 46 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 1d9d92e..75bd36b 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -126,7 +126,7 @@ struct Feature { */ bool initializeAnchor( - const CameraCalibration& cam); + const CameraCalibration& cam, int N); /* @@ -491,12 +491,10 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, //@test center projection must always be initial feature projection bool Feature::initializeAnchor( - const CameraCalibration& cam) + const CameraCalibration& cam, int N) { //initialize patch Size - //TODO make N size a ros parameter - int N = 3; int n = (int)(N-1)/2; auto anchor = observations.begin(); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 04efc65..ba88474 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -206,6 +206,9 @@ class MsckfVio { bool nan_flag; + // Patch size for Photometry + int N; + // Chi squared test table. static std::map chi_squared_test_table; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index edd0657..9c95b1c 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -20,6 +20,7 @@ + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index c58dc27..c87bfd4 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -127,6 +127,10 @@ bool MsckfVio::loadParameters() { nh.param("initial_covariance/irradiance_frame_bias", irradiance_frame_bias, 0.1); + // Get the photometric patch size + nh.param("patch_size_n", + N, 3); + // get camera information (used for back projection) nh.param("cam0/distortion_model", cam0.distortion_model, string("radtan")); @@ -288,7 +292,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 100; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.05); + boost::math::quantile(chi_squared_dist, 0.2); } if (!createRosIO()) return false; @@ -979,9 +983,6 @@ void MsckfVio::PhotometricMeasurementJacobian( Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Vector3d& t_c0_w = cam_state.position; - //temp N - const int N = 3; - // Cam1 pose. Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0; @@ -1128,7 +1129,6 @@ void MsckfVio::PhotometricFeatureJacobian( const auto& feature = map_server[feature_id]; - int N = 3; // Check how many camera states in the provided camera // id camera has actually seen this feature. vector valid_cam_state_ids(0); @@ -1346,7 +1346,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { if (H.rows() == 0 || r.rows() == 0) return; - cout << "decomposition..."; + cout << "Updating..."; // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -1379,8 +1379,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { H_thin = H; r_thin = r; } - cout << "done" << endl; - cout << "computing K..."; + // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; MatrixXd S = H_thin*P*H_thin.transpose() + @@ -1390,8 +1389,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { MatrixXd K_transpose = S.ldlt().solve(H_thin*P); MatrixXd K = K_transpose.transpose(); - cout << "done" << endl; - // Compute the error of the state. VectorXd delta_x = K * r_thin; @@ -1457,15 +1454,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) MatrixXd P2 = Feature::observation_noise * MatrixXd::Identity(H.rows(), H.rows()); - //cout << "H: \n" << H << endl; - //cout << "cov: \n" << state_server.state_cov << endl; - //cout << "P1: \n" << P1 << endl; - //cout << "solv: \n" << (P1+P2).ldlt().solve(r) << endl; - double gamma = r.transpose() * (P1+P2).ldlt().solve(r); cout << dof << " " << gamma << " " << - chi_squared_test_table[dof] << " "; + chi_squared_test_table[dof] << endl; if (gamma < chi_squared_test_table[dof]) { //cout << "passed" << endl; @@ -1480,11 +1472,14 @@ void MsckfVio::removeLostFeatures() { // Remove the features that lost track. // BTW, find the size the final Jacobian matrix and residual vector. int jacobian_row_size = 0; + int augmentationSize = 6; + if(PHOTOMETRIC) + augmentationSize = 7; + + vector invalid_feature_ids(0); vector processed_feature_ids(0); - int N = 3; - for (auto iter = map_server.begin(); iter != map_server.end(); ++iter) { // Rename the feature to be checked. @@ -1513,7 +1508,7 @@ void MsckfVio::removeLostFeatures() { } if(!feature.is_anchored) { - if(!feature.initializeAnchor(cam0)) + if(!feature.initializeAnchor(cam0, N)) { invalid_feature_ids.push_back(feature.id); continue; @@ -1540,10 +1535,6 @@ void MsckfVio::removeLostFeatures() { // Return if there is no lost feature to be processed. if (processed_feature_ids.size() == 0) return; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; - MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); @@ -1569,11 +1560,11 @@ void MsckfVio::removeLostFeatures() { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); - cout << "approved chi" << endl; + cout << "passed" << endl; } else { - cout << "rejected chi" << endl; + cout << "rejected" << endl; } // Put an upper bound on the row size of measurement Jacobian, @@ -1647,10 +1638,12 @@ void MsckfVio::pruneCamStateBuffer() { vector rm_cam_state_ids(0); findRedundantCamStates(rm_cam_state_ids); - int N = 3; - // Find the size of the Jacobian matrix. int jacobian_row_size = 0; + int augmentationSize = 6; + if(PHOTOMETRIC) + augmentationSize = 7; + for (auto& item : map_server) { auto& feature = item.second; // Check how many camera states to be removed are associated @@ -1686,7 +1679,7 @@ void MsckfVio::pruneCamStateBuffer() { } if(!feature.is_anchored) { - if(!feature.initializeAnchor(cam0)) + if(!feature.initializeAnchor(cam0, N)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); @@ -1702,8 +1695,9 @@ void MsckfVio::pruneCamStateBuffer() { //cout << "jacobian row #: " << jacobian_row_size << endl; // Compute the Jacobian and residual. - MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, - 21+6*state_server.cam_states.size()); + MatrixXd H_xj; + VectorXd r_j; + MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; for (auto& item : map_server) { @@ -1719,9 +1713,6 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - MatrixXd H_xj; - VectorXd r_j; - if(PHOTOMETRIC) PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); else @@ -1731,29 +1722,23 @@ void MsckfVio::pruneCamStateBuffer() { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); - cout << "made gating test" << endl; + cout << "passed" << endl; } else { - cout << "failed gating test" << endl; + cout << "rejected" << endl; } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } - cout << "resize" << endl; - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - cout << "done" << endl; // Perform measurement update. measurementUpdate(H_x, r); - - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; + //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_id)); From e3ac604dbf95824bfb08d312b56b737590f33fb9 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 26 Apr 2019 10:45:10 +0200 Subject: [PATCH 26/86] changed structure for sublime folding --- src/msckf_vio.cpp | 41 ++++++++++++++++++++++------------------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index c87bfd4..9cd6334 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -292,7 +292,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 100; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.2); + boost::math::quantile(chi_squared_dist, 0.05); } if (!createRosIO()) return false; @@ -301,9 +301,7 @@ bool MsckfVio::initialize() { return true; } -void MsckfVio::imuCallback( - const sensor_msgs::ImuConstPtr& msg) -{ +void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){ // IMU msgs are pushed backed into a buffer instead of // being processed immediately. The IMU msgs are processed @@ -491,7 +489,8 @@ void MsckfVio::initializeGravityAndBias() { bool MsckfVio::resetCallback( std_srvs::Trigger::Request& req, - std_srvs::Trigger::Response& res) { + std_srvs::Trigger::Response& res) +{ cout << "reset" << endl; ROS_WARN("Start resetting msckf vio..."); @@ -559,17 +558,16 @@ bool MsckfVio::resetCallback( imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this); -// feature_sub = nh.subscribe("features", 40, -// &MsckfVio::featureCallback, this); + // feature_sub = nh.subscribe("features", 40, + // &MsckfVio::featureCallback, this); -// TODO: When can the reset fail? + // TODO: When can the reset fail? res.success = true; ROS_WARN("Resetting msckf vio completed..."); return true; } -void MsckfVio::mocapOdomCallback( - const nav_msgs::OdometryConstPtr& msg) { +void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) { static bool first_mocap_odom_msg = true; // If this is the first mocap odometry messsage, set @@ -668,7 +666,8 @@ void MsckfVio::batchImuProcessing(const double& time_bound) { void MsckfVio::processModel(const double& time, const Vector3d& m_gyro, - const Vector3d& m_acc) { + const Vector3d& m_acc) +{ // Remove the bias from the measured gyro and acceleration IMUState& imu_state = state_server.imu_state; @@ -739,7 +738,8 @@ void MsckfVio::processModel(const double& time, void MsckfVio::predictNewState(const double& dt, const Vector3d& gyro, - const Vector3d& acc) { + const Vector3d& acc) +{ // TODO: Will performing the forward integration using // the inverse of the quaternion give better accuracy? @@ -802,7 +802,8 @@ void MsckfVio::predictNewState(const double& dt, return; } -void MsckfVio::stateAugmentation(const double& time) { +void MsckfVio::stateAugmentation(const double& time) +{ const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0; const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu; @@ -866,7 +867,8 @@ void MsckfVio::stateAugmentation(const double& time) { return; } -void MsckfVio::PhotometricStateAugmentation(const double& time) { +void MsckfVio::PhotometricStateAugmentation(const double& time) +{ const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0; const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu; @@ -936,10 +938,9 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) { return; } - - void MsckfVio::addFeatureObservations( - const CameraMeasurementConstPtr& msg) { + const CameraMeasurementConstPtr& msg) +{ StateIDType state_id = state_server.imu_state.id; int curr_feature_num = map_server.size(); @@ -973,7 +974,8 @@ void MsckfVio::addFeatureObservations( void MsckfVio::PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, - MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) { + MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) +{ // Prepare all the required data. const CAMState& cam_state = state_server.cam_states[cam_state_id]; @@ -1125,7 +1127,8 @@ void MsckfVio::PhotometricMeasurementJacobian( void MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, - MatrixXd& H_x, VectorXd& r) { + MatrixXd& H_x, VectorXd& r) +{ const auto& feature = map_server[feature_id]; From 750d8ecfb1b4377dd111f1e85aade1cbfd64fc75 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 26 Apr 2019 14:42:31 +0200 Subject: [PATCH 27/86] sublime folding changes --- include/msckf_vio/feature.hpp | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 75bd36b..e84a847 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -240,7 +240,8 @@ typedef std::map, void Feature::cost(const Eigen::Isometry3d& T_c0_ci, const Eigen::Vector3d& x, const Eigen::Vector2d& z, - double& e) const { + double& e) const +{ // Compute hi1, hi2, and hi3 as Equation (37). const double& alpha = x(0); const double& beta = x(1); @@ -263,7 +264,8 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci, void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci, const Eigen::Vector3d& x, const Eigen::Vector2d& z, Eigen::Matrix& J, Eigen::Vector2d& r, - double& w) const { + double& w) const +{ // Compute hi1, hi2, and hi3 as Equation (37). const double& alpha = x(0); @@ -300,7 +302,8 @@ void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci, void Feature::generateInitialGuess( const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1, - const Eigen::Vector2d& z2, Eigen::Vector3d& p) const { + const Eigen::Vector2d& z2, Eigen::Vector3d& p) const +{ // Construct a least square problem to solve the depth. Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0); @@ -320,8 +323,8 @@ void Feature::generateInitialGuess( return; } -bool Feature::checkMotion( - const CamStateServer& cam_states) const { +bool Feature::checkMotion(const CamStateServer& cam_states) const +{ const StateIDType& first_cam_id = observations.begin()->first; const StateIDType& last_cam_id = (--observations.end())->first; @@ -399,7 +402,6 @@ bool Feature::estimate_FrameIrradiance( float irradiance = (anchorPixel - b_A) / a_A ; anchorPatch_estimate.push_back(irradiance); } - } @@ -441,6 +443,7 @@ bool Feature::VisualizePatch( float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const { + return ((float)image.at(pose.x, pose.y))/256; } @@ -476,8 +479,7 @@ cv::Point2f Feature::projectPositionToCamera( return my_p; } -Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, - const CameraCalibration& cam) +Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam) { // use undistorted position of point of interest // project it back into 3D space using pinhole model @@ -490,8 +492,7 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, } //@test center projection must always be initial feature projection -bool Feature::initializeAnchor( - const CameraCalibration& cam, int N) +bool Feature::initializeAnchor(const CameraCalibration& cam, int N) { //initialize patch Size @@ -532,7 +533,7 @@ bool Feature::initializeAnchor( cam.distortion_model, 0, und_v); -//create distorted pixel points + //create distorted pixel points std::vector vec = image_handler::distortPoints(und_v, cam.intrinsics, cam.distortion_model, @@ -561,8 +562,7 @@ bool Feature::initializeAnchor( return true; } -bool Feature::initializePosition( - const CamStateServer& cam_states) { +bool Feature::initializePosition(const CamStateServer& cam_states) { // Organize camera poses and feature observations properly. std::vector Date: Tue, 30 Apr 2019 17:02:22 +0200 Subject: [PATCH 28/86] added visualization with a ros flag, which shows feature with projection and residual (the features apparent movement) --- include/msckf_vio/feature.hpp | 123 +++++++++++++++++++++++++--------- include/msckf_vio/msckf_vio.h | 2 + launch/msckf_vio_tum.launch | 3 +- src/msckf_vio.cpp | 41 +++--------- 4 files changed, 106 insertions(+), 63 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index e84a847..f278b37 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -171,8 +171,8 @@ struct Feature { bool VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0) const; - + CameraCalibration& cam0, + const std::vector photo_r) const; /* * @brief projectPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em @@ -204,6 +204,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, // NxN Patch of Anchor Image std::vector anchorPatch; std::vector anchorPatch_ideal; + std::vector anchorPatch_real; // Position of NxN Patch in 3D space std::vector anchorPatch_3d; @@ -408,43 +409,104 @@ bool Feature::estimate_FrameIrradiance( bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0) const + CameraCalibration& cam0, + const std::vector photo_r) const { + double rescale = 1; + + //visu - anchor + auto anchor = observations.begin(); + cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image; + cv::Mat dottedFrame(anchorImage.size(), CV_8UC3); + cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB); + + for(auto point : anchorPatch_real) + { + // visu - feature + cv::Point xs(point.x, point.y); + cv::Point ys(point.x, point.y); + cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); + } + cam0.featureVisu = dottedFrame.clone(); + // visu - feature cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; - cv::Mat dottedFrame(current_image.size(), CV_8UC3); cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB); - - // project every point in anchorPatch_3d. - auto frame = cam0.moving_window.find(cam_state_id)->second.image; + // set position in frame + // save irradiance of projection + std::vector projectionPatch; for(auto point : anchorPatch_3d) { cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); - + projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image)); // visu - feature cv::Point xs(p_in_c0.x, p_in_c0.y); cv::Point ys(p_in_c0.x, p_in_c0.y); cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); } - // testing - //if(cam_state_id == observations.begin()->first) - //if(count++ == 4) - //printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y); - // visu - feature - cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2)); - if(cam0.featureVisu.empty()) - cam0.featureVisu = dottedFrame.clone(); - else - cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); + cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); + + // irradiance grid anchor + int N = sqrt(anchorPatch_3d.size()); + int scale = 20; + cv::Mat irradianceFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255)); + cv::resize(irradianceFrame, irradianceFrame, cv::Size(), rescale, rescale); + for(int i = 0; i0) + cv::rectangle(irradianceFrame, + cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), + cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), + cv::Scalar(255 - photo_r[i*N+j]*255, 255 - photo_r[i*N+j]*255, 255), + CV_FILLED); + else + cv::rectangle(irradianceFrame, + cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), + cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), + cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), + CV_FILLED); + + cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale); + + cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); + + // create line? + + //save image + + std::stringstream ss; + ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; + cv::imwrite(ss.str(), cam0.featureVisu); + + //cv::imshow("patch", cam0.featureVisu); + //cvWaitKey(1); } float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const { - - return ((float)image.at(pose.x, pose.y))/256; + + return ((float)image.at(pose.y, pose.x))/255; } cv::Point2f Feature::projectPositionToCamera( @@ -514,16 +576,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) int count = 0; // get feature in undistorted pixel space + // this only reverts from 'pure' space into undistorted pixel space using camera matrix cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), cam.intrinsics, cam.distortion_model, 0); // create vector of patch in pixel plane - std::vector und_pix_v; + std::vectorund_pix_v; for(double u_run = -n; u_run <= n; u_run++) for(double v_run = -n; v_run <= n; v_run++) - und_pix_v.push_back(cv::Point2f(und_pix_p.x-u_run, und_pix_p.y-v_run)); - + und_pix_v.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); //create undistorted pure points @@ -533,23 +595,24 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) cam.distortion_model, 0, und_v); + //create distorted pixel points - std::vector vec = image_handler::distortPoints(und_v, - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs); + anchorPatch_real = image_handler::distortPoints(und_v, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); // save anchor position for later visualisaztion - anchor_center_pos = vec[4]; + anchor_center_pos = anchorPatch_real[(N*N-1)/2]; // save true pixel Patch position - for(auto point : vec) + for(auto point : anchorPatch_real) { if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) return false; } - for(auto point : vec) + for(auto point : anchorPatch_real) anchorPatch.push_back(PixelIrradiance(point, anchorImage)); // project patch pixel to 3D space diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index ba88474..cd5bb92 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -202,7 +202,9 @@ class MsckfVio { void onlineReset(); // Photometry flag + // visualization flag bool PHOTOMETRIC; + bool PRINTIMAGES; bool nan_flag; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 9c95b1c..0032c70 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -19,8 +19,9 @@ + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 9cd6334..0990c45 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -63,7 +63,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh): bool MsckfVio::loadParameters() { //Photometry Flag nh.param("PHOTOMETRIC", PHOTOMETRIC, false); - + nh.param("PrintImages", PRINTIMAGES, false); // Frame id nh.param("fixed_frame_id", fixed_frame_id, "world"); @@ -1027,11 +1027,11 @@ void MsckfVio::PhotometricMeasurementJacobian( //add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - //add jacobian + // add jacobian // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame); - dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame); + dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame); + dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame); dI_dhj(0, 0) = dx; dI_dhj(0, 1) = dy; @@ -1043,7 +1043,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //dh / d X_{pl} dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point); - dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose(); + dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation);//.transpose(); //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; @@ -1074,6 +1074,7 @@ void MsckfVio::PhotometricMeasurementJacobian( std::vector estimate_photo_z; IlluminationParameter estimated_illumination; feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); + for (auto& estimate_irradiance_j : estimate_irradiance) estimate_photo_z.push_back (estimate_irradiance_j * estimated_illumination.frame_gain * estimated_illumination.feature_gain + @@ -1121,6 +1122,9 @@ void MsckfVio::PhotometricMeasurementJacobian( for(auto data : photo_r) r[count++] = data; + if(PRINTIMAGES) + feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r); + return; } @@ -1151,15 +1155,8 @@ void MsckfVio::PhotometricFeatureJacobian( VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; - // visu - residual - //printf("_____FEATURE:_____\n"); - // visu - feature - //cam0.featureVisu.release(); - for (const auto& cam_id : valid_cam_state_ids) { - //Matrix H_xi = Matrix::Zero(); - //Matrix H_fi = Matrix::Zero(); MatrixXd H_xl; MatrixXd H_yl; Eigen::VectorXd r_l = VectorXd::Zero(N*N); @@ -1176,27 +1173,11 @@ void MsckfVio::PhotometricFeatureJacobian( r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; } - // visu - feature - /* - if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000) - imshow("feature", cam0.featureVisu); - cvWaitKey(1); - - if((ros::Time::now() - image_save_time).toSec() > 1) - { - std::stringstream ss; - ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; - imwrite(ss.str(), cam0.featureVisu); - image_save_time = ros::Time::now(); - } - */ // Project the residual and Jacobians onto the nullspace // of H_yj. // get Nullspace - - JacobiSVD svd_helper(H_yi, ComputeFullU | ComputeThinV); int sv_size = 0; Eigen::VectorXd singularValues = svd_helper.singularValues(); @@ -1339,8 +1320,6 @@ void MsckfVio::featureJacobian( H_x = A.transpose() * H_xj; r = A.transpose() * r_j; - cout << "A: \n" << A.transpose() << endl; - return; } @@ -1348,8 +1327,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { if (H.rows() == 0 || r.rows() == 0) return; - - cout << "Updating..."; // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; From acbcf79417f759eba0820a21562c1a198b059a7d Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 30 Apr 2019 18:25:05 +0200 Subject: [PATCH 29/86] fixed some typos in jacobian --- src/msckf_vio.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 0990c45..965484d 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1047,7 +1047,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; - dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); + dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho)); dGpi_XpAj.block<3, 3>(0, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho))); dGpi_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); From ee40dff740ff22921f5d778eca18f57d5dd8f7e0 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Thu, 2 May 2019 17:02:44 +0200 Subject: [PATCH 30/86] added minor visualization changes --- include/msckf_vio/feature.hpp | 6 ++++++ launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 10 ++++++++++ 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index f278b37..76e4eca 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -491,6 +491,12 @@ bool Feature::VisualizePatch( cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); + // write feature position + std::stringstream pos_s; + pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1); + cv::putText(cam0.featureVisu, pos_s.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), + cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA); + // create line? //save image diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 0032c70..4b9666d 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -19,7 +19,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 965484d..1998f1b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1836,12 +1836,22 @@ void MsckfVio::publish(const ros::Time& time) { Eigen::Vector3d body_velocity = IMUState::T_imu_body.linear() * imu_state.velocity; + // Generate camera frame form IMU Frame + Eigen::Isometry3d T_i_c = Eigen::Isometry3d::Identity(); + T_i_c.linear() = imu_state.R_imu_cam0; + T_i_c.translation() = imu_state.t_cam0_imu; + // Publish tf if (publish_tf) { tf::Transform T_b_w_tf; tf::transformEigenToTF(T_b_w, T_b_w_tf); tf_pub.sendTransform(tf::StampedTransform( T_b_w_tf, time, fixed_frame_id, child_frame_id)); + + tf::Transform T_i_c_tf; + tf::transformEigenToTF(T_i_c, T_i_c_tf); + tf_pub.sendTransform(tf::StampedTransform( + T_i_c_tf, time, child_frame_id, "camera")); } // Publish the odometry From 53b26f76139279175fef99d46a8c1b4ec3202692 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 3 May 2019 16:42:27 +0200 Subject: [PATCH 31/86] minor changes to visualization, jakobi tests --- include/msckf_vio/feature.hpp | 50 +++++++++++++--- include/msckf_vio/msckf_vio.h | 30 +++++++++- launch/msckf_vio_tum.launch | 4 ++ src/msckf_vio.cpp | 105 +++++++++++++++++++++++++++++++--- 4 files changed, 172 insertions(+), 17 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 76e4eca..bf71e2e 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -172,7 +172,8 @@ struct Feature { const CAMState& cam_state, const StateIDType& cam_state_id, CameraCalibration& cam0, - const std::vector photo_r) const; + const std::vector photo_r, + std::stringstream& ss) const; /* * @brief projectPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em @@ -410,7 +411,8 @@ bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, CameraCalibration& cam0, - const std::vector photo_r) const + const std::vector photo_r, + std::stringstream& ss) const { double rescale = 1; @@ -421,14 +423,15 @@ bool Feature::VisualizePatch( cv::Mat dottedFrame(anchorImage.size(), CV_8UC3); cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB); + // visualize the true anchor points (the surrounding of the original measurements) for(auto point : anchorPatch_real) { // visu - feature cv::Point xs(point.x, point.y); cv::Point ys(point.x, point.y); - cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); + cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255)); } - cam0.featureVisu = dottedFrame.clone(); + cam0.featureVisu = dottedFrame.clone(); // visu - feature cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; @@ -487,23 +490,52 @@ bool Feature::VisualizePatch( cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), CV_FILLED); + + // visualize position of used observations and resulting feature position + cv::Mat positionFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255)); + cv::resize(positionFrame, positionFrame, cv::Size(), rescale, rescale); + + // draw world zero + cv::line(positionFrame, + cv::Point(20,20), + cv::Point(20,30), + cv::Scalar(0,0,255), + CV_FILLED); + + cv::line(positionFrame, + cv::Point(20,20), + cv::Point(30,20), + cv::Scalar(255,0,0), + CV_FILLED); + // for every observation, get cam state + for(auto obs : observations) + { + cam_state.find(obs->first); + cv::line(positionFrame, + cv::Point(20,20), + cv::Point(30,20), + cv::Scalar(255,0,0), + CV_FILLED); + } + // draw, x y position and arrow with direction - write z next to it + cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale); cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); + cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu); // write feature position std::stringstream pos_s; pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1); - cv::putText(cam0.featureVisu, pos_s.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), + cv::putText(cam0.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA); - // create line? //save image - std::stringstream ss; - ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; - cv::imwrite(ss.str(), cam0.featureVisu); + std::stringstream loc; + loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; + cv::imwrite(loc.str(), cam0.featureVisu); //cv::imshow("patch", cam0.featureVisu); //cvWaitKey(1); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index cd5bb92..606d037 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -107,6 +107,15 @@ class MsckfVio { */ void imuCallback(const sensor_msgs::ImuConstPtr& msg); + /* + * @brief truthCallback + * Callback function for ground truth navigation information + * @param TransformStamped msg + */ + void truthCallback( + const geometry_msgs::TransformStampedPtr& msg); + + /* * @brief imageCallback * Callback function for feature measurements @@ -144,11 +153,23 @@ class MsckfVio { bool resetCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); + // Saves the exposure time and the camera measurementes void manageMovingWindow( const sensor_msgs::ImageConstPtr& cam0_img, const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg); + + // Debug related Functions + // Propagate the true state + void batchTruthProcessing( + const double& time_bound); + + void processTruthtoIMU(const double& time, + const Eigen::Vector4d& m_rot, + const Eigen::Vector3d& m_trans); + + // Filter related functions // Propogate the state void batchImuProcessing( @@ -202,12 +223,16 @@ class MsckfVio { void onlineReset(); // Photometry flag - // visualization flag bool PHOTOMETRIC; + + // debug flag bool PRINTIMAGES; + bool GROUNDTRUTH; bool nan_flag; + double last_time_bound; + // Patch size for Photometry int N; @@ -227,6 +252,8 @@ class MsckfVio { // transfer delay between IMU and Image messages. std::vector imu_msg_buffer; + // Ground Truth message data + std::vector truth_msg_buffer; // Moving Window buffer movingWindow cam0_moving_window; movingWindow cam1_moving_window; @@ -268,6 +295,7 @@ class MsckfVio { // Subscribers and publishers ros::Subscriber imu_sub; + ros::Subscriber truth_sub; ros::Publisher odom_pub; ros::Publisher feature_pub; tf::TransformBroadcaster tf_pub; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 4b9666d..eca1ebc 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -19,7 +19,10 @@ + + + @@ -59,6 +62,7 @@ + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 1998f1b..8123150 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -56,6 +56,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh): is_first_img(true), image_sub(10), nan_flag(false), + last_time_bound(0), nh(pnh) { return; } @@ -64,6 +65,7 @@ bool MsckfVio::loadParameters() { //Photometry Flag nh.param("PHOTOMETRIC", PHOTOMETRIC, false); nh.param("PrintImages", PRINTIMAGES, false); + nh.param("GroundTruth", GROUNDTRUTH, false); // Frame id nh.param("fixed_frame_id", fixed_frame_id, "world"); @@ -255,6 +257,8 @@ bool MsckfVio::createRosIO() { imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this); + truth_sub = nh.subscribe("ground_truth", 100, + &MsckfVio::truthCallback, this); cam0_img_sub.subscribe(nh, "cam0_image", 10); cam1_img_sub.subscribe(nh, "cam1_image", 10); @@ -263,7 +267,6 @@ bool MsckfVio::createRosIO() { image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub); image_sub.registerCallback(&MsckfVio::imageCallback, this); - mocap_odom_sub = nh.subscribe("mocap_odom", 10, &MsckfVio::mocapOdomCallback, this); mocap_odom_pub = nh.advertise("gt_odom", 1); @@ -292,7 +295,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 100; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.05); + boost::math::quantile(chi_squared_dist, 0.2); } if (!createRosIO()) return false; @@ -319,6 +322,10 @@ void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){ return; } +void MsckfVio::truthCallback(const geometry_msgs::TransformStampedPtr& msg){ + truth_msg_buffer.push_back(*msg); +} + void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam0_img, const sensor_msgs::ImageConstPtr& cam1_img, @@ -342,7 +349,10 @@ void MsckfVio::imageCallback( // Propogate the IMU state. // that are received before the image feature_msg. ros::Time start_time = ros::Time::now(); - batchImuProcessing(feature_msg->header.stamp.toSec()); + if(!GROUNDTRUTH) + batchImuProcessing(feature_msg->header.stamp.toSec()); + else + batchTruthProcessing(feature_msg->header.stamp.toSec()); double imu_processing_time = ( ros::Time::now()-start_time).toSec(); @@ -558,6 +568,9 @@ bool MsckfVio::resetCallback( imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this); + truth_sub = nh.subscribe("ground_truth", 100, + &MsckfVio::truthCallback, this); + // feature_sub = nh.subscribe("features", 40, // &MsckfVio::featureCallback, this); @@ -632,6 +645,71 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) { return; } +void MsckfVio::batchTruthProcessing(const double& time_bound) { + + // Counter how many IMU msgs in the buffer are used. + int used_truth_msg_cntr = 0; + + for (const auto& truth_msg : truth_msg_buffer) { + double truth_time = truth_msg.header.stamp.toSec(); + if (truth_time < state_server.imu_state.time) { + ++used_truth_msg_cntr; + continue; + } + if (truth_time > time_bound) break; + + // Convert the msgs. + Eigen::Vector3d m_translation; + Eigen::Vector4d m_rotation; + tf::vectorMsgToEigen(truth_msg.transform.translation, m_translation); + + m_rotation[0] = truth_msg.transform.rotation.x; + m_rotation[1] = truth_msg.transform.rotation.y; + m_rotation[2] = truth_msg.transform.rotation.z; + m_rotation[3] = truth_msg.transform.rotation.w; + + // Execute process model. + processTruthtoIMU(truth_time, m_rotation, m_translation); + ++used_truth_msg_cntr; + } + last_time_bound = time_bound; + + // Set the state ID for the new IMU state. + state_server.imu_state.id = IMUState::next_id++; + + // Remove all used Truth msgs. + truth_msg_buffer.erase(truth_msg_buffer.begin(), + truth_msg_buffer.begin()+used_truth_msg_cntr); + +} + +void MsckfVio::processTruthtoIMU(const double& time, + const Vector4d& m_rot, + const Vector3d& m_trans){ + + IMUState& imu_state = state_server.imu_state; + double dtime = time - imu_state.time; + + Vector4d& q = imu_state.orientation; + Vector3d& v = imu_state.velocity; + Vector3d& p = imu_state.position; + + double dt = time - imu_state.time; + + v = (m_trans-v)/dt; + p = m_trans; + q = m_rot; + + // Update the state correspondes to null space. + imu_state.orientation_null = imu_state.orientation; + imu_state.position_null = imu_state.position; + imu_state.velocity_null = imu_state.velocity; + + // Update the state info + state_server.imu_state.time = time; + +} + void MsckfVio::batchImuProcessing(const double& time_bound) { // Counter how many IMU msgs in the buffer are used. int used_imu_msg_cntr = 0; @@ -1039,17 +1117,22 @@ void MsckfVio::PhotometricMeasurementJacobian( dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix::Identity(); dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); + + //orientation takes camera frame to world frame, we wa dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose(); //dh / d X_{pl} dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point); - dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation);//.transpose(); + dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose(); //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho)); - dGpi_XpAj.block<3, 3>(0, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho))); + dGpi_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear() + * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), + feature.anchorPatch_ideal[count].y/(rho), + 1/(rho))); dGpi_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); // Intermediate Jakobians @@ -1091,6 +1174,7 @@ void MsckfVio::PhotometricMeasurementJacobian( // set anchor Jakobi // get position of anchor in cam states + auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; @@ -1122,8 +1206,10 @@ void MsckfVio::PhotometricMeasurementJacobian( for(auto data : photo_r) r[count++] = data; + std::stringstream ss; + ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) - feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r); + feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); return; } @@ -1238,7 +1324,12 @@ void MsckfVio::measurementJacobian( dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2)); Matrix dpc0_dxc = Matrix::Zero(); - dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); + + // original jacobi + //dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); + // my version of calculation + dpc0_dxc.leftCols(3) = skewSymmetric(R_w_c0 * p_w) - skewSymmetric(R_w_c0 * t_c0_w); + //dpc0_dxc.leftCols(3) = - skewSymmetric(R_w_c0.transpose() * (t_c0_w - p_w)) * R_w_c0; dpc0_dxc.rightCols(3) = -R_w_c0; Matrix dpc1_dxc = Matrix::Zero(); From ad2f4647163152ac19b5a964370ab8bb0ed6ea76 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Thu, 9 May 2019 12:14:12 +0200 Subject: [PATCH 32/86] added 3d visualization and stepping through bag file - minor edits in jakobi --- CMakeLists.txt | 1 + include/msckf_vio/feature.hpp | 146 +++++++++++++++++++++++++++--- include/msckf_vio/math_utils.hpp | 15 +++ include/msckf_vio/msckf_vio.h | 13 ++- launch/msckf_vio_debug_tum.launch | 6 ++ package.xml | 1 + src/msckf_vio.cpp | 130 +++++++++++++++++++++----- 7 files changed, 277 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3f5770a..99992f6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS pcl_conversions pcl_ros std_srvs + visualization_msgs ) ## System dependencies are found with CMake's conventions diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index bf71e2e..8a6cc27 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -16,12 +16,17 @@ #include #include +#include +#include +#include + #include "image_handler.h" #include "math_utils.hpp" #include "imu_state.h" #include "cam_state.h" + namespace msckf_vio { /* @@ -168,6 +173,10 @@ struct Feature { std::vector& anchorPatch_estimate, IlluminationParameter& estimatedIllumination) const; +bool MarkerGeneration( + ros::Publisher& marker_pub, + const CamStateServer& cam_states) const; + bool VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, @@ -207,7 +216,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, std::vector anchorPatch_ideal; std::vector anchorPatch_real; - // Position of NxN Patch in 3D space + // Position of NxN Patch in 3D space in anchor camera frame std::vector anchorPatch_3d; // Anchor Isometry @@ -406,6 +415,118 @@ bool Feature::estimate_FrameIrradiance( } } +// generates markers for every camera position/observation +// and estimated feature/path position +bool Feature::MarkerGeneration( + ros::Publisher& marker_pub, + const CamStateServer& cam_states) const +{ + visualization_msgs::MarkerArray ma; + + // add all camera states used for estimation + int count = 0; + + for(auto observation : observations) + { + visualization_msgs::Marker marker; + marker.header.frame_id = "world"; + marker.header.stamp = ros::Time::now(); + + marker.ns = "cameras"; + marker.id = count++; + marker.type = visualization_msgs::Marker::ARROW; + marker.action = visualization_msgs::Marker::ADD; + + marker.pose.position.x = cam_states.find(observation.first)->second.position(0); + marker.pose.position.y = cam_states.find(observation.first)->second.position(1); + marker.pose.position.z = cam_states.find(observation.first)->second.position(2); + + // rotate form x to z axis + Eigen::Vector4d q = quaternionMultiplication(Eigen::Vector4d(0, -0.707, 0, 0.707), cam_states.find(observation.first)->second.orientation); + + marker.pose.orientation.x = q(0); + marker.pose.orientation.y = q(1); + marker.pose.orientation.z = q(2); + marker.pose.orientation.w = q(3); + + marker.scale.x = 0.15; + marker.scale.y = 0.05; + marker.scale.z = 0.05; + + if(count == 1) + { + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 1.0f; + } + else + { + marker.color.r = 0.0f; + marker.color.g = 1.0f; + marker.color.b = 0.0f; + } + marker.color.a = 1.0; + + marker.lifetime = ros::Duration(0); + + ma.markers.push_back(marker); + } + + // 'delete' any existing cameras (make invisible) + for(int i = count; i < 20; i++) + { + visualization_msgs::Marker marker; + + marker.header.frame_id = "world"; + marker.header.stamp = ros::Time::now(); + + marker.ns = "cameras"; + marker.id = i; + marker.type = visualization_msgs::Marker::ARROW; + marker.action = visualization_msgs::Marker::ADD; + + marker.pose.orientation.w = 1; + + marker.color.a = 0.0; + + marker.lifetime = ros::Duration(1); + + ma.markers.push_back(marker); + } + + //generate feature patch points position + visualization_msgs::Marker marker; + + marker.header.frame_id = "world"; + marker.header.stamp = ros::Time::now(); + + marker.ns = "patch"; + marker.id = 0; + marker.type = visualization_msgs::Marker::POINTS; + marker.action = visualization_msgs::Marker::ADD; + + marker.pose.orientation.w = 1; + + marker.scale.x = 0.02; + marker.scale.y = 0.02; + + marker.color.r = 1.0f; + marker.color.g = 0.0f; + marker.color.b = 0.0f; + marker.color.a = 1.0; + + for(auto point : anchorPatch_3d) + { + geometry_msgs::Point p; + p.x = point(0); + p.y = point(1); + p.z = point(2); + marker.points.push_back(p); + } + ma.markers.push_back(marker); + + marker_pub.publish(ma); +} bool Feature::VisualizePatch( const CAMState& cam_state, @@ -489,12 +610,15 @@ bool Feature::VisualizePatch( cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), CV_FILLED); - + cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); + +/* // visualize position of used observations and resulting feature position cv::Mat positionFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255)); cv::resize(positionFrame, positionFrame, cv::Size(), rescale, rescale); + // draw world zero cv::line(positionFrame, cv::Point(20,20), @@ -510,20 +634,20 @@ bool Feature::VisualizePatch( // for every observation, get cam state for(auto obs : observations) { - cam_state.find(obs->first); cv::line(positionFrame, cv::Point(20,20), cv::Point(30,20), cv::Scalar(255,0,0), CV_FILLED); } + // draw, x y position and arrow with direction - write z next to it cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale); - cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); - cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu); + cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu); +*/ // write feature position std::stringstream pos_s; pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1); @@ -534,11 +658,11 @@ bool Feature::VisualizePatch( //save image std::stringstream loc; - loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; - cv::imwrite(loc.str(), cam0.featureVisu); + // loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; + //cv::imwrite(loc.str(), cam0.featureVisu); - //cv::imshow("patch", cam0.featureVisu); - //cvWaitKey(1); + cv::imshow("patch", cam0.featureVisu); + cvWaitKey(0); } float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const @@ -586,7 +710,7 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCa // save resulting NxN positions for this feature Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho); - Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); + Eigen::Vector3d PositionInWorld = T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); return PositionInWorld; //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); } @@ -653,7 +777,7 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) for(auto point : anchorPatch_real) anchorPatch.push_back(PixelIrradiance(point, anchorImage)); - // project patch pixel to 3D space + // project patch pixel to 3D space in camera coordinate system for(auto point : und_v) { anchorPatch_ideal.push_back(point); diff --git a/include/msckf_vio/math_utils.hpp b/include/msckf_vio/math_utils.hpp index 3000d4c..29fb540 100644 --- a/include/msckf_vio/math_utils.hpp +++ b/include/msckf_vio/math_utils.hpp @@ -43,6 +43,21 @@ inline void quaternionNormalize(Eigen::Vector4d& q) { return; } +/* + * @brief invert rotation of passed quaternion through conjugation + * and return conjugation + */ +inline Eigen::Vector4d quaternionConjugate(Eigen::Vector4d& q) +{ + Eigen::Vector4d p; + p(0) = -q(0); + p(1) = -q(1); + p(2) = -q(2); + p(3) = q(3); + + return p; +} + /* * @brief Perform q1 * q2 */ diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 606d037..27b0ef5 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -181,6 +181,9 @@ class MsckfVio { const Eigen::Vector3d& gyro, const Eigen::Vector3d& acc); + // groundtruth state augmentation + void truePhotometricStateAugmentation(const double& time); + // Measurement update void stateAugmentation(const double& time); void PhotometricStateAugmentation(const double& time); @@ -230,7 +233,7 @@ class MsckfVio { bool GROUNDTRUTH; bool nan_flag; - + bool play; double last_time_bound; // Patch size for Photometry @@ -241,6 +244,13 @@ class MsckfVio { // State vector StateServer state_server; + + // Ground truth state vector + StateServer true_state_server; + + // error state based on ground truth + StateServer err_state_server; + // Maximum number of camera states int max_cam_state_size; @@ -297,6 +307,7 @@ class MsckfVio { ros::Subscriber imu_sub; ros::Subscriber truth_sub; ros::Publisher odom_pub; + ros::Publisher marker_pub; ros::Publisher feature_pub; tf::TransformBroadcaster tf_pub; ros::ServiceServer reset_srv; diff --git a/launch/msckf_vio_debug_tum.launch b/launch/msckf_vio_debug_tum.launch index e80c5ef..7b3b45f 100644 --- a/launch/msckf_vio_debug_tum.launch +++ b/launch/msckf_vio_debug_tum.launch @@ -21,6 +21,12 @@ + + + + + + diff --git a/package.xml b/package.xml index 86a6e20..cf1b6f6 100644 --- a/package.xml +++ b/package.xml @@ -18,6 +18,7 @@ nav_msgs sensor_msgs geometry_msgs + visualization_msgs eigen_conversions tf_conversions random_numbers diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 8123150..8289cb3 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -27,6 +27,10 @@ #include #include +#include +#include +#include + using namespace std; using namespace Eigen; @@ -248,9 +252,14 @@ bool MsckfVio::loadParameters() { } bool MsckfVio::createRosIO() { + + // activating bag playing parameter, for debugging + nh.setParam("/play_bag", true); + odom_pub = nh.advertise("odom", 10); feature_pub = nh.advertise( "feature_point_cloud", 10); + marker_pub = nh.advertise("/visualization_marker_array", 10); reset_srv = nh.advertiseService("reset", &MsckfVio::resetCallback, this); @@ -301,6 +310,19 @@ bool MsckfVio::initialize() { if (!createRosIO()) return false; ROS_INFO("Finish creating ROS IO..."); + /* + rosbag::Bag bag; + bag.open("/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag", rosbag::bagmode::Read); + + for(rosbag::MessageInstance const m: rosbag::View(bag)) + { + std_msgs::Int32::ConstPtr i = m.instantiate(); + if (i != NULL) + std::cout << i->data << std::endl; + } + + bag.close(); + */ return true; } @@ -349,10 +371,14 @@ void MsckfVio::imageCallback( // Propogate the IMU state. // that are received before the image feature_msg. ros::Time start_time = ros::Time::now(); - if(!GROUNDTRUTH) - batchImuProcessing(feature_msg->header.stamp.toSec()); - else + + + batchImuProcessing(feature_msg->header.stamp.toSec()); + + // save true state in seperate state vector + if(GROUNDTRUTH) batchTruthProcessing(feature_msg->header.stamp.toSec()); + double imu_processing_time = ( ros::Time::now()-start_time).toSec(); @@ -645,6 +671,19 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) { return; } +void MsckfVio::calcErrorState() +{ + // true_state_server - state_server = err_state_server + StateServer errState; + errState.imu_state.id = state_server.imu_state.id; + errState.imu_state.time = state-server.imu_state.time; + + errState.imu_state.orientation = quaternionMultiplication(true_state_server.imu_state.orientation, quaterionConjugate(state_server.imu_state.orientation)); + errState.imu_state.position = true_state_server.imu_state.position - state_server.imu_state.position; + errState.imu_state.velocity = + +} + void MsckfVio::batchTruthProcessing(const double& time_bound) { // Counter how many IMU msgs in the buffer are used. @@ -652,7 +691,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { for (const auto& truth_msg : truth_msg_buffer) { double truth_time = truth_msg.header.stamp.toSec(); - if (truth_time < state_server.imu_state.time) { + if (truth_time < true_state_server.imu_state.time) { ++used_truth_msg_cntr; continue; } @@ -675,7 +714,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { last_time_bound = time_bound; // Set the state ID for the new IMU state. - state_server.imu_state.id = IMUState::next_id++; + true_state_server.imu_state.id = IMUState::next_id++; // Remove all used Truth msgs. truth_msg_buffer.erase(truth_msg_buffer.begin(), @@ -687,7 +726,7 @@ void MsckfVio::processTruthtoIMU(const double& time, const Vector4d& m_rot, const Vector3d& m_trans){ - IMUState& imu_state = state_server.imu_state; + IMUState& imu_state = true_state_server.imu_state; double dtime = time - imu_state.time; Vector4d& q = imu_state.orientation; @@ -706,7 +745,7 @@ void MsckfVio::processTruthtoIMU(const double& time, imu_state.velocity_null = imu_state.velocity; // Update the state info - state_server.imu_state.time = time; + true_state_server.imu_state.time = time; } @@ -905,7 +944,6 @@ void MsckfVio::stateAugmentation(const double& time) cam_state.orientation_null = cam_state.orientation; cam_state.position_null = cam_state.position; - // Update the covariance matrix of the state. // To simplify computation, the matrix J below is the nontrivial block // in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision @@ -945,6 +983,33 @@ void MsckfVio::stateAugmentation(const double& time) return; } +void MsckfVio::truePhotometricStateAugmentation(const double& time) +{ + const Matrix3d& true_R_i_c = true_state_server.imu_state.R_imu_cam0; + const Vector3d& true_t_c_i = true_state_server.imu_state.t_cam0_imu; + + // Add a new camera state to the state server. + Matrix3d true_R_w_i = quaternionToRotation( + true_state_server.imu_state.orientation); + Matrix3d true_R_w_c = R_i_c * R_w_i; + Vector3d true_t_c_w = true_state_server.imu_state.position + + R_w_i.transpose()*t_c_i; + + true_state_server.cam_states[true_state_server.imu_state.id] = + CAMState(state_server.imu_state.id); + CAMState& true_cam_state = true_state_server.cam_states[ + true_state_server.imu_state.id]; + + true_cam_state.time = time; + true_cam_state.orientation = rotationToQuaternion(true_R_w_c); + true_cam_state.position = t_c_w; + + true_cam_state.orientation_null = true_cam_state.orientation; + true_cam_state.position_null = true_cam_state.position; + + return; +} + void MsckfVio::PhotometricStateAugmentation(const double& time) { @@ -1068,9 +1133,6 @@ void MsckfVio::PhotometricMeasurementJacobian( Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0; Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); - // 3d feature position in the world frame. - // And its observation with the stereo cameras. - const Vector3d& p_w = feature.position; //photometric observation std::vector photo_z; @@ -1080,9 +1142,13 @@ void MsckfVio::PhotometricMeasurementJacobian( Matrix dh_dCpij = Matrix::Zero(); Matrix dh_dGpij = Matrix::Zero(); Matrix dh_dXplj = Matrix::Zero(); - Matrix dGpi_drhoj = Matrix::Zero(); - Matrix dGpi_XpAj = Matrix::Zero(); - + Matrix dGpj_drhoj = Matrix::Zero(); + Matrix dGpj_XpAj = Matrix::Zero(); + + Matrix dCpij_dGpij = Matrix::Zero(); + Matrix dCpij_dCGtheta = Matrix::Zero(); + Matrix dCpij_dGpC = Matrix::Zero(); + // one line of the NxN Jacobians Eigen::Matrix H_rhoj; Eigen::Matrix H_plj; @@ -1099,7 +1165,7 @@ void MsckfVio::PhotometricMeasurementJacobian( double dx, dy; for (auto point : feature.anchorPatch_3d) { - Eigen::Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w); + Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); //add observation @@ -1118,27 +1184,31 @@ void MsckfVio::PhotometricMeasurementJacobian( dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); + dCpij_dGpij = quaternionToRotation(cam_state.orientation); + //orientation takes camera frame to world frame, we wa - dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose(); + dh_dGpij = dh_dCpij * dCpij_dGpij; //dh / d X_{pl} - dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point); - dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose(); + dCpij_dCGtheta = skewSymmetric(p_c0); + dCpij_dGpC = -quaternionToRotation(cam_state.orientation); + dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta; + dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC; //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; - dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho)); + dGpj_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho)); - dGpi_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear() + dGpj_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho))); - dGpi_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); + dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); // Intermediate Jakobians - H_rhoj = dI_dhj * dh_dGpij * dGpi_drhoj; // 1 x 3 + H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 3 H_plj = dI_dhj * dh_dXplj; // 1 x 6 - H_pAj = dI_dhj * dh_dGpij * dGpi_XpAj; // 1 x 6 + H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; @@ -1209,7 +1279,10 @@ void MsckfVio::PhotometricMeasurementJacobian( std::stringstream ss; ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) + { + feature.MarkerGeneration(marker_pub, state_server.cam_states); feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); + } return; } @@ -1220,6 +1293,12 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd& H_x, VectorXd& r) { + // stop playing bagfile if printing images + if(PRINTIMAGES) + { + std::cout << "stopped playpack" << std::endl; + nh.setParam("/play_bag", false); + } const auto& feature = map_server[feature_id]; // Check how many camera states in the provided camera @@ -1278,6 +1357,11 @@ void MsckfVio::PhotometricFeatureJacobian( H_x = A.transpose() * H_xi; r = A.transpose() * r_i; + if(PRINTIMAGES) + { + std::cout << "resume playback" << std::endl; + nh.setParam("/play_bag", true); + } return; } From 44de215518d7caea2d1640467ba6373ac4b8c388 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 10 May 2019 17:19:29 +0200 Subject: [PATCH 33/86] lots of additional debugging tools implemented to check parts of the algorithm. still no good --- include/msckf_vio/math_utils.hpp | 31 ++++- include/msckf_vio/msckf_vio.h | 6 + launch/msckf_vio_debug_tum.launch | 4 +- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 197 ++++++++++++++++++++++++++---- 5 files changed, 211 insertions(+), 29 deletions(-) diff --git a/include/msckf_vio/math_utils.hpp b/include/msckf_vio/math_utils.hpp index 29fb540..d6cb57a 100644 --- a/include/msckf_vio/math_utils.hpp +++ b/include/msckf_vio/math_utils.hpp @@ -54,10 +54,39 @@ inline Eigen::Vector4d quaternionConjugate(Eigen::Vector4d& q) p(1) = -q(1); p(2) = -q(2); p(3) = q(3); - + quaternionNormalize(p); return p; } +/* + * @brief converts a vector4 to a vector3, dropping (3) + * this is typically used to get the vector part of a quaternion + for eq small angle approximation +*/ +inline Eigen::Vector3d v4tov3(const Eigen::Vector4d& q) +{ + Eigen::Vector3d p; + p(0) = q(0); + p(1) = q(1); + p(2) = q(2); + return p; +} + +/* + * @brief Perform q1 * q2 + */ + +inline Eigen::Vector4d QtoV(const Eigen::Quaterniond& q) +{ + Eigen::Vector4d p; + p(0) = q.x(); + p(1) = q.y(); + p(2) = q.z(); + p(3) = q.w(); + return p; +} + + /* * @brief Perform q1 * q2 */ diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 27b0ef5..bcf5ee2 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -160,11 +160,14 @@ class MsckfVio { const CameraMeasurementConstPtr& feature_msg); + void calcErrorState(); + // Debug related Functions // Propagate the true state void batchTruthProcessing( const double& time_bound); + void processTruthtoIMU(const double& time, const Eigen::Vector4d& m_rot, const Eigen::Vector3d& m_trans); @@ -339,6 +342,9 @@ class MsckfVio { ros::Publisher mocap_odom_pub; geometry_msgs::TransformStamped raw_mocap_odom_msg; Eigen::Isometry3d mocap_initial_frame; + + Eigen::Vector4d mocap_initial_orientation; + Eigen::Vector3d mocap_initial_position; }; typedef MsckfVio::Ptr MsckfVioPtr; diff --git a/launch/msckf_vio_debug_tum.launch b/launch/msckf_vio_debug_tum.launch index 7b3b45f..c4e12e7 100644 --- a/launch/msckf_vio_debug_tum.launch +++ b/launch/msckf_vio_debug_tum.launch @@ -22,8 +22,8 @@ - - + + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index eca1ebc..38f93a2 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -22,7 +22,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 8289cb3..2473996 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -203,6 +203,9 @@ bool MsckfVio::loadParameters() { state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose(); state_server.imu_state.t_cam0_imu = T_cam0_imu.translation(); + true_state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose(); + true_state_server.imu_state.t_cam0_imu = T_cam0_imu.translation(); + CAMState::T_cam0_cam1 = utils::getTransformEigen(nh, "cam1/T_cn_cnm1"); IMUState::T_imu_body = @@ -345,6 +348,54 @@ void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){ } void MsckfVio::truthCallback(const geometry_msgs::TransformStampedPtr& msg){ + static bool first_truth_odom_msg = true; + + // errorstate + /*if(not ErrorState) + return; + */ + + // If this is the first mocap odometry messsage, set + // the initial frame. + if (first_truth_odom_msg) { + Quaterniond orientation; + Vector3d translation; + tf::vectorMsgToEigen( + msg->transform.translation, translation); + tf::quaternionMsgToEigen( + msg->transform.rotation, orientation); + + mocap_initial_orientation = QtoV(orientation); + mocap_initial_position = translation; + + first_truth_odom_msg = false; + } + + // Transform the ground truth. + Quaterniond orientation; + Vector3d translation; + //tf::vectorMsgToEigen( + // msg->transform.translation, translation); + //tf::quaternionMsgToEigen( + // msg->transform.rotation, orientation); + tf::vectorMsgToEigen( + msg->transform.translation, translation); + tf::quaternionMsgToEigen( + msg->transform.rotation, orientation); + + Eigen::Vector4d q = quaternionMultiplication(quaternionConjugate(mocap_initial_orientation), QtoV(orientation)); + + translation -= mocap_initial_position; + + msg->transform.rotation.x = q(0); + msg->transform.rotation.y = q(1); + msg->transform.rotation.z = q(2); + msg->transform.rotation.w = q(3); + + msg->transform.translation.x = translation(0); + msg->transform.translation.y = translation(1); + msg->transform.translation.z = translation(2); + truth_msg_buffer.push_back(*msg); } @@ -376,16 +427,32 @@ void MsckfVio::imageCallback( batchImuProcessing(feature_msg->header.stamp.toSec()); // save true state in seperate state vector - if(GROUNDTRUTH) + + //if(ErrState) + //{ batchTruthProcessing(feature_msg->header.stamp.toSec()); + + if(GROUNDTRUTH) + { + state_server.imu_state.position = true_state_server.imu_state.position; + state_server.imu_state.orientation = true_state_server.imu_state.orientation; + state_server.imu_state.position_null = true_state_server.imu_state.position_null; + state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null; + state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0; + state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu; + } + //} double imu_processing_time = ( ros::Time::now()-start_time).toSec(); // Augment the state vector. start_time = ros::Time::now(); if(PHOTOMETRIC) + { + truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); + } else stateAugmentation(feature_msg->header.stamp.toSec()); double state_augmentation_time = ( @@ -673,15 +740,47 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) { void MsckfVio::calcErrorState() { - // true_state_server - state_server = err_state_server - StateServer errState; - errState.imu_state.id = state_server.imu_state.id; - errState.imu_state.time = state-server.imu_state.time; - errState.imu_state.orientation = quaternionMultiplication(true_state_server.imu_state.orientation, quaterionConjugate(state_server.imu_state.orientation)); - errState.imu_state.position = true_state_server.imu_state.position - state_server.imu_state.position; - errState.imu_state.velocity = + // imu error + err_state_server.imu_state.id = state_server.imu_state.id; + err_state_server.imu_state.time = state_server.imu_state.time; + err_state_server.imu_state.orientation = quaternionMultiplication(true_state_server.imu_state.orientation, quaternionConjugate(state_server.imu_state.orientation)); + + // convert to small angle approximation + err_state_server.imu_state.orientation *= 2; + err_state_server.imu_state.orientation(3) = 0; + + err_state_server.imu_state.position = true_state_server.imu_state.position - state_server.imu_state.position; + err_state_server.imu_state.velocity = true_state_server.imu_state.velocity - state_server.imu_state.velocity; + + err_state_server.imu_state.gyro_bias = true_state_server.imu_state.gyro_bias - true_state_server.imu_state.gyro_bias; + err_state_server.imu_state.gyro_bias = true_state_server.imu_state.acc_bias - true_state_server.imu_state.acc_bias; + + err_state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0 - true_state_server.imu_state.R_imu_cam0; + err_state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu - true_state_server.imu_state.t_cam0_imu; + + err_state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null - true_state_server.imu_state.orientation_null; + err_state_server.imu_state.position_null = true_state_server.imu_state.position_null - true_state_server.imu_state.position_null; + err_state_server.imu_state.velocity_null = true_state_server.imu_state.velocity_null - true_state_server.imu_state.velocity_null; + + auto cam_state_iter = state_server.cam_states.begin(); + auto true_cam_state_iter = true_state_server.cam_states.begin(); + auto err_cam_state_iter = err_state_server.cam_states.begin(); + + for (int i = 0; i < state_server.cam_states.size(); ++i, ++cam_state_iter, ++err_cam_state_iter, ++true_cam_state_iter) + { + // calculate error in camera rotation + Eigen::Vector4d q = cam_state_iter->second.orientation; + Eigen::Vector4d p = quaternionConjugate(true_cam_state_iter->second.orientation); + err_cam_state_iter->second.orientation = quaternionMultiplication(p, q); + // small angle approximation + err_cam_state_iter->second.orientation *= 2; + err_cam_state_iter->second.orientation(3) = 0; + + // calculate error of state position + err_cam_state_iter->second.position = true_cam_state_iter->second.position - cam_state_iter->second.position; + } } void MsckfVio::batchTruthProcessing(const double& time_bound) { @@ -706,7 +805,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { m_rotation[1] = truth_msg.transform.rotation.y; m_rotation[2] = truth_msg.transform.rotation.z; m_rotation[3] = truth_msg.transform.rotation.w; - + quaternionNormalize(m_rotation); // Execute process model. processTruthtoIMU(truth_time, m_rotation, m_translation); ++used_truth_msg_cntr; @@ -714,8 +813,8 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { last_time_bound = time_bound; // Set the state ID for the new IMU state. - true_state_server.imu_state.id = IMUState::next_id++; - + true_state_server.imu_state.id = IMUState::next_id; + err_state_server.imu_state.id = IMUState::next_id; // Remove all used Truth msgs. truth_msg_buffer.erase(truth_msg_buffer.begin(), truth_msg_buffer.begin()+used_truth_msg_cntr); @@ -960,7 +1059,6 @@ void MsckfVio::stateAugmentation(const double& time) size_t old_rows = state_server.state_cov.rows(); size_t old_cols = state_server.state_cov.cols(); - // add 7 for camera state + irradiance bias eta = b_l state_server.state_cov.conservativeResize(old_rows+6, old_cols+6); // Rename some matrix blocks for convenience. @@ -991,18 +1089,26 @@ void MsckfVio::truePhotometricStateAugmentation(const double& time) // Add a new camera state to the state server. Matrix3d true_R_w_i = quaternionToRotation( true_state_server.imu_state.orientation); - Matrix3d true_R_w_c = R_i_c * R_w_i; + Matrix3d true_R_w_c = true_R_i_c * true_R_w_i; Vector3d true_t_c_w = true_state_server.imu_state.position + - R_w_i.transpose()*t_c_i; + true_R_w_i.transpose()*true_t_c_i; true_state_server.cam_states[true_state_server.imu_state.id] = - CAMState(state_server.imu_state.id); + CAMState(true_state_server.imu_state.id); CAMState& true_cam_state = true_state_server.cam_states[ true_state_server.imu_state.id]; + // manage error state size + err_state_server.cam_states[err_state_server.imu_state.id] = + CAMState(err_state_server.imu_state.id); + CAMState& err_cam_state = err_state_server.cam_states[ + err_state_server.imu_state.id]; + + err_cam_state.time = time; + true_cam_state.time = time; true_cam_state.orientation = rotationToQuaternion(true_R_w_c); - true_cam_state.position = t_c_w; + true_cam_state.position = true_t_c_w; true_cam_state.orientation_null = true_cam_state.orientation; true_cam_state.position_null = true_cam_state.position; @@ -1163,6 +1269,7 @@ void MsckfVio::PhotometricMeasurementJacobian( int count = 0; double dx, dy; + for (auto point : feature.anchorPatch_3d) { Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); @@ -1197,6 +1304,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; + // Isometry T_anchor_w takes a vector in anchor frame to world frame dGpj_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho)); dGpj_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear() @@ -1216,7 +1324,6 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } - // calculate residual //observation @@ -1299,7 +1406,26 @@ void MsckfVio::PhotometricFeatureJacobian( std::cout << "stopped playpack" << std::endl; nh.setParam("/play_bag", false); } + + + // Errstate + calcErrorState(); + /* + std::cout << "--- error state: ---\n " << std::endl; + std::cout << "imu orientation:\n " << err_state_server.imu_state.orientation << std::endl; + std::cout << "imu position\n" << err_state_server.imu_state.position << std::endl; + + int count = 0; + for(auto cam_state : err_state_server.cam_states) + { + std::cout << " - cam " << count++ << " - \n" << std::endl; + std::cout << "orientation: " << cam_state.second.orientation(0) << cam_state.second.orientation(1) << " " << cam_state.second.orientation(2) << " " << std::endl; + std::cout << "position:" << cam_state.second.position(0) << " " << cam_state.second.position(1) << " " << cam_state.second.position(2) << std::endl; + } + */ + const auto& feature = map_server[feature_id]; + // Check how many camera states in the provided camera // id camera has actually seen this feature. @@ -1344,18 +1470,24 @@ void MsckfVio::PhotometricFeatureJacobian( // get Nullspace JacobiSVD svd_helper(H_yi, ComputeFullU | ComputeThinV); + int sv_size = 0; Eigen::VectorXd singularValues = svd_helper.singularValues(); for(int i = 0; i < singularValues.size(); i++) - if(singularValues[i] > 1e-5) + if(singularValues[i] > 1e-9) sv_size++; - int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size(); - MatrixXd A = svd_helper.matrixU().rightCols( - jacobian_row_size-sv_size); + int null_space_size = svd_helper.matrixU().cols() - sv_size; //TEST used instead of svd_helper.singularValues().size(); + MatrixXd A = svd_helper.matrixU().rightCols(null_space_size); H_x = A.transpose() * H_xi; r = A.transpose() * r_i; + + ofstream myfile; + myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); + myfile << "nulls:\n" << A.transpose() * H_yi <width = feature_msg_ptr->points.size(); feature_pub.publish(feature_msg_ptr); - return; } From 2ee7c248c116d82b8466bc9374133df5d4a4ea74 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 14 May 2019 16:03:24 +0200 Subject: [PATCH 34/86] alterations at nullspaceing, jakobi changes --- launch/msckf_vio_tum.launch | 4 +-- src/msckf_vio.cpp | 55 +++++++++++++++++++------------------ 2 files changed, 30 insertions(+), 29 deletions(-) diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 38f93a2..c3a91d9 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -21,8 +21,8 @@ - - + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 2473996..fb9bdde 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1158,8 +1158,6 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) size_t old_rows = state_server.state_cov.rows(); size_t old_cols = state_server.state_cov.cols(); - MatrixXd temp_cov = state_server.state_cov; - // add 7 for camera state + irradiance bias eta = b_l state_server.state_cov.conservativeResizeLike(Eigen::MatrixXd::Zero(old_rows+7, old_cols+7)); @@ -1287,7 +1285,8 @@ void MsckfVio::PhotometricMeasurementJacobian( dI_dhj(0, 1) = dy; //dh / d{}^Cp_{ij} - dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix::Identity(); + dh_dCpij(0, 0) = 1 / p_c0(2); + dh_dCpij(1, 1) = 1 / p_c0(2); dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); @@ -1305,16 +1304,16 @@ void MsckfVio::PhotometricMeasurementJacobian( //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; // Isometry T_anchor_w takes a vector in anchor frame to world frame - dGpj_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho)); + dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); - dGpj_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear() - * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), + dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() + * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho))); dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); // Intermediate Jakobians - H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 3 + H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1 H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 @@ -1335,6 +1334,7 @@ void MsckfVio::PhotometricMeasurementJacobian( IlluminationParameter estimated_illumination; feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); + // calculated here, because we need true 'estimate_irradiance' later for jacobi for (auto& estimate_irradiance_j : estimate_irradiance) estimate_photo_z.push_back (estimate_irradiance_j * estimated_illumination.frame_gain * estimated_illumination.feature_gain + @@ -1382,7 +1382,6 @@ void MsckfVio::PhotometricMeasurementJacobian( count = 0; for(auto data : photo_r) r[count++] = data; - std::stringstream ss; ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) @@ -1469,25 +1468,21 @@ void MsckfVio::PhotometricFeatureJacobian( // of H_yj. // get Nullspace + FullPivLU lu(H_yi.transpose()); + MatrixXd A_null_space = lu.kernel(); + /* JacobiSVD svd_helper(H_yi, ComputeFullU | ComputeThinV); int sv_size = 0; Eigen::VectorXd singularValues = svd_helper.singularValues(); for(int i = 0; i < singularValues.size(); i++) - if(singularValues[i] > 1e-9) + if(singularValues[i] > 1e-12) sv_size++; - int null_space_size = svd_helper.matrixU().cols() - sv_size; //TEST used instead of svd_helper.singularValues().size(); - MatrixXd A = svd_helper.matrixU().rightCols(null_space_size); - - H_x = A.transpose() * H_xi; - r = A.transpose() * r_i; - - ofstream myfile; - myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "nulls:\n" << A.transpose() * H_yi < svd_helper(H_fj, ComputeFullU | ComputeThinV); int sv_size = 0; Eigen::VectorXd singularValues = svd_helper.singularValues(); for(int i = 0; i < singularValues.size(); i++) if(singularValues[i] > 1e-5) sv_size++; - - int null_space_size = svd_helper.matrixU().cols() - sv_size; - + cout << "sv size: " << sv_size << endl; + MatrixXd A = svd_helper.matrixU().rightCols( - jacobian_row_size - sv_size); - + jacobian_row_size - 3); + */ + FullPivLU lu(H_fj.transpose()); + MatrixXd A = lu.kernel(); + H_x = A.transpose() * H_xj; r = A.transpose() * r_j; + /* ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov < Date: Thu, 16 May 2019 13:56:37 +0200 Subject: [PATCH 35/86] minor print changes --- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 42 +++++++++++++++++++------------------ 2 files changed, 23 insertions(+), 21 deletions(-) diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index c3a91d9..eca1ebc 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -21,7 +21,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index fb9bdde..31a891e 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -427,22 +427,19 @@ void MsckfVio::imageCallback( batchImuProcessing(feature_msg->header.stamp.toSec()); // save true state in seperate state vector - - //if(ErrState) - //{ - batchTruthProcessing(feature_msg->header.stamp.toSec()); - - if(GROUNDTRUTH) - { - state_server.imu_state.position = true_state_server.imu_state.position; - state_server.imu_state.orientation = true_state_server.imu_state.orientation; - state_server.imu_state.position_null = true_state_server.imu_state.position_null; - state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null; - state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0; - state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu; - } - //} + batchTruthProcessing(feature_msg->header.stamp.toSec()); + + if(GROUNDTRUTH) + { + state_server.imu_state.position = true_state_server.imu_state.position; + state_server.imu_state.orientation = true_state_server.imu_state.orientation; + state_server.imu_state.position_null = true_state_server.imu_state.position_null; + state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null; + + state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0; + state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu; + } double imu_processing_time = ( ros::Time::now()-start_time).toSec(); @@ -1484,6 +1481,14 @@ void MsckfVio::PhotometricFeatureJacobian( H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; + + ofstream myfile; + myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); + myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov < dpc0_dxc = Matrix::Zero(); // original jacobi - //dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); - // my version of calculation dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); - //dpc0_dxc.leftCols(3) = - skewSymmetric(R_w_c0.transpose() * (t_c0_w - p_w)) * R_w_c0; dpc0_dxc.rightCols(3) = -R_w_c0; Matrix dpc1_dxc = Matrix::Zero(); @@ -1626,13 +1628,13 @@ void MsckfVio::featureJacobian( H_x = A.transpose() * H_xj; r = A.transpose() * r_j; - /* + ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov < Date: Tue, 21 May 2019 13:34:58 +0200 Subject: [PATCH 36/86] minor output changes, added arrows for gradient and residual visualization --- include/msckf_vio/feature.hpp | 129 ++++++++++++++++++++++++---------- include/msckf_vio/msckf_vio.h | 4 +- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 96 ++++++++++++++----------- 4 files changed, 151 insertions(+), 80 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 8a6cc27..ce090ae 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -15,7 +15,7 @@ #include #include #include - +#include #include #include #include @@ -182,12 +182,14 @@ bool MarkerGeneration( const StateIDType& cam_state_id, CameraCalibration& cam0, const std::vector photo_r, - std::stringstream& ss) const; + std::stringstream& ss, + cv::Point2f gradientVector, + cv::Point2f residualVector) const; /* - * @brief projectPixelToPosition uses the calcualted pixels + * @brief AnchorPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em */ -inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, +inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam); /* @@ -533,7 +535,9 @@ bool Feature::VisualizePatch( const StateIDType& cam_state_id, CameraCalibration& cam0, const std::vector photo_r, - std::stringstream& ss) const + std::stringstream& ss, + cv::Point2f gradientVector, + cv::Point2f residualVector) const { double rescale = 1; @@ -573,45 +577,107 @@ bool Feature::VisualizePatch( cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); - // irradiance grid anchor + + // patches visualization int N = sqrt(anchorPatch_3d.size()); - int scale = 20; + int scale = 30; cv::Mat irradianceFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255)); cv::resize(irradianceFrame, irradianceFrame, cv::Size(), rescale, rescale); + + // irradiance grid anchor + std::stringstream namer; + namer << "anchor"; + cv::putText(irradianceFrame, namer.str() , cvPoint(30, 25), + cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); + + for(int i = 0; isecond(0),observations.find(cam_state_id)->second(1)); + // move to real pixels + p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); + + for(int i = 0; i0) cv::rectangle(irradianceFrame, - cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), - cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), + cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), + cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), cv::Scalar(255 - photo_r[i*N+j]*255, 255 - photo_r[i*N+j]*255, 255), CV_FILLED); else cv::rectangle(irradianceFrame, - cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), - cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), + cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), + cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), CV_FILLED); - cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); + // gradient arrow + /* + cv::arrowedLine(irradianceFrame, + cv::Point(30+scale*(N/2 +0.5), 50+scale*(N+(N/2)+0.5)), + cv::Point(30+scale*(N/2+0.5)+scale*gradientVector.x, 50+scale*(N+(N/2)+0.5)+scale*gradientVector.y), + cv::Scalar(100, 0, 255), + 1); + */ + + // residual gradient direction + cv::arrowedLine(irradianceFrame, + cv::Point(40+scale*(N+N/2+0.5), 15+scale*((N-0.5))), + cv::Point(40+scale*(N+N/2+0.5)+scale*residualVector.x, 15+scale*(N-0.5)+scale*residualVector.y), + cv::Scalar(0, 255, 175), + 3); + + + cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); /* // visualize position of used observations and resulting feature position @@ -703,7 +769,7 @@ cv::Point2f Feature::projectPositionToCamera( return my_p; } -Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam) +Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam) { // use undistorted position of point of interest // project it back into 3D space using pinhole model @@ -742,27 +808,19 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), cam.intrinsics, cam.distortion_model, - 0); + cam.distortion_coeffs); // create vector of patch in pixel plane - std::vectorund_pix_v; for(double u_run = -n; u_run <= n; u_run++) for(double v_run = -n; v_run <= n; v_run++) - und_pix_v.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); + anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); //create undistorted pure points - std::vector und_v; - image_handler::undistortPoints(und_pix_v, + image_handler::undistortPoints(anchorPatch_real, cam.intrinsics, cam.distortion_model, - 0, - und_v); - - //create distorted pixel points - anchorPatch_real = image_handler::distortPoints(und_v, - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs); + cam.distortion_coeffs, + anchorPatch_ideal); // save anchor position for later visualisaztion @@ -770,19 +828,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) // save true pixel Patch position for(auto point : anchorPatch_real) - { if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) return false; - } + for(auto point : anchorPatch_real) anchorPatch.push_back(PixelIrradiance(point, anchorImage)); // project patch pixel to 3D space in camera coordinate system - for(auto point : und_v) - { - anchorPatch_ideal.push_back(point); - anchorPatch_3d.push_back(projectPixelToPosition(point, cam)); - } + for(auto point : anchorPatch_ideal) + anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam)); + is_anchored = true; return true; } diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index bcf5ee2..77a8099 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -14,7 +14,7 @@ #include #include #include - +#include #include #include #include @@ -38,6 +38,8 @@ #include #include +#define PI 3.14159265 + namespace msckf_vio { /* * @brief MsckfVio Implements the algorithm in diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index eca1ebc..9e103b7 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -24,7 +24,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 31a891e..2e8e574 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1238,6 +1238,8 @@ void MsckfVio::PhotometricMeasurementJacobian( //photometric observation std::vector photo_z; + std::vector photo_r; + // individual Jacobians Matrix dI_dhj = Matrix::Zero(); Matrix dh_dCpij = Matrix::Zero(); @@ -1262,9 +1264,31 @@ void MsckfVio::PhotometricMeasurementJacobian( auto frame = cam0.moving_window.find(cam_state_id)->second.image; + //observation + const Vector4d& z = feature.observations.find(cam_state_id)->second; + + //estimate photometric measurement + std::vector estimate_irradiance; + std::vector estimate_photo_z; + IlluminationParameter estimated_illumination; + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); + + // calculated here, because we need true 'estimate_irradiance' later for jacobi + for (auto& estimate_irradiance_j : estimate_irradiance) + estimate_photo_z.push_back (estimate_irradiance_j * + estimated_illumination.frame_gain * estimated_illumination.feature_gain + + estimated_illumination.frame_bias + estimated_illumination.feature_bias); + int count = 0; double dx, dy; + // gradient visualization parameters + cv::Point2f gradientVector(0,0); + + // residual change visualization + cv::Point2f residualVector(0,0); + double res_sum = 0; + for (auto point : feature.anchorPatch_3d) { Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); @@ -1273,14 +1297,24 @@ void MsckfVio::PhotometricMeasurementJacobian( //add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - // add jacobian + //calculate photom. residual + photo_r.push_back(photo_z[count] - estimate_photo_z[count]); + + // add jacobians // frame derivative calculated convoluting with kernel [-1, 0, 1] dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame); dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame); dI_dhj(0, 0) = dx; dI_dhj(0, 1) = dy; + + gradientVector.x += dx; + gradientVector.y += dy; + residualVector.x += dx * photo_r[count]; + residualVector.y += dy * photo_r[count]; + res_sum += photo_r[count]; + //dh / d{}^Cp_{ij} dh_dCpij(0, 0) = 1 / p_c0(2); dh_dCpij(1, 1) = 1 / p_c0(2); @@ -1320,28 +1354,6 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } - // calculate residual - - //observation - const Vector4d& z = feature.observations.find(cam_state_id)->second; - - //estimate photometric measurement - std::vector estimate_irradiance; - std::vector estimate_photo_z; - IlluminationParameter estimated_illumination; - feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); - - // calculated here, because we need true 'estimate_irradiance' later for jacobi - for (auto& estimate_irradiance_j : estimate_irradiance) - estimate_photo_z.push_back (estimate_irradiance_j * - estimated_illumination.frame_gain * estimated_illumination.feature_gain + - estimated_illumination.frame_bias + estimated_illumination.feature_bias); - - std::vector photo_r; - - //calculate photom. residual - for(int i = 0; i < photo_z.size(); i++) - photo_r.push_back(photo_z[i] - estimate_photo_z[i]); MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); @@ -1379,12 +1391,13 @@ void MsckfVio::PhotometricMeasurementJacobian( count = 0; for(auto data : photo_r) r[count++] = data; + std::stringstream ss; ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); + feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss, gradientVector, residualVector); } return; @@ -1481,13 +1494,11 @@ void MsckfVio::PhotometricFeatureJacobian( H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; - ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov < H.cols()) { // Convert H to a sparse matrix. SparseMatrix H_sparse = H.sparseView(); @@ -1663,8 +1676,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp); (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp); - H_thin = H_temp.topRows(21+state_server.cam_states.size()*6); - r_thin = r_temp.head(21+state_server.cam_states.size()*6); + H_thin = H_temp.topRows(21+state_server.cam_states.size()*augmentationSize); + r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize); //HouseholderQR qr_helper(H); //MatrixXd Q = qr_helper.householderQ(); @@ -1676,18 +1689,19 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { H_thin = H; r_thin = r; } + */ // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; - MatrixXd S = H_thin*P*H_thin.transpose() + + MatrixXd S = H*P*H.transpose() + Feature::observation_noise*MatrixXd::Identity( - H_thin.rows(), H_thin.rows()); - //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H_thin*P); - MatrixXd K_transpose = S.ldlt().solve(H_thin*P); + H.rows(), H.rows()); + //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); + MatrixXd K_transpose = S.ldlt().solve(H*P); MatrixXd K = K_transpose.transpose(); // Compute the error of the state. - VectorXd delta_x = K * r_thin; + VectorXd delta_x = K * r; // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1722,7 +1736,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { auto cam_state_iter = state_server.cam_states.begin(); for (int i = 0; i < state_server.cam_states.size(); ++i, ++cam_state_iter) { - const VectorXd& delta_x_cam = delta_x.segment<6>(21+i*6); + const VectorXd& delta_x_cam = delta_x.segment(21+i*augmentationSize, augmentationSize); const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>()); cam_state_iter->second.orientation = quaternionMultiplication( dq_cam, cam_state_iter->second.orientation); @@ -1730,7 +1744,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { } // Update state covariance. - MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin; + MatrixXd I_KH = MatrixXd::Identity(K.rows(), H.cols()) - K*H; //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() + // K*K.transpose()*Feature::observation_noise; state_server.state_cov = I_KH*state_server.state_cov; @@ -1744,7 +1758,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { } bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - + return true; MatrixXd P1 = H * state_server.state_cov * H.transpose(); From 2aef2089aaf0aaa1a39df98566f582e06cf305f3 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 21 May 2019 14:26:26 +0200 Subject: [PATCH 37/86] added undistort point --- include/msckf_vio/image_handler.h | 10 ++++++++ src/image_handler.cpp | 41 +++++++++++++++++++++++++++++++ 2 files changed, 51 insertions(+) diff --git a/include/msckf_vio/image_handler.h b/include/msckf_vio/image_handler.h index 6e8286f..d664be6 100644 --- a/include/msckf_vio/image_handler.h +++ b/include/msckf_vio/image_handler.h @@ -36,6 +36,16 @@ cv::Point2f distortPoint( const cv::Vec4d& intrinsics, const std::string& distortion_model, const cv::Vec4d& distortion_coeffs); + +void undistortPoint( + const cv::Point2f& pt_in, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + cv::Point2f& pt_out, + const cv::Matx33d &rectification_matrix = cv::Matx33d::eye(), + const cv::Vec4d &new_intrinsics = cv::Vec4d(1,1,0,0)); + } } #endif diff --git a/src/image_handler.cpp b/src/image_handler.cpp index 5d868cc..cb98426 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -14,6 +14,47 @@ namespace msckf_vio { namespace image_handler { +void undistortPoint( + const cv::Point2f& pt_in, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + cv::Point2f& pt_out, + const cv::Matx33d &rectification_matrix, + const cv::Vec4d &new_intrinsics) { + + + std::vector pts_in; + std::vector pts_out; + pts_in.push_back(pt_in); + if (pts_in.size() == 0) return; + + const cv::Matx33d K( + intrinsics[0], 0.0, intrinsics[2], + 0.0, intrinsics[1], intrinsics[3], + 0.0, 0.0, 1.0); + + const cv::Matx33d K_new( + new_intrinsics[0], 0.0, new_intrinsics[2], + 0.0, new_intrinsics[1], new_intrinsics[3], + 0.0, 0.0, 1.0); + + if (distortion_model == "radtan") { + cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, + rectification_matrix, K_new); + } else if (distortion_model == "equidistant") { + cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs, + rectification_matrix, K_new); + } else { + ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", + distortion_model.c_str()); + cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, + rectification_matrix, K_new); + } + pt_out = pts_out[0]; + return; +} + void undistortPoints( const std::vector& pts_in, const cv::Vec4d& intrinsics, From e788854fe8137e88a8488b62a67ade93a28eef24 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Thu, 23 May 2019 09:55:37 +0200 Subject: [PATCH 38/86] activated gating test --- src/msckf_vio.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 2e8e574..b233848 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1493,13 +1493,13 @@ void MsckfVio::PhotometricFeatureJacobian( */ H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; - +/* ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl; myfile.close(); cout << "---------- LOGGED -------- " << endl; - +*/ if(PRINTIMAGES) { std::cout << "resume playback" << std::endl; @@ -1758,7 +1758,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { } bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - return true; MatrixXd P1 = H * state_server.state_cov * H.transpose(); From d9899227c234a18c4c491552440880709d4d2f69 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 24 May 2019 15:00:18 +0200 Subject: [PATCH 39/86] added scaling changes to dI/dh for pixel size --- GPATH | Bin 0 -> 16384 bytes GRTAGS | Bin 0 -> 32768 bytes GSYMS | Bin 0 -> 131072 bytes GTAGS | Bin 0 -> 32768 bytes include/msckf_vio/feature.hpp | 39 +++++++++++++++++++++++ launch/msckf_vio_tum.launch | 6 ++-- src/msckf_vio.cpp | 56 ++++++++++++++++++++++------------ 7 files changed, 79 insertions(+), 22 deletions(-) create mode 100644 GPATH create mode 100644 GRTAGS create mode 100644 GSYMS create mode 100644 GTAGS diff --git a/GPATH b/GPATH new file mode 100644 index 0000000000000000000000000000000000000000..de480554594697feb169435fc2d511232be50b29 GIT binary patch literal 16384 zcmeHNJ#5=X6uzbn&_SpyOO|BIw$xVX4;oXU$fOC7rAQSZXp$PSQKUcsmZ4~ijYtY2 zWjKL{3>h+H?9eeo#ta!UX2{SnqlXL`GiKeqG+pQffGQR=6&yb@4mb5 z?kM=SdXjz2;M2e#{L>iU$lp={sen{KDj*e*3P=T{0#X5~fK)&#AQg}bNCl(=kP7@S6!=J}@&w$!X1RPW|E9j3f0}<#-z>kZKQ5LQ z9xhxhyjys@e82iv<+sZDF)UszzFB;QUR%eR6r^q6_5%@1*8H}0jYpgAfZ4G z{^I`&Zy3t;&du$gI=?dg0zLZzk@|V2w3NNT^1F7+GOWSC7!3V^Jq#T?P<{`5kNE&= zz;zO?W5K|Oc+9zic>Q}JB(THK@pc8?DnO=qlOTv6^J&C)4)`qJ+i`YVmg!nO)A2SO zcLetv_Gsu^8=i0PheO8;94k=nnr6`QOPr$ks@h?#QZQvi6)*W-#R`zKS zK8tG=8}xP*SE4u-;)0>oas%tmjw0;ap<+nJC_IA+ymly>8zg z5(l;m9BBZ=Vmp1&qFtD-!|=c&eLEyB#0XrIViO?XKn(dnL!V*4cv|q7We0)JE8`e} zZx#p`cuw-IhQ6A?mneWS#Afp*z`)3{G=3WT4EaZ)(+{Z6_*~$M8vu*X8*nj={Nb8+ z?AB$+>u!yl(B2xD!@yQ7dnKG5FhC}SAf>$_!{LwzfhQKhCPD@Mc_t^$UO_B!QzORW#U8sxWR5~dkE%*#rlf3 z<16rhWMdV;L>kWoX=gYN`DonQ=p%$)GF^LUUbD>r#BgmdoRFfI1kTw&;2Dow9qVkP zpOE)5mv;v7T9P*d2t?8PPV>HvzPftlQs??sM+x^sXcX2hu>p{LettvspkEx%>W^R_ zoh%3O-Nb4=hWKbq<_l;G{%WJYjtEnFo2Rqur~NqX}+o95oRTi%gm7-2$=+ZRP<#! zFC5Q|AT&e!xcMmpfeY(tz{OPbWttaGe)I8)K;Vjb(YJ%Z9(8>>0>`%oxfnbytp9PV z5EB7aJSZaXcT_L>bT&L8C9JRz#=eD^Xbqe*NTmZK<||M>^lx-R38DiRJkQ6xiAI%g zgH-S56py~_nPG?$d}DrE=T-D+#Ge>Qe`q8Eed>2I$B_ZR z?6VV47wLue%}9eMZr;Q-M!i0PYpw{sjZ&Tl#-LqOawP`@=YNuavcHK#NBbz&Pv;-B zYexOklj>B8e_AIog1--9o)%mT?VGBfpW+fPf`VWhseEyNvLop=kaVN>477KYcWA>a zt1u$v6n_FizMYIS+BQR{nLuQ&u9G3B8rV$xD6S$QCtJQ T&-kTtW9!P#*O_qNfW7)(V%EU{ literal 0 HcmV?d00001 diff --git a/GRTAGS b/GRTAGS new file mode 100644 index 0000000000000000000000000000000000000000..689414259375a076957e3fe64c5d2d8de58e59a4 GIT binary patch literal 32768 zcmeHPJ8)E2nm)!n1nALvzgmx7dtGczd#mrg{kZL=YS9R6X$OG}ZPr^gwW4V?sK>2t zZL7uL83*1f*De8p0|EjE1P%xY2pkYhFaZGp0Re#n1q2Qp5D+*ZaIoL^pK~95btS~i z?#ymg11(zj{P&#y{O3Ra`CsS1VjWU%EBu+|kItVC1v(VyP@qGB4h1?C=un_Tfer;a z6zEW(LxBzjIuz(ophJNU1v(VyP@qGB4h1?C=un_Tfer;a6zEW(LxBzjIu!W-NP)LP zNb~!c+MfAoW_#xL%*IS&?Dp8ruIE!-M;8(|CszB%j=vi2N`5!BJbLnw8cy__?z(m4 z!SubZKaLd--5mI_r)&JXgUNxP5C35>G4)0757Q4O7N$R)nw|b}^l{Ifq2gfI%$?!0 z$7YkyjxUV2j$Q6r8yo8T>G(GXlKtD`>A|!8&)<4?@@8UVIz5x3uM8a47lq+L0HNkNb-w>4SHsGDD}vejKR1{rpgIXyL%8lkX<(9lz82 z?c|dqYsptfQimTVZzt~^Ii0-Gy>|G~Q1`^EgHO8`29k$fcAq%%_1kL$Cnvr*{=Dn* z#8;E)@wL8#hr0*w_B`x;c0@6^-Jbqo=%a@dy1syuEPb>E!KWC#P=pb&uXS@G9}N_gUYg;pLuh4nKVBtHD~LYx3Z+?TNcb z7lt>E*2b5+PaRKoEgXG*v^9LAXZi5suE$5e>3-SU8hY^div!OGZ}q1}o*a07_*F8r zEU{m^@N|SN8(UXu)y*}Pv1U)zXESalWoOeVD{J9LL8)$lyhpf;tx~J9c)IxcM#X~# zgq10zoI=XB^C>HXSMye?V5KZ4mCY$Nir0?u-O}pnhm}%mt64dJWwX+}UTRev^-Zsv zoCT0^e2x>gve~MxW2lu6o2B*2VzXIVt(NL#Z&1t5=Lwd;NBZ7qR#vO!*15`W1w+$m zfw)h3Fw1L=R%5-=YF5hvG`Dqay;8@7fQE-f)>4N7-;0mhYE^32YnB`azl9V@=3;vI!#yjwm7Cagl?#o{#q%C+c`RE_IkN`= z!#$_gOB-IBopI0?C?!3C2CA_|o|8GW^PJ%p+ffi3eSneiYK_wBa*6x4RcUUTZh@(l zb<}#JOgP!&*D9@IvsB-_+GwuNUovo@T25UeUP6=2)~ac+ZS}!<;=%o_ARMk$R@O@O z)mo)#AcSjE8R6Iv-%`d=2Kh41FfTw{7mS?)ds+Fk%umK#-z(`s;fIpb+)()A~&bZ_xELr@%%$t1)?%}==*}^@RljmSNFg1m#Wy>Cv zxp3cj?G`x`J2nr-w^fz=B)A%W(qL?AH3!fpa|G=#RmxV*UjWlxCX2H>fcX_f$Cc*A z*5|cK6w)cEYJ+R(_2&a{Ee&oM^1WdXrrFT4w%GW13k=z;Q|>x8XPF0+5pUr!gSRwT z7D=oIy*|!w;9RKwPGt6cqkgq|&Gg_{>Vs2t;zGVx^K(o3Akh6oQe_;qsmKe`ZuXS_ zGPaQVy;ALbCm4Xa>nAvVWDhSlHX&O58CX_Ytyg?{%$L?>{EIl}$e_|yu0X=DpD3T{ zk+SMFw9F!WW9z(Fq9A|KTUACRYH%mDrzcAYsGG{0t}T=j-|uH3O0V)TuhF?$=Az#&WA^+OcLIoI2v+)2J`v z6!e;Gg?$|}zV6w|D((?kYsjk7#`)F8x-Z%7ESc5Gq=_EaTHwm1TKP<~*}y&lTPqD@ z<+7@>%)T@~LK-$p=m^_M6GMYRkAbwl?#tI^WwX+XjAv!D9FMX=#)E7yh@=auxlX#v z^Wc2>s9CyRZGAphU;SmZwCQ1#$pyOD)oOk9LS+@lf2oe~G_!h;(Br@}>{I$(Ds31x zHaO=T^`3Tg%Nxxw5%g ztzYx7bknL-_uLZ#+C(&8*icuT`Q+3%j6K&vP?I*IG9tecdqheupB5oPy^+sP1VH4aRG+l zewFbznp^dXw?4nzx_TADME7RfkS0E_Zj$xd6k3CA0cUQh>eF+*RL8-&)VL0c{|k7a z@mr7X*@C+M_vF>c-ou^~u6Namr4s2aa;IG<@y3GwK?P9#p8XJ3+Y9a71GNPzvT6}V z<`~vbOiyV?{obG3dcahlY^(-b%!W!(+bmKBh#Xj4`XsyqT}X7wEE(e(HeLN%5oU!? zzKmKbQQk>A^&dSpPHT0JdW@mJ@aIxvwNmpa<=QegX$McW)}nl8ur^pDz&|OQT-M6Gfk{je4zGuUw!+@#cn8GT^NcT_Xrt z1y==fa6|9WBB$DFfXW^r@;Fl0N<52Xp0cL}YUi%T(MP@!xU1R-jErfgv+AlpKab_x zSjWw(cTTO6KcyefW;Hwmg=C&{C*QQ$siwW)!aT|11EKjko78(HtC}JQg}(y5P&Y%P z@aGh@KTQX~xUoO`(;G1(I*8^Qo@W_3ajAvios~T-*iGCT6 zn^itvApM|UYH(LfD+(>hCjQJ(OwlNVia~T4+GPWJY)W?9(v6 zZ)fQNEK9n@8vyZnH*8NCIQSA;+|RNW!C7bUgyM>CyU8b+qDxggN1w#9KTezM5hx3M zBzBRkfyf0-AlFeF{&~}G2N}xZ=)%7D^@8XgHQhVv)T+NmW~VfoMw!(;IXJm4(0LP) z#fwXGADvlQzHom2%*Bi6FHj%SGgHu|ckT;4V8K=u&Lzsj(u$Mbmpg6ti$NqyW3o~w zzViMKo^r^aaI;iCChxq1kMNE>wly1&LBt&(KWUYNX|&xWeTHeNYxiD^m|xzO~8|YAN1_Nn$!LaTU-`^5)%$Y50aYS+m% zl$A}Z*{bN|k@+vp6&F@MDK4J9xU#UktTZ@rr`;#A6Y~Byl`_0q+Q2C`AdI2BDqCyl zxh!mK%7DCT5MKNT8l+9XHP!>B2oE8wf1v=BC-Rc`33l+3ypc!$P+C@&$WOQM5q>hp z0Vsw3C}rqxviL=QIBEKvM32HF81?uKbS9j99Pt>@y+j?%#?Op=qdUmK2Z|WoY#A~v zya8$Hf{R0k7C&%?(Om~0mpTNLI0sdRR%(a>(c;3yck*g>-rw6B)f<)CX}AdKxSaDq z;ZqICx$;`2{Mk}N3`s*@+fsGvkN?Fn3_r>Ab99VpY03EkXK?_LmoTR^9=X)4fGHZl zW1MOZ&y0@_|H*G@^a6(wWDJOdZnV=VS0EX(m?&mXr=v}&E-o~Z79-@YDev+(@`Ica?Xo~m8}fiH|6-}9XZ$|Y;8Qg*=4Iv8bQu3#axmc@htyk+pW8Y`# z4GcxUd9v)=*8#mFehp2C49xVMXRr*Mb$}doj%j?_byJO47HsCRB&Y*^vU%{9{IPe9 zhv=~>jLGk^H-6(=-qLviIxn2K6|SM|NqCvbRh2~@8~A7|;6W>0NV%3Pz6RlA`Whsx z0mD>eixMBDj6)q!w}Gj_)==vyx8w{pa?89b=St)pGDnc#eHbX#yIp5~0v%{eZ! zsP9-&eK;y;c%+f||9n}h!Cgpj)%-`{JE3En?W$p*iLGK$qjX{T+ccKUm^|hkS=rl2{Ur9#V-; zb?1HgNMEuid_RMJ4Y5*2NoCBKc>+3^r;Gz-i@3{P6o-X5C3JUiRpTsy(>V@*9V&2x z_h(mD{^8P@3l|s9pHrdlvsrbTdsOx*pUS= z#2Z)m=m39U4)Q;~5{}66n#)6b_KPkl3W zaOy?!4?W)|SCb3LRPxp2_mf}rem%K7d1CTmubTXR;`YSa#O1`viLr?n<0tx_j^7{u za{OlE)A7uB_xKNEkH+qdt&N=;8yb5t`gru-==0vMMps8qjwX5&qj&nAkBs%)A4&Fq zG18qlJ91)VyE`%Rr0=`L_V9PZUk}%Y7lwcAdw2L{;?>aEo~J`@;@;3t-Cqse=~*2* zJv2Mi)u)D@4?Y;YJa}?&cCgj+WFRx}p#OG%tN*O1DVSpCPlo~>3UnyYp+JWM9SU?P z(4jzw0&h%#|Hy_jXW#hs&I^A53ea~ZwlA%2ogZu4nY{S<j)eCP+&C@Y*A{|Jomuy`4oVEWS zj(<4#>8odMApcrc9`K+KR)>y%x+Gl5@s;)8BL~6Ez|DhBIV=X_8{#+kxYf91i0J?m zbQkb^#9!=F0dpihg{Dz|u>o?@+QX!fD@C8A&?`1~z|aQ4Z^8*beRN^&(qi#nSLWvD zkr8%bu1Lp+fy-+Dk*rg^|Hxoq=W}psajo9Nr_G0Cx}x9Q%V}q{4xykwF{j7 z=3oi!_Y(`@8VOC>Vwk6*1pAad72`8!nF}$jZ^#aFv7{Y#_K&E14ulZ{fSQK>!3U1ScK8z?#!w6X}XWt zCiej8s`kh&BMk9xhtXO|^!^NGgXl*4$R#A~FW7GsR(EY9Zui>zU6ivzt9JW9(=(LM zv5Oty2^~K#dN(+&WwGhpLuTH`+sVV9{Z8K zVQ|!8^^C-n|1aij5#RmB`6F{P`3#om32&O`NBJha6O$V#YiI7h>;-2u2gz&ZKIEQO zugUEpoUBh2PBV<;knaW)zNf`Lgin3ngci46&I8-^{NnGLv0e0f|9vBdWAe`^|G4MX zKJ(=`uE2+4{{LI?e~u;d^zsPi=8JwB0ly>uPkX}40pS1Xw;mnS@&DFLs()c-A@S~v z>V7qy>ivHD{`8mA#p(2P_wRsdmJWD=EW_rF(ZX}nJ>0~1L za`NfqM&kbDv*TY*emeP0B0ZUyd^PcG;{L?tzMB)@_gtR1*Sjz=JJB`qa{Lc{kH=N- z-SMA}-54(-M__FH=dtI=61X>ZYiwhzIOdKe$JE%5qfbYfCvfWc!f0wVG5S^C%aMEC zPey*-e{bZEy*EdG=({}f=vZpxX3xQq?cqNTAMF2j_~!7ZJ&zN=9!?Ku`nra2cGvn8Mrsl8aO>L zJMeS=v;K$uU-sXXBMfBf{OM4jLxBzjIuz(ophJNU1v(V?D^h^>pTuXk+x;gmdQ?Cd z60o#e#8%x|CqP5mxG@N4KJ|2gYrorXU~ugc->5(TCqAsLqZ{%%&I+@b@m7Qdk!8#a zNvsLPpZqS_%JM%$ue$yYt~lVD3g195R>Fb&1s`<7@T`NI7s30uorVZ0f_n%Y=WMo{ zo^jXMzk3kexro#i(3m>72TMCgY^7c53TQ~wyTP3X`a2ChcHHgPa2Z@Io|n7k0w?!K z{5#ef#Kv7q+)tyQSZu;g)Uv3})vwhmANv(C3?&g5Wd$MJJZqK+D|kllW*Gz>R8-i1 zy#RwPuG$d~xswxH<3z({T`me@*SOz-E4bp%6&o&m*VW99Pm_5LbJHK0R~;%xN>~n#pSd}-2iV=83q3G*D9uNfB}?1I{Onp zS?Er=xvQ_itO71eA*8R$a)(!7LwE8Gkc)eCj6t_Z&jLQ7dyt_!P8!;kicGkZM;8!o zDEPX zTD9Nz*8sSjkI@BP(m~+DwT;4g6uBRCq$qIHF2$EicU*t})K>!si z=1a8E1txs&Ujt-_oj-ThRMO9#Ebd^`d#MHnw~=_gfdU7Mgd1kO7;3ZL7+>N36yCTRsS#lCaPv$Iv1LyPTp*jw zoiPs(d)~@hn4aXKL}2~9V;(d_A4@=eOqs`XAjEf(yI%SYf779w0JCBWYU>((rS#+Y zb*40EP)m=jh;`r?LhoJgbQ!GPpvE2jcEU?>HS5sk`}HFCl=xh5iK%PFwUG1hHTQH0 zCw#X9PP}(gZ-6ke?{>iG8`Qi%P8jmyj+%74)YT*Wo7S)+AH?9%c4wS)l=-ypb>qv| zwLSKKMtC8Bbo>5u^u0d^W3}x|?SQmdbE^Hh_A;{u+8(mo8%%1x`15Grhi&)62_x&@ z4r8#>b)6`Sg=Vp5n+DB*uU`Wv?snGs^6_fdhYaqz){%eiyr%w-f9?h{G;~gAe{1wE zEZ-sTg!Xvo9X+3D?JSWW!5D#@*3|UxY&-H1V@&{XEWHkG*%zBFb0t zu-Z+}Hg580odMH#^z1QwqifMnUWMVJb$ht(roFvhsOg3ES^}7T+}{tMwYm=*(R#Jl zoz7^R4w>V=C+d>X4iH}3<=(BxN3{8QLXp~l|C9AOg-*mn2VwmBF#dC4A8O3abv_dw z4Qk}*pfZvMW7;&OU^m3H8Scu(w&|E%Hm*&h2`xCLPur{05!0rFI`Q>A{Cl)9@99m4 zJGL=xdP#R_^X?9lS2fMT^`1038Qhs=yTo1E?$jD4GxebSvtXwUssT-{x&UtPT3Y@d zj?)Eok5U_oXA|cwbwl-jm3jj0&o$$)ZVsPQj=iqVX#4v*O2~R#{&^5%hZwY^ei`?U z;1$u6y;L;^wRZ?3dQAkz1lM6_2X#tieIjsrJY8#xIcs{n;0`?r!S`g0UGC!h^9l4< zUne!xn6r2Ot9rYHd6Uf{Hn6^4gaqZ#y!m;i8 z^0Gr4A+B9xsO#skJ%XdjQ`I$deOo7NJDWvjov8SBecr(lAp1(zC;Yx<0KaZ8^%Obx z_F2b*@S0|AwP}s)$%}sernPH*U8&s%ViwcEO!QeQFu=e70|N{UFfhQt00RRI3@|XjzyJdS3=A+Zz`y_l0}Ko>Fu=e7 z0|N{UF!29H3_O+rTEFL#2a6Au9xQ&mG_>^P(rZiaE!CF3U3z=z4@=#p50;)?`tYeQ zCO#eCow`3aJ#y%Y+LI5)pP%{u@bLWI;mwJy$=8ODKYi=T4;H@o`NgMJpWaw}F!{;+ zhohfNy+5@v^3Cig$G%vcK6+>N3fIo4_%zeJ@Mt_7ehZzbZ3Sq?#+Mm_}!_M`L&;&onM{&>}TEC zCE|`pB{N{;_%$P$5sx#H~;+H(9+QO z-LW5zoF2OQ#1Ds#58Zy^=G^eY^yKqLA3XEnGZ&vJjURs|NrqXI5v@^szp>ug?>6>Y z8&}s`8%ZI*d}3$0UaYN@>MP}ya*|&@LbWN?-S6(TcdqbvtGOL2=Ih0kV!60d&aLE& zxs_V7vs`+ds!yx_*2Y#hc33SZPw|n)O)l>>w_A;N9IR9-tdwdigZO%@uDDVuB&(e#X+EhwHe1b&)?R>;nbN4FmianET{CzMCmXG; zZnLq|*^8L17whV~P)QEa&Aj>o^P!`1olyn9{Z+{G8wlQL@2l=vuN%>uuQ6JlA(=hk z+GOq5e6hTeFI!1{g+B5X{V5dbO86;(&^-Suu%;EwXIcHey58JsreIX)p|%3z)RUU# zt7=uTx!XFn_PgzEmSN-E*4EYS_KsmQ%pzYaB*o|QqjYQQx_@D6-Wxzb88$yc6xBG&C(?KXBpKh;`wr3$HnlV!?9L&=$QFP?e1@zTk^KGQgP{@jZ% zpE=!lW%bO3lWVVBXuN#pZ$rysJ$bIE`a^01{<_Vb^;Yb*3dJPdrqf- z0n*l58zSD0FqU%Vm6Fr0)nyuu-S+hq###v&le*U0%3D9x3uM8U=-7A;G{AKG?e#cN zMXOeX^h#oyd_FPw7uDz>Pl%#f4agFrhgKjtmMJ+*RpXp z*qUsvcXrmBk&VkW`pXrPt0^yw7bQRf&g$?1OT}U>pAgL$-M-Y^`(1_y@+%c_L2kwP z0Lo>`s=LNdvVEW~0`Pjaa){fDn?7Xz4HcNhoV)5LKyY zD*T;CI8|0^RX{Hy6~Pq9X5INad1be&d90{?D)S>|SX*I1$OT3GqxGUcsGR2T5o_qQ zE}0LUjoPnI6+`QCnYFC&vEZ}vt@h;>(kwDesm96`lD25q`01r4ym~z}uLEQ~w^FIp zC{DB|I*+!0AKTji65_>T8CsGI1pNzvNhi;Z$?`Rc(8( zvU8;iT15D;rloQrVIavf>pv@!SDV_DyS*m$a*}jyKe4&(ZFa)M3hbFM8(`Cqh_NmG5i zq%k9@*3vO0A+&|-AILw`vDWdAbgX`Vp?;&JkdgumunlPdpfh1t0<#j;?3O$GYGX3! zTAV*XBCe91D@j^*H7%1ws3zB1=T&AhIbt-h8%)h?QAnu?29SuzXNF4qdL#5zLJAk2 zD<-R(9hJ@SlSF+YFFrJtMtDH(czdIhbl)gQMKYvtxUv4H)kl=dMP|+Jfk4)VNJXz> za_+YQh6>uEHZ~3ChzBp~QVsP9%aP<)1m6ku-Hx!c3xnER&9phzR!SZ4`o;%`shhAz zqRVLu;BUH2cGQ>g(PrqYfGkvH6*HR3MeoU#I2PP4Tqds?t(r~}Ij;t)rChRN!bCCI zYe_yCuXKA1T0;7`e($o>%74>Z?{@Y|QAsUm7es-SOc~?HWaYK?{?+DI`}ekcqH0}< z8wXn7*)Yyuc)h3y=nzn!CG{ElF{msUu z=KAkq4Rmn$TB26iDGA*#)^7XHe>mTI;+5rxW@rOVm=1y0c393|;` z+TYgsNgP+jxF){;igk^+_!||2WfpQkj2(jbP zo3d(xG?UJCT;uLa*$;Cr!&e45M5UI@f!-z7tSR%^Q#;9dj#-&@fK{{>l72Q0l8@gE zM*_n4SUQl5HXdNVf8pHu^Jh+nu`7VtJI2_0zo>Cu4&~CcN{B@r6Pc$&DCVj0y8+7Q zVtXOp`OjdG>;o;eWH~PyT2`OG3VlYoB`j9NDCs=QDNXsgI|8X4lm#f-x`|)Hx)tKO znK-9A6C-))K%D3JKh$L4NKll^LQbW^*{&d-HGHza#Xh8@G6Q~e45eyGTLT1$l1q0L zenYyOs#_60P3}woVs}iB+_f6SE3`{jWB1oTQ4~Q1$qOypi%j;O4AU#Jg>fWUsla!} zdQKau);VT|`gL~T3Y)X_X>zH%*WB6P%AR3#U}39;0!-o17Lga+ z#u&6`*UzICp%n3@v#iad+v%b|WlxK;{0ORI0@8F}$m-pQa=u&{ENyFL;!eYPv(-qZ6an2WTu_lAU&R;wH8iNS+>wf=f@^?riw!OE_+ z_quG)afAXe2u{P%uFj+DX{U5q7f*Vh2S(%|o0c7fCaFA^mhJAfvE;Slc=@Diwj}Fw zeXF^@pAKjHl6{Wh>IyAthJ&2xsrLDH=NfoyyotEvRP5!Yobij5uQf!4`YRtwt5P6R z`f~f{m1cV<6c>%n4wPSNHv!5ANA-99FkKnTsefKZK+Ed6l26-aXxHm7G-{1#*Ko@o zkJ9BuMRo<+s?6Y~tk8~SRa+1HTsEF`%vvSEs-QM@ra9BT(u&hSBCxH$kivq#5v{ag zz-n@GXI*_-zo$;VWXHKMBqz?CXwmvbj$henZ8Vx&TOI6A9ca-}VPmX_->r`=h&2|K zlHcodqtk!e+UtbR*$*Miq^ZxXO*?Cf^lmHFB%hWC8QRFK0K3(4$|tfDcJ`#_A!-k`Az9aIiDC;XraE%o_a<8MODJWPz1!H+N$hhFYT! z$r$T9!*_-=EK8yr!%IB9Hg>LV#X&hEAi?dNIz~P0%#lJ+8BiZ&7nG5VBJ2N;*;XCq zay}^(HD4PyV+ZRSx@1G=v?p2L2koL^{`q9HlhRejChEngR=;np3NOYVnXv+nu=t}f zSCX#AusRVBu>!ggd$GYVPMx4rrK~~+W<@idh+_4VsmD&?kW}bL2Xe1h?(tC2;R3wy zQ1X1rcWyBWGp(2%!C*97lYguQK-=ocnCw39RZ96}XI=Df^kI8s>{0~MZVG1G2d=f? zsdo29lCMg%N$a#d@Z3&2rJ?5fI^Kvj)?u$y&Ds8J?IjDWCAzxQVWd~OHd*;&b+GaC zrLpysxQ$HsveI%TIVHJoWl_#pNnI+4*`Y((^{xpaD})_S*q(1XkMp{ZR*@kXr9oB? zZANT{DQsPr%kAx}arA0}{7TE$8Wc4ybv7bqfc7Us`{ce>M=O2w9JE_-l%yuY=mP8u?F zrTh1lz2>zZ$>Qv4bp?mkrq3qmlfgWkv{YvOU%9c@>6;9PNt;ZP7mRim^s}${-R3JB zmDx-FDB zquATiS;+V|@b{_C_HJ{p^~)O<&>HEnmE71<8?B@4-FcSrN@M@(CH||krQu^QxgcHI z@W0)>uHS`6&NHt{=7`^r@j6Srq_Ol_vU>KFwO3v`vv%R!Dg9S%B};lX`BSf`p7tNy z6uX;E??-({AE$m53|bHSmtTc{{r>+XL&IM$-C4T1w7WDk`TWxK(hrMYFTOSY_Sojg zr;G0`ZY-W%Jia)zcz@xGh1(17F1)@_TNqy$o+wS+8oxjO-TdwOcjsT9KRv!OpUi(d z_xao>bEUC&<~HWe&rQ#LJ9~Tft=ZRSYqQg{Kg@hJ^YKh~=Hkrv6Z106JK?(n_QZ$@69Ix%u+s!|KJ{SotI3bXKAU`ha(8ld z^0*xsndraJua6#cz~aAOvAAYP`4 z^&OpUbT{#n(IK!`MW&gQ40lvO6;(wW!f(;cXpx^_{))q0MU?+=3phVF|R#ZwF zyBf#rV`m$eV_#(qqGcj6voM~Fg#{v0fD!ZS3DJ$s@x=?T{QBk7YZrdKb~aRFfr}N~ zHYYNDT<)!d;tMdUW4l(l*$Ejx#dSrC1WiD*5)lE9Sk6Q5g+fu0i7D*RZYZe}9s+G8 z1@>j%@qA3U*$KB32qC4Y*Q`h<}h~A=1FdN}u%O?a+MZeraJBbA||mAY_m zxINeS>$q_FDw3g?RAwo&d7iR}(3e_Gh23uhZ)#f&K!d`(=!{s~Vl}7!jE7H}6};i@ z_JH=12uF^Aon`rN3_jw3Uj6kiUpn_PcLRQN=EAGzUU@mLGJ$U^!lTirLrDe z%1wsD6MP*kxAw(?#f_<4b1>Zg17e=V8(uf^Mz|DJ^}XsqS;0VsP}I(NF6F7B?R5AO z%s;Dg{q%UQXl)DZJi?*3O}tS_vvwJ5<4FcPRtkAmMsg?YQ$+FFY{iB|?)cuL^3d*b zT26-=$7Iae_$hK23}!wrn$I}gXHi}2Co5mWcz9i{%=+umo5L^h&aAaAHs=FAPQmV< zyKH=^)oq?`c8RRCl_l2Q<3J53L4IZgK;l5=^F$-q+GhL0joEE0ZnU+=?W4qzOk}C( z$L7*6k9~0qcp^i2woc)6q%{jIxBWI-9zu=KW!xX^WzGF=7~?b{^$K(i02gwsf%FIC zsTW>(d9864fphlE-!)FX^3v-0GZ!1LzI5{Z`Os~-Zb9gZt6{fSJelxm_Z<{_CHdYJ39+2Q_($%5xeKo#AL>82jnh7=$-l!mPGLyr=ZgQ}aolxp zF%;rW+au3}nN}1O?D3!W`RS{B+2FizFU918(aJ%6B1lg}j7XrTLxy^hxe%|m*0!GM zmKz$^T;3fhbFm9{v>zDnKD2ksB^tT6B9ck8&zmfJsBUl2upgkCpc;VuQQG+24(bL$ zt`#9DW~X`%<3sfV5LGC~>zQ8fFq}esTdC4xqg2RAC z6xM2wUqZc8ue|)~+Q}3ut|0r~DwqvdCof!hCC1zPC5PU=eDX9qhe^}kB>>uqmuL7% z;#xc+-dS}l*5G7hlpGgeQMAI3g}Lj$P^16tbYpm2ZZ!TXDxjI)m&?g%!EA8rb8R;- zpM2>|()S;Wk?X5-{@lyGI`*G!;nMIKo1Q$imUg4xVFZ~gRYz;2`TVz!ee~G$@SP{W zTDtr5vqQIu1K3zPzx2uCyU)Ci=YM5!eDRYb-!FXeGkO0%dhFKFI}2M2pFDkfVR_-~ z;bh^fqhHVeVSf4O2lL(e)nnI>T%7;>@zVUE`P+xTncMvNM^D|I`|;S#CqJHhZ|?k4 zug#U_4$nP%o-#+nR^5)OKn7l>qfbQhY zBg=<3hm*tShu0=Q9^QK9`N>0*-~9aBiT4f-{cQ8l7l*!l=95FikB>if|Iq5=xyK(o ze(%_wiQ1vf!@E!3oYJ8}Bw&rS?IIW)2I=Qmv3APJ=D3w zBKavgvG!Y6LrI>43Q%x|3kBHjDw_Q2-7jo-_^bsdh4YW@WuPjRIlJP;?Tbg9Qk~S%eE@FvAH+UPIbogoLu98p0^OLT4MCvqJBc`TR;|v3lw`XApz!9 zoirrGD|Q`*>vt;f<1xk*`wzt&pjD4gz>b2P>M}o}Z4m`2la3v$Fqae(aIJm7mXPq| zZbQL<#HMktJOEH5LTX26SmReku7%ibMG{%)paZI_4JfxKb~Ind<$fcCQ#JMp-VXEO zNXBI@f=St8xcr-m$KGtUuWZJ8n2&NvS+w)7`AL4;T<@?HI}I)fH|)A)bSpvD1|4@0ms$^7li#&&C|b{RI5gV2m>N2+bJAC*G*3#BFSgF2 zt;R+tLRlvz0BFwXKDp``j;OQSSBELH>Gt|4^!H)L54Ppya=F(>p}&t{hf=xm%I}H? z`p|`!nvD_5voLU<2K2a|DH_`xO)Ne*`4gxTM83Y10d)tzYPi8+ege2>Y}Xg5Au5)>TFJ zMqi88-cD*LgP&nI3=bwxPN*WW%MhO3Pxj#|kbsz%C1qh*H^>J4O@Z62wI#2>VxW5) z#OM+^hpAMT(!iVMeLzNWAt7jP-gy~mR74XwA^84{#&>xZrcz*Ar(^)Oux{8?%M#o~otOMZuwnA~zXwLe-ulz`w0(m;s- zsvcWtYr7S5o`gE(3Tu@VCChA_ZI~>i67U*T-N7O?M!{fl`hsr`)XLWddC}I=bXCa; zg$#Np1@0p0wkk(hzYi_mk1>5-rGzOL?1OUQ2FXtg$+WzLF8^}PSck7-Ss!8+@Hsne zWjME%Go2-&2jRr&s@vKQfaL2Hbzk-)Dt}W)^3V1T+o$@VHmVce(s`$Gqyc|*8LNv% zcfL+satgULLJjmP%=KIY7Oa255Bs6fdE?U6%a>D`Q-tnBh=v9DQAbc@8^kJFl5XsB zHRRI>con;08PyHHQkAoBYU`AYh;z@{tXEJ}@a z(;3S4itXkOmsT2mS5&f%N}i8Eb-(7v=1VLnH)8@yjPTh<^9rDr%Bd^91zkH8Z*u=M zYmR$EXl$vjKq3G1 z{%bZ@TK9b@b|Mi6eh1p)A1J~*s;Cbz_6qk#N88QrW|(S`>z-T->Dk?###%e;9i^a& zaK(c>I^fOP}=j0HPmq{34wrO$WGxO1exw_GB0&()aqKY zobrGaQA{LOb&XRSbg?XhPhOWhnKH)=ZiNW{+G+Vparis-$pCR(5-7>%#RJCg0oFL7 zATx~zpH(NByBj6Dr>3ikwv{Mu93ZeX*?(a(;8&k^UfS$zcd%-8uI_U&jivFnq#|*G zGE)`*(Ga3-*KK!pl_iMkep)Tov?A<&bj+7{?u34E9oXv>5NXgQ)M@(y4lH5B93R>^ zp-qu}Q0$}~hF=+3dy%ba%oAeD3wMQ-yeFKOi{Q)LLtss3SCjiaz zY@AwOqXqESLV4@9&TU^cgQ?dmaUr{eflYK}c9-_{{w9~x!mtHQ*QRT`EX6457lUfB zh<$#BcG7MA^CCAVrOYZ}gKlSX&rCZbbRtG&!_f*!o#h`h4?c(4SrWq3Jar{Fd{5hb zSvjr3csa7>*cu5&$t-Yxr4jVY2&=h4anDH)7A2OcCOUZ8>hEl|;YC;LBU*Kc1|U(7 z6l*9-jL$A5?h{NZ%Hvh?o7>EWj>U~+lwuswzuMA&s>f+BZfvz9L zcex7|C&=l;@c*Y7Sfyjf`I9n%34uR+WoTmDD0Y#cv4t~};p|bZw99IVF75MQSfAkQ z&GrUDETRh;EM>T&sfb$f-}0NBg;G=aN__!aohzYLy@1tA>oaP7X7v~ER`x9<405Po z>tt}DliP*phc6{~qiWG?X_5s#kDzA+NO4Kq9L+`@@}h-?tXfs@8~tc43s9i$Xozl8 zxnrjQZ&9l_v#ZRQnXKk`q?4u6fDbba9 z)Mc=4UnBB$M=9vmk@~6<4|Jw_Cac(EbW__XHC8HnA_9?g`0n|gx10OF3k!qEOa~*8 zyy$~wT(LIy!Iz#N9KJ$DJR&*|d;CLpBU2vGy-A;Zq0FRZ zf6y8;r)aK|F#)+J4Ew&9(sSI~d{Erf8M~+JZLtKZ&G2*B<3piDM9*I8c9U3S?X;hp zZv6M;lF>RDZERgdd~WTcsz$$?$R0lG~mA?^Qd?`xeew?tm+S*7RG!7f5HXYx`BKYrt^K zKyrz<(|&s5j*Up}D_J%8wmZzu$G+b@I!Bk}x~+5En?rv^l-u4;=lvKBrS+VlB)7G0 zf=PXm|E%xcu|6~2NzZUjWXfBqHeS}B4^!*spC7RAI5iZMoJBHagdg%#VFQxBX`wu! z^I&CyI|Z}pUuiuy^H|G}U9fdAIq>)7y3(F(^ZRL8?+B^aO}DiEqVOtKrH+ww$@h5C z@n70hdAYen6x?JO)+P4ZJP$2vWGKoH0U&tLk8a(CX>^TyA%){ODTaiw4gSS~lMjqy&eeUz!n z$J3L~kH~{28S8WZ`*TtnZDQlHyp=9kY7&}i-**NNI!6vAQf618>(I6`GLb$4`4H*g zdrRm$wGp@r*6)^UZpL4k^Gl&9FBUcir9Xj2RLAMBXJ_!$FAyF@JtSm-I;!_8ezIpF z2e-oQT69Asn<+1L9FpP7v|A3!61Uh|kfZEQeoW#wDYN=N5ovIV_iyq&3#+-J9?l&q zoI8CSJXZwBi4stGs1wgqp|EKBo^+moo1K_XhHIy0w9WdZIZKf%mTWD2N%kFV)0cyW ziJ}2o9g_vFO{IE&06?0T>9`r(+xZ|P9f+*dmT2l;hDZGz(IVq4xnwNp1kZq#+rApc z^KxIEI%gndUeg+CF9|q_!6uoroA;L|tWWq2{aL+(*#qkwcTqD;U}kXQbvL-8iy$ud9CPnupfk1dg-5?0%?u4-$8lm;)dG~RGngFf!yA*~-(<}Q2@s@gbD zsGZq}TwcI&J$XnOcHw9*3tdaiusXKq9X@;v*KMdzv z7fn@#A{p-mcz-gFjrdcvV&~5OTsOt41#&r^NMAI)UnX4#nD|ERZoZxYpE9-e&VXU^ z1U6=PM*cOkTRQHF1*FYGee5@{(UGO2^2ti5i5^^@S|2^TR6ro)C{i?iF3h_xUybY= z;f$@%9aR>;fPO1mrXpllOtm(;_dXt-8%27u^J}~z4~2!?4=S^BPpWdr{suXk5h-Tr za$VcUV2?G9tJ%7%z2lm`%`VZ)Yojt>FKiPQMg-7cP`bqle*Up?BHr}A!b+RNu8y6f zE?I2mC`1UZy^w`gM=Mz_F&n_Xb*zqvD^*h)$b`vb)537ZkEi>h46Rk4$<>7bSE)>_ z$R*YQhUG)DD6%3dG3%WFCli}T{zs2{6earSe;(j=)NtryApO`0*tB_#C`dAKnC+lf zk*ruiYtKma%uV9&_ZqF;{q|OxTpe{Ndu};_1cd#kYp<9KF9VeB|`8wIhdzzd82#!Y2#wFLW0!k_BLX z;Sm1+FX!*fe>i`A{`~y%{Eu@V9=SVrb8d6)G`IhMnEn39*R$Otw`Q-;){eY3`#f0y zzMHwHEC9#en|Xa^eCEgLZ>L`y`Cxi&`q}A2(;rOjPJKG^{M7u^kCX2o`(|=w_~y~i zCO@2fYx3;m>hS#JgNb_+?@Vk=9G^Hmac}(A`1SGC@#XPb!w<&39(#XmdF1-o*|Fne z4@U2eemC;L==ITyqj!!yKl;V+yhO|3Yk+|P1_l@yU|@iO0R{#b7+_$4fdK{v80guR z+*ljTDIw&hfVY*L=`yD2%x-7?OC3F`;}twmrLHnNgEw`jGnDZFmz>*0^4aSQXXj{T zFl?kr9=(qO?-)=@%}VE0vp0lw%3QK6$^j&M-e2;QG_PLKO;odQ+FLAPEi2e3@EPDd(ERoO zhR2c6y(5^VDkOESV`d!B?M+t9&`W__xHZ{-%zl=o8Rfu?l`Pys@fn(5CMWMYP6iLUArx`&Zjv2BUH#S`mUZo#x3fx)2CUU4ZV9pS-f4O+a)gJ1{A ztt@K+{&V5`V7<^~87g84la+_pMX9HKlDhOve&dp0%B)>#U~k~|-wkznvE;cjzpFpt zF62#je@CpN*K5dSpHG_G{eAY575DW?*Z}YDY-eHgoQqi)l_IjeEdxN{PrVbC&DS1Q ztf=eNjJ>lg8}45TM%E)AVZ&f1FD&MMkuHB(+##RHBMWb^L?F%Vzh^_+T-w%bJZ7*m zlTwlQUc^$?R54{Rm4zV4waw>Phat?8V%#NqNTI$gU#qQ^(MTpiu_rn1LklsZSgex1 zHu=UMX~WR8Fn?DNor zM{@Sa4el0cxa8$21AebaXo+8-pfW*>0s%!wuG?85cj9Ok<234@$=5-%dC_CeICO zS{5rO86tS7%liV&^@9;J2qok>zAvg}*C5A5ht{=&Yfv&jkm~jN?W-5o0HbG_TXeV_ zgm=|<7&3nKy2mNQGKSM>s>C&5Y{HY#SF? zmz-5@K3Bto_Z|HW<{Hv7BNn0UQHk};3;b7v2aN~?KuY9{ zBJ`x@Xp@mdOMjJX(de&-qkh>Y#u5$Kom+#YU$&_$v|>Eo@)Js6y4*40q58p{_3!$| zhh&$2X1^RUwkTb(yo zZ|t4_>p*WMZt3N@l`nnY_dL@J10gdY@mDHy8R$Brg;fKGFwU-G=$8M+#|nNUNC5;% zEL)tK(fXyU?X8V|K$_dVfMVsQP<^er^>wM!?RK`a5w--6p|w*km`sNI8ORZ9kLDJ& zCt114k5Fz1u1cqK8ZnxpjAwFeP0)mNFpy%!0wdK3d!(wZlgVDMYuyk~z>dlaQ%@?A zleQ#`rPi@I3qGUkOLpGzJPUcdGG~1{>?E{^E>~US5z8<+>7`>D{1T}t!iHy;@WQI( z5c;~?ByC1>G=0+QW+M@ZOw|dD{#3_w4m->FJvr}+2WNGLG6^ZM?szp>>gPW92lI}p zg$pX$QSavE6+oS{b=N$9!B03Rdbf73LN|ugHDrt4Av6)-!d*#Q0-K}br$zRBbC6ql z^T;BE6k8pmyGsY^_$VdT40R8eD=Q@&j2j%yeV&bGxp##CCX4C`q;F-$i(ckQHxwOCn{2!F zzo#8#eeFcc-siz{p5wNrV|jqd)-!W%4gm+~LuI=$rC1czG5!zr);UAtZ$|lw%mSlT z3Z_qM+%UGCr`p%iB)rp-HlXCo$4pLGjDz>*XD;3+j};3rN>Q=!>>^4XA_*fHG_QZi zyIPX757mutS_fe4f|Sf5K&^F^zS!3{=lX~XG;D5wDEe0dD}@J9c{M&e3m< z9zHg{^xfjei*GHiEzU3guy7B*|K>t&;qbzj^B-~B|MdLy{10Rj;mLawww~WW1_l@yU|@iO0R{#b7+_$4fdK{v7#LvS-wOjekJ;Jn4J^csH-ndtoXI#$Wec&g zOunbq&PHFB5AsSCG_d+Hdl>KE{-gBCnAtvex-n&UNtq4OZ!&z#Fi?}N!p^t$rZ3Oi zr}c4>E1V#tN2w?K`?9;3?Io?N3lZ)*Es;^YP)bgyFR$ZI5G&UeS5x*pKUS>F5fqPv z&hBP6B4sm1h8^B(eQE#7TF29)dcQ?w1H&wUeX@`vkBZLkW}Cqx!6h7Cc)d>l(dAyq zb1hh|mq*mQ(O*wCDUxnI+~4o}>&YyEQAjosvn{3L?&y8iFnQV8WtYQkgJ(uod4e(I zf~_fMRt6_mcUrxqB>R07p~UPpv%8rMAwxTvlrkmKvSCL^my!RuebLu2;slJ4W*-mV zW`~Ec9Y_JD@Q@lddkodgC!YTk@K28MG-L%Uu9dw`o!1W`b=I zBc*&11OwQAL^rl>c8^8YNO=jkJI|+#k5fW9J9G(xMpVV?v)0iZd>&3a+~$8|mMl8Q zrm3`SI8Na=*)LWE>lXtviF$X?hX8TxTxJoD5JQU|4h!9 zUGfcOp~EHW_YE;>y1moemePjiL7>m{e$N{u%(qTiK5Sk3-0b9h?tPgF3KYu(<>e*^ z-oVT4?H~a6E%(}z-_zj0qa!zpi6_60;_yT>7fV>6?doPnYi;(vHzKCY@}(Feql5F5 zLkB)&NqNwr5;>0KDM#%!cPd<~zRtujg22t}~ z$Xf1oT#onG2{v%t8e$0O7D*v1zVaZacVfA)H zUbCc7FHr8F+oPyXcn-gd z5^^b6Bd^Rua(Ka5GWOyfB2K9$baHhK&x79`Q@>%)NO{bBSTvIxLw0Xl>s_7>2neVu zg&agAdJQo1-n{QkAqvHkYiOS{ntsH4^eXc;^SA1~orQE5FZ7i;BCziJew^*I;P!uA z&$9LMcHR9(=PK*v7mU?VYe^OG7s@;V64DWDC=5<%wD8O6SN?&Xp#;%E{5AC9^Nsx* zU14t6G6vR=ty)RAFcDJ%kl1h;SqrtTrrlDPd*_MTIG!f;@C); zFtXfU8Gj*jL(Lk##AqWO}E)X3#mLo%7-3*zSYst!7H}tZwR&a2k=QrJz=P@iX zJ_E#i&z|pPA-xuBs@O1UNdCuuH*pdfqUxT_N%Kp~Vv$m)@OkQM+wDfwlXh7W+{?@# zsj?u*@YiVO!kog73=QIqOg|BYq{cLNv6UTHhr8XF(7A#(!UAef7233u`Y-yAf?wkJ zqu!2pt!+jX1#Xfu3KhWjm_PIKP+WxHN;$17@(6zPL3Hi68Tw_XnLR66yKXwIe z@#0poZXqL2_uAK#;V@tKgbhjcImuI(wV})fxW%nJTxLR=$e^r@j_nL>N{MT^io;qF zo@S4Fqhh9sWE^?Q-UJazplB@~uCIixu`1Z|swNxF$z&mIv2TJ{NNH;UX=xiG9pDS~ zFM0ZK{}ya>pp7;Ummk83%PNJ~^%#msYI|AaV0mO5%1v(*?y~IzSUmQH1fxuHVw@l7 zRQ+c`C$L;H4(Nltm5lGfZ%MTt#-|>N0(h-WKR#pq9zM%BR6)J%H^NO1pRw-4_$+IO zg?bgd@{?`Sz&>BMvVt?9QO#F;*_*>bS|D&+dlYqqSC=&w+SsE3lty|(cyWk9#RESG zY}*So<*dvBpD{5R3)hQHt)Z>vC3BY5!0_Hz&w*Batb#5SGX&FB5J4=st*P+Z!&N*o z?WDQV$`+9b^HWc2?|Xsf5$Zb?FMF3`jJiR-djG6Xtml(0aB#zGPd&ln`>)`A1aE*B(I<8;`0R7J_JAdHem`?R`J`-0dQZcmnG7=q%RVk^R75 zII$$=f;VUN9ESdSw)?T%$`~SAbeauyLN~J3h(8ksR2|20tP`>`pg1W?So(tcP#vAA z?k8^_8yfp&>F&~rky}gO9(!kOdHnm)XGac?d_Vlf@N2`}rP9*$(l?8D7LPA}cI^Jb zdqe?zHTKr%k7FkmrWcZh@8<8$-{B$z?&D8Uwcc;D@|9I-%sg3cqsb{B>sjnyRP97fqcyxXKE0aG? zd^5pq|B0L9r^i1X|77^{(RU_ZBYt3f;(>`YraAZ;U|@iO0R{#b7+_$4fdK{v7#Lt+ zfPn!91{fG%;6Eq^bUyzJjd|PajEob6F2ryRo~p!e&%J%wSM;Md z0k?Ns8yH4*_PG@l-~6e{kY2)CkHJzlgK5>hK*&pcYL2T;*p1}^s~Y1}g9VqeD!NOf$0CREjBi#&gv56tvAVV{)^~u`ud0!{ z7KnNaqNofnBD6_>y4{=!P%oG%B0A)cl`{XYvIYVG&agC z)P)5467HHC9+-WxCaU=P;VE={Yd|T;r{teP!*Hm(HwRICm=4&_z1gB6(Dg zzwqUN0qof_;B(z3saEAHKCZU#1bxdu89TSTxdp;QidabX0=zPx=<<{~&A5lGwpsq{ z15D=lfGdj!f+XJl8j`9;nka0vUpeJ&bE z{wB1kD}uq-(mq#z*WmL|T+J!Rhr?}l?kES+a+nPX01W*XB;MM;-ln}6DlPkLf?Yl_%p*_-i-I4h|Z8~5UzxK zmzmM@p0oHGKDM$y)h}!DdLzX1ocZOyhL8fR zk2xSJ=G*_1Ono0&FUFK}+NLb-1D8BFlea=HK%-To`3s$mz4jHvX`o1g&qPfR)3bXO z$faZS6KLJ#a4e!{?On#d7jQO5A~(Ku8Wr#QxTJt+g$h+Yx8$^Id~l(;v(edp`RX<+ z{$h*V@y%|BcT9{U@XmSUmRPA|*+Lyf^A50xVSm_Ee5>sfcMhDA|0Mj2KlRThL( z2r&2hQih6NUy^}>H_Y1{W-1Tx5bGm7L!n{*Cdr5QULM3&;Qpi zH%^{E-&i~I>e{Pe8LE{YFJ0&_H#%YK)DY^!^$PS|?dgqIUOsgugeu8zigaSjljzFO ze5e~Sg~ztX53ww88Sw<_-|-T3>c-mH3$LuLoj((%uT3EIZw|av_x3*Wma&JeNHUEd z{cA##e;@i*lrdkzpW}ux&+y*(g~qFE7tWk~DTr2Oy%9|Pb&`hq|io{fBPCdq3iJn{!sb_o&QjoifeK_LNn{=k0H5OYz;*0?8|cdhe)? z$&5@L4}z((op>LvqxAX+^4k)*OYSHx$#A&Z^SE=Z`@rYkN{)W&kkszK(l4={vr3Vu4xLhg(1NTK%VXK%+Bv;Qe=fw_QG_}AUIXg{T!e)#vkW|_M054$Q_~ zb+$HJ8oz#YX!gO(A7)Mr-vA1U~&J2&%cn#pe^cT~&i2&G|e)q`Qbj`f~ zpC9>d>ei8aQy)*gJ@wl7Yf~r47I2tX0=}I*KYDla*5u~!`LVYrFHV*whbF(9_>}hn zUdQkM_3+_|uZaX;dV{Y41_l@yU|@iO0R{#b7+_$4fdK{v7#LvSpB)3T)0+LH+iXX- z6c1OMZCGbCvmJCV+neC-w90y9-H!81%_8-D6=leg?Vi7Q?^owqtf4rN^_T9wVfo~` z6;GK`M2cP=*6mYHsO6%@9JT)B;3Q#;N6)pgy03pFb| zLfX)Q-|X@68dmm7OaL@b0uFejEdRX5{U7)Vw6xy276}Qbc$_O_9a#(9{BLZ(Cd#?x{^xDN0Np8*(4 z3`+XnqWQ%Bm0)E)>BAg?)g+o*mt|SjTh(@vjkpESg0-=fSHG$3&b%uU7gWwwVMKe{ zuQWzBafyb|TyV`P+XJ!;EHEf7?1msQ9ZZ57NRqSMX*GK6h%wJ-$ zKHg`4vi%a+2VKIeEbIR5ZF)F+q1Eh?XCb}sO!D#2ulZuE-?WZHp{LRFIQjSz4Z3c? z{7Kf}{ypB)5vgc__!2J%dEaerad(;UAa`ffSxAiSXa`B~+j?KdGd@-ow&j91bB6Q0 zFOv=3j^)xNIZ#}XqH-c=UI*k(fSGNX9@N&?wkI!xi(I5;ru@`TZnWxNU1fBkT3H;bpu_I{%;ceqI*Z5<3@ zb98*FULQJbmq(#XZTaYc&J4i`CI_Ybf4=^g10+Nv@tO8dpO=y)BPiSnXqVd4y|o+5 zf+rWa)yDhdU7syo+77Ij%jDHJT{f(GE^q7L>lFRx(Z{Y70R<~}T*mTvYgsA5*)ADV zYw6?m{NrI{yBMwV(1+aEwtodSQv8KI0HZoS=iVL{{P@*`q*Fb5>6LpQviu44z`YVgO5RmA5q;&D3#sFBK4@Y^h{#@P7KnF{d`6j}qWv-LvJ6 z7Cm&ukpn7B>B8q6`C6}ySy{Gi!E?btQFa1*No_@=aSo2DiZ)_&sG|ABZI7Yv=ulKX zA}O}^mz{_Z6TGe?xfF2O+G}+(8ONM6(ZzfE|2`S9`s(FAtp%W2<9r``V%aVlo?0R*ao^8CePM94^qNSxUJegcb&$D4)ATc~(8{WtGmvz|)L}^DasTSJI z#CKs`h(#~~U(m5hM{TH!KT1hrU0L0;v9G`i(s&d`$nnFZ3)SscZ8ikesYIZP3XBkR zct#~AXcdx_7QeGx5eycWD;UGW2uA#ZyoL1yFJYfjEGA4Jl@j$CWCV3QXA>yn+Z^@A z+^S=_;54o;Fw%oZLjcY^`+ks18wp`ZDINW^4}&6fXa3cJMSTT2rLTSl6s-iB%GfQ! zf-)nO1)fs5&Z1sO(2D3k%p*;x=yax66#Q&q`B|WI>1u(`rRO^pl$0TWRb`R1$E!cl zr1p~0$KHD8tED?jL*qA>x=X7|xuwsa`Dpm{(fh|9jCM!1j!qAcFHMi#U%a>Y5pMy! zzId8E0EZUuKl9NwaDT2le0T1n zxr=ivL<4*>dw2Nl+1F;*Mo!N@JNv`T#Usxiy?*5E(XVH|9R2)AY2@b7&&NKUd2eQO zrZzJ?b9?BA>94218~bef{h{5VLr1h|ad zN8Wno`KjrtA1B9$?i@Wm^u^@&M?RX|ove+XoqTrk59BczntU+v)x>8LA5FYFu{&{g z;@OF>#=kxK>G-?juZ^#mUx5(Jbj>znXVCb5TBE(U*W76H^jY-i~uSg->0K5bEu{MvHiT9RkQesk-VW3bLQqgMa-jg8&jrQ343jw#E+v6~0##EYA} z;ccPxa!)mNKC-*J8;-Y(V+`CWd|ZxFa3~OfQ)f4`2QVma!|p~84CV<=rO95locFl1 z<#ggX^b8N84UC6@KH69p0^X`z^~9oSl;QpyTeFQf8Yyb(oQ?$)m1M`znbFS3ah*0A z?X9h=+wC3CA|7d_s#CnVm@&UWmx5JmocLU>N7oj2&*Z`6#P4TSJL?L`jg3s5s;&XZ zbZdQ>9dF|TFcy?~bBD{B>D|ma`3aTRPIzU@K)T;)mg~QP+j?)VI-zua74IpC2qLZ)7b6hY#g4rZ^ zJR-)1kb_fYqBbS`#I|kZAlsq311{ZtGnI~qyA9))?b&HX zslaS?%##=&nV@X9`iu3&cyDsS_6NzZWP|yNwBr;C80Lr=f&3*3tX_(j7yIhrLqxhr zKp=;*bMD1#@E8fhB|t4Zpzo_0XM7DhSCp9SuHXWgYq^^GKKUyONZu!H^&V!=Q zFX#syJSqBsvL9ES_+fad*c1>=wc0c5~wwOjQEG2rck>-k-uDW5SyV4gMJf-Q9ABan#{qkL zKr}EbEk9glp(|ELu-pFcvwQgEajA8vMogJBC3J6YTQ+5GD9Y#H@PrW*!Dfz~;JHrr zTB_qRKVGILay7_c*C$Pw_~zFMC2U$WNLbYrji z<_jTA=2i3QMV8Q0=eAe*{_t=;2c)1NGW3O99`)Gd)kFQh)Y<7ouC9BGr1YI6JGRyF z9tq$ToAzW4y8b?I4DodPjxsexw-;N2q+3ruY{Z9f@+g=_!bbOo_l-^$CV7DeZQywi zt%VdU^i$f7whz&P&$Sby3~HvDD!L9#tK8>?z7t5F!=BCAJ3WxXQ^FP>^ zBcURHs5VcuKZR3i*uMwX1D<=Y`6kiK>o>>yK^z8M zIUuoaYjY5Xp-w=eyu+XaaI}s_L>ah3T}Ptp9_)*#DnO=iwvy@aAQN1OdJZB}eg4$= z0WkzgLrY=&zvAC4UZU>r9#W`RgB(B%_>gcyi?Zm0s=Cm)2iGvp+bG!=k!9~KqHt+4 zR`WMK*VkM6c_Hz$uFI5I?r5n~uFx0BA*c0Lmy9Yr%aKXp4>PD-a3vera_^(SDTp9J z@aZ{IOs07~wzZwsn=uc6%>7d3Xa^%njyX)$XN+|KP3kF2BW$XBof{{&w*IzF#+El@ zKV-E}_qvVNZkI?1!%vZ``Y=Q9jAiExmnBxAu4pwlC-bh1**`-2I=dH(D)?M(wYGpT zNHk9X@lctCED%Qo;Nqu%%S%`FJfKB$@Q72|gTHYS@Ohw)<1{aR&#@zu??>wzaH_g& z|7k{>e6i}Ag7!~8dr7YkeH80OTEg@*t6@bU+bRgY@r%Np$T!q zuc9y8pVRU%28re@2V0o4(O^%{c5SOjkT>`gPK2AJR~`sRTNk2%*ArC7aJPQ#Z=3Dz zU$=;^vL&k)wcQvTcD`7@_G(|&B1@&G<;gW-Ee-+-d_V(e6W1l?n>{F$&_n({|`*HO9W1k=UVs!k-N29x=tE0?JU@~=Jw((2$Q^tQFfhQt00aNX z7?AEI@e=<{wma+1-A0F;ZCK=@=b>7VwL|9u=@Ne`7`8jt2=Q&aY05&h^~iE(rb;TK z{8jD78cr5AKl;^q&~!Z?qrJo9yTtcjw)2Bqiz;retJ}H2^n%@;&DcA}MAMHfmu2UY z?(4brb*Izr+0oEGWd+w`t(J7w-`UZgX<~Gj3m+k^Xf&JYVc>8Cw!O???CiF91<|%e zVjHD8l258y&>k&&Q?k>%CIm&JWkGKkq5}}Di7lLjSEZz`vyJt&yB^KS1xuhw7_s(D zi^a_NFk~X2t=w4{*VG5+Ubbe{wK}aMh?O-%-q}QG>U~n|7R%8&hfOx_EpQyo3CtemU;UJL%0A*hOF`d5rTk5^O z%gJ(Cc+r_V+07)GB}`u@4+t>5dtvlSozyv}Yg0rzD^{ACTRYJ?nXf6C zQ_|TJe*QC!*JH>c$4f{=lqQlf3%%xSb{P3rbyhb@Lha))$c5Fq(W0FNGCVGgIjozU z72I)X?x>E_Ew|gYPRGNHS0f7E99E%@*TWBI4{on@bic+cR&{Yed|>x*ou@8y#>m{w zE7O(uiZAh%;q-vyA;q$;i%Oa>9oCC=*SXDb+Al}oBzw&}j9=8dqxA%tp}v>-NqEQ_ z#|E$NhNVHzaf|&^dE3Bx_udMH$JWBh|qcEEx z2lt0AsCp(d&gn82bK^^^rG*l=+k1=pSEpCZQ06@>7~el)uARL$gpzf=%5nx%ir-BC z@9gMibc2gpEv|lWBctd2_jh*bX!Y3}7w!IcHupzBBO8QH z-I||RQpZvLZ}HUNS$$LV}KC)#Obm-!BM@u$|jrR#0uED<*%;WIn`w6_KnNESKIMZ+U0B<#Kokf zayvh3ALzvq?6-d(ojXcd%tC>7$ui-_&W$ohkonG??_kx9Gn%xPTSSgrbGwA?@!R|B zzq{PH*5)3j?n`>^N+}I4YlV0A2CN4yY>4rFPhVdvOWViNoY^mJzuU!JZU}vu3xySUBPpSh>*KRXP=NNoQ`}lRZSa$`tSG2D{=OXPAlQnaN z^=*2C866v!T3Ak_Yk=g>y8QsPhaZqB+D+lf`Vq)CAPLnEFL$o|oy`bup@LOH@M`Zc zJD2v>Ul=?NB+BavYr(oFNRb=001=Ec1m+n=>QDejsZ z8yLvKN+H>}ac1ODlhWt5t=_tXGS-8@7lw*ICEG)6Zs{)LCRAN*mR>44{7Zf$1AG^9 zlM0a))ZlL!?;r;G1}(8a`?9tK=9o)f+3iY~iS45?y`Xg4u%c@yvZb*v;1)m(T2c;6 zd`!F4KF8RN)G?@(kV!{6n#sYSqd8P%Qt*2_TLYI7p-3ro8Bzf&ARZNiZ(SF(|H{T? z`iSwJbaS`aTM}cm!u(;oy>eg7JmM73UtSNWvQ^QiE;|b^%lrgc>{Yc7>6VktOLL3guB*Ci zKz46!wYIV$9>GLb)T7gMskw%_?Y(%}3M#E9?WW3%{=`qAolJPRaCd}@-Z>;qVqLaG z4=(bIww=}*_(bdoQc)HJ;ixxmHUl=3pkM6^M$7T8j^{^ZQxF}aPVzFV>X^*s@}}K% zA`CFL9j7dzt$ohR<|e4M{o#*Mza}Giv%ZCMvD4XYXj@QFnX4gzeB_6y^H?=ssScHC zv(B~<=44N}M>@3mQm+wVuhw)OshqI!3umgAsJy)$`JJrLE*elY6jO!P1d?D8VG22e z2-bB+#p$_RkQ}r%!~e6{xzgF;@M&>m9>fz@7}c)W^VdA27x`^p9lL~|1V7{m{n~mz zT-QX6spFL-r?DM}8GXsSgK!~Cx>FsyyO!xI0!ADriW37fs~`=~78FI<+)_K^ooqW_ zVY%V0U&nEFYy(d(g6JWTuGW8npX5W4C>_tDhb%0iMu|n{{v;4k zEW5eMft?6xiKG?sH0F%|y^KBEd(Ygu^L9%bL-v5it`2F9s~A(l?p2Yx4wcB?#&2{f zj~HPKk{wznYZr+MR6tkM*Q*CEUo=;*;}`nba%}Gbf!2rN!E6x$KTx3hHo4*+AQK@a z*i&7la=`G_xbn~|0D*?Y=eGV)*UP&mh&o$(sLZpUTTseFWd(c7-2xF*W^}*PIY}V$ zjew()LJYM>XdQgMoAEj-s6&WQF5r882OCD<*E~soROYw{Wq}eju;$z=i|PjoVjy<~ z)vAueruLaSzEc&6Mj*_{WlN~{W@EpzbyeQ$uzqz?8Ob{hUCUYdhrlhP-XamZyO*{4Hd?1}XlLZ9cHJcF0HG~(FsX=e|Sm&5zgRmEw4nki( z_h`3s{iKL~qH#jmPUG@c^Gd3_jiStYAkSMQkM%i-|kf z)=&n&sO`Zrr}pxr${bj>)km}o)b8yL!eh}}Qn11t4~!X?z87n>qYU(cxOgl{ii2t5 zc0(EIML*IZoM%G4nnLh(wqs&BFLyY?DrG-xT z((xTqJy~L(TmPo}8?W25M5;f=kJ-r_V|rJIm`IdD4#lDs8$UfSx&z*NC}eP(mWMYM zkjcHaeRgj@1eCnobklyjc#z(NSer5}9TjaQ7!pOdKEHk&MWjs0<44uW?0)v{hxD3E zKhdtSo6b%6Sf-b1LiL zqJ4bpWX{?`b!7AN?cW{&g1COVo=*am&y-#5*QTpv?51%it?79d1)o?}d>yn9=^#^W zOrG?$@jXoTS{m6y2P4!o`lcQchTTRQ@8J`w%d#l@G=g4z_Ue+*%AN-EdNm8HN3cQt z+AF*sN;9^m7;vS)0MscqVc!d!KKm7G!%(GcG${KXvC&z@J75{<33X)L#3(_6*5{ryu+FnQxZv z58qz8JM!Mh?#S-YdrO;3rU~K#q-?w|6$>ag%1|C$o&7+(PtOFoxd}G z>&W}VAI*1%-<|*QsNMiro5Iocn7%u_JAE1-|KaI-Q|}FZ zI`#h4d3^i#C%>4SKJxZtZgTDD;mL2g$^Ye%cPEyQzK$pV+h>L+z8nAi*z?D}J$4U| z{`=$Kj@`kJe|_xi*!xFU#(o?+Joe4#XQLkv-yD5w^tI9FN7shtM-LBuFM=6-4KOgk zzyJdS3=A+Zz`y_l0}Ko>@c#o0=saTQ7QzTtGcIjT2N=jW2=P3%&V87(ly{Dtwhc4! z2TyuQMm&t$vF5kSeP3JSzH5L z%whKC2jf2&^MHd85&k7~zN@3fpBWt6nPXaeBL$}}Jj(L{e&r6q)MvW2XGWVq39ENPpY;yPS1E38|lj7@CA3en8ZB}&EL+(d#x)3KehJKjEvr8 zq2_U7W|L+94dpQnqZO1dl*lu+4}B;Aht~%}jRHX60!#hZMAn6pI#;4>9^yH>PaSV# zhYCfxk+G37ckzTgr*f!UU$B5^KavG^h>LdV22=J=hR}hx8fJ&y8GnECYL}}Y5uh^n z%5=VOs_#tH2R2iv7V4FcjYJ4=IEDu<1H)Zl5rz)A9h66aPw{)mU8cAmEHUPzBR;|m zhL+3`PSnNI0@lM7;l=2Wo7EAgMMWVXEQtL&q4Ge_>0I$6liSnF0?ywEZS|hK&B5C2 zzmf7PaeoBMB}LgmyuWz!y>~~~msA%Dc_kh{LDwkDiQ?`kTs0#$KIBJjW*m|#ZuH6-f<16LLI+Z$J#TfecJL2>E@z5c5UCYp!l zZn6>oYphTbCb5>P>|a?wy$lwXkrgNyTi$2t={qk681`CyJHBVUT|4d5LdgtB6DinY z?GCv4oRIP4f^?gc<=VCje_D21dwdqSxifwh^HEWslig3nL>B<4V>1()DO`_D%*oM z5By8!(9+*@H=C%-GIP~v>r`X4HF=(iI^orMdiUJoBZdRLOR?cj12f9X?EpP_IGgDv zrl0I*>X|Lou74R#8?R;e!{FE8_LfDhlQ9ECYE0o>0|X?ng)iq} zZpA4e{&G{dID-qLU=K&Aj_W-AbsV;uB~rH)&|EcD1ts#Ou}v84(=Nsolqw47Hr0f{ zR*Rn0_Rnqr``ACCKj&94BG_F&CfZjiRYKgLd&m^x;dE|uFg@8>%tVp3_F3Oa#rSQ%e`^=EuV+XgCQdUH~V!I+))rdS?CbulNG;8tr*Wri=> zM)6_W<0_-08oXw}q8D&U^bQdXsx@ z*`y&AmRn^O8!Vm5(WH&OpeP;a3Hb#(u@K3r-&kF{pP{;rIz$^`$94K!xZ`xw(Egj7~i1HEUWh*A@mthd8%JZJE^4-F}cGVBDUGRM`ybAxwW zV9izTI;_q(B#el$kyXRJ(Y)$-Kj}Fru&trm=I(HyuXzcvr=U<82W?em`!V{>Zf9q| zMPTq&TaLd#WAGnT1p^Wc$uIKzYVBOliMe51Na}hFq?f1I>h$#+`$IRSH+GgB)WZpb zy5b3^hu&Gm9k+=7lRN7w7tKfqi)R^!&V;3QQ>NtOs;=XsSJ$V_Z4JsK<6?PccO|x_ zZLyfX{^)wW-4u^qS@|B=*?rz=os2#hNKoM2Vq2oBy^LHk2R@In*ggFP94QLh;uuU*d#Epu%z@xVTM-6HwN<|+hqa8-hU0zjo(xA%+`tOo+T5ZOnei^2{TFK1j6Seu`qrjFQ3OlhtG8;{`Jmw zbC0KZ&bZea)^m2Ugc(!xZF?(Ma~@UK0WA@%s9i+gy0{R(o&LjE2hNh-k|&@{HoAOl zUFOcl1}^0F2m~_5bQ{rUsI%T}Uv97a1unAG=pD5FBt5F@@`#=Np}HX&9}bnQGu}ha z^PBNNfb|jW;j8WVu&9+;0GtsfC8Oh7%`ZPLrkcE!+5|C3JB5!J-DGCL@=};Oz7cw*I;AB_9giV7MP@P}DrUtWr4@q@qwo>Kg?* z^y^9o)u%m;NxUcghW-DM2k#z!^x^jozjk=5=j`Ey!##)ZOumEN|McY48M&@J7$g@ske^^gOVC_~63_u8%K_fB5jg`1*tQ#_k>bVC?4Di34ZH zPK-VB;PD4v-#{m6{6o{D1EY^UbbI7&JOECQJTmh6 z@P|bBZw;RqZta)t|L)M$hi;PJ|16RHAMYO@dhOuW!#_Ll>EL^VCk{Tp|Bb<|!JP+B z53U|u7l^R;taqpP&EA#XO!Ckfi48P5a>dn3xO^Ke#If6J+B_0DVA5)Qo_NU z>$#}|mbOmb8`sM1Z<~w}RI=_zd)vRuQ&hgH9r?`Jud*p+qdTqRI>%%b1u|2i43N^q zaih*SA-!{o$(7Dad+>tr_-qOMCAqO>9KaGpi#%u-q@6<7UCe20))g@aBFXI=t=NgM z9}%D-!{sb{XSUldlxLWRe4h4xeHn{v%~q*l&lNw~);T=jvc7RKm(Ox_HYWm&+lTmC zE`y_CUc>fgTeq~yGA`@hHRNK^FM)lgm}=_m9&?FXHA&Z2MdJG^?jLQ74t&P1 z@JoihRkW+gu-a6}YjIiUai4|IAg2T{SK7~5pK@~ThZ#_)eL<|wXaVC*LUPeAID<*u zKcOWJk_g>hx6^*;_g*15MO4D2+KU251_^g+X5N*|r+Dj+VPK%)~w3D2Q~P zO5k0&diO(3P~-ny0ZCMf4XKb?HaCmTGzU^%w3qS-=43JvKaszKvz~}exCuVqxOkCc z{}UH4RXl^{TjgJOJEbpqMNtjF%-K?ON9V8~ZOgDDbv#-x!Q-EHyqitC8VHJf&6c0& z;9xsIFZf#(DoY6-0aW^{UT=>$0BB0GAef<8W_}m}oU;_Ap&-i4%>Yv8StkyrAc2-3F+TlBVs{DI_9&8GerVFH!kV`E&d4Aeca0!G*sH}l`_)&Rxl*N zCwODy%F<^2Dvd^i!#cO#H@+cS4;i$FFEa5=1Hc7kRum{TPjnt`x8l3@kG9*@MI8*4 zffiF5n~%5^jbjC_OzG2HSS?h}6QC=8vp&D7TlWW2MtqS0;z*_|@P?|P;0^+{Z(a1V zav9`UIECvc{Ubg^i6L4CYMu7ClYjX#cI#2vvhcd zOs23e*59W5ovce*=90#+#M-FHmAeWicYI>-71S2NWtQ#oe-V{B7TA_|Fs^=2RHwzPKCZBa=(XFS3qaLI7T6;e^qU1=3fATy@@wt z4ocNhle_EW+n8sXEJdkKsn>P?m$FuwWYb~85T2{?r3409#U=%t`N*L&o=d<{V?r&u zyc%Y7N(kQF7ZiuN+Th-g;W72b92oK*!*lrN3;0Umeog~nZbFTs}k z78p0$;5k?s7)$6$=z|sh&wG3!N6Pmr1U8kJ<396+CU(zP%4^NHqEp6Fs~7X-hHNps z_KS`>zR$cfz1@G_^0b2QW8S|-e}Vt#E6gg&SiTB_M>ztJ%nwT{mHj49E1P zy=__x*Iyhf)2xPx8xi(Ve~{ui&bH@Ctz zb$a8(#)S<9sI$&zc?+%bGA=HvE<9Ub&wG3K1ED6zr1J~*OKH5^!>hgnu3UtH+lFrN zl4nT?R9wykhmbE(r>qjqH09VJbh30AZmB(7RYTNgyZ)oJ!#w#sy@&+UPO7r&s}Y}2 z>>wqY794G7;XS!Hcxh_k3jq^(I|f^^>u;)Q^&7mjvA)axZ(bF=(*9-rnVNe5DH_dZ zvLVEtfjGr9L_0kwIUmx?`#qUrsVA7k9#_GLi(4=4N~JW7z+)0}4U%}N$>>x+b-?^p zQ8`9@l-k|ywLR^!3!rl5ol5pcyE(A{?W&@DljhX_?sl*4X;5U5w07Dq>7?a2vL@1+7RW$Q(<#TmtIh|>uQ%a9a9f{=wEy`nIB3>16l)8)wo-$Q z1+`{B6z$=i9gL{(5Uwi9pU!eVLtcn(f)0IdO?!^W!Kt#~9hZd%1pAPRXMF}q6d+T# zQbl5<3e&jjrT3R`D=f1^)Y{eOW%h^NpNB_9oULl@By(@mw)@x%tZi}}${-C__ekES zBqHAX#2h=%`@#JdfbJ!jlLuaBBpiJuUsS}m3 z8|P4B-cmn%`pSlWZM2!3;Ch6oI@gySMcY+Jx>AMl*-4CTQp`y2e?7lXM14D>U_l!r z?Z6RXj7Xa}c)`bWGu(IDOC5d3y8(KXNWbWx4rYv;;+UIo-7S%TG&wXQ3zlW+BWG;yT=*2F6lcY4lFymsIio&X>A789==yfgmR`1<(C@kho#AG_E0 z-q>4XpY*Pd9UmJWTkk!6@U7lYM{gatJ^FnA&Cypz*AJ|W9v?k%;E~Z|y~XIu{U43I zKk{ zKrG8lv-{PBKoMt{ z>1Q}>s*d#-QRjuVrR*a>9RjI!R*g2|-qnul8Ked!mR;qJuR2G{+`WD}9Fgs(W1F13 zBV_YjC>GC4H)fse`OX{|@~Y9vI+VzlJ-*+hZoW6V@&Q6$Ck?NRx@hLGV&QsVF932+ zV40Hrwy87qMW6X&9&4K@3wK3Age`my#~x|Lim4gjM-*^L_swP#H*xujSyx-qOBClf zmsWIV$gcQu?kkG;@>Ve;eU5b>=dW#UEDFRmCkNdbf?}O-@|dtGjR}?a+Us5G6VA6c zF0{8cuhp|XG`Wz2u}Y^623`+5bzS;UJjiJOLs;&C1GTd&2X|-4?^&`Ob z%G%boP^Tvk8@npaG^E$H{%f_pl-)m2FQPG0$Axicoy{(fvA8R~p<(I{C}=QrL9!(j z=S&x9$0E~hXkwF(#8H{1!&WQZQH_qaHn+D{ci-yOMB~X|W(xAU2wv9rWO&tGv{1j2 zf!%kC#CSD^409q087FZg_E-QmQ=)^QInj|5LdO7)%7v$)RKGO1X&c4}bw!(kVa5+U z6)`Ar5wBEXEsf3k#tiT3qWW38)U}E=!Q0~#{ienih|B^gKv7m93>L-Wvc~85rIOB5 z<=HuGH&8}+VBi3)qif_tIBQ06?TURb zHibLkq@*k8(boNdjP3=q3yO?9x|^od>zX!7Si^s`2JMKCOj|)aq*BlRT~bgY#~Sm^ zVt!`6?6;h2xG!Z`V=F))ffb^o8%67g+VwbHzkabA-jt~-FU)8Rf{S_H^d0*9qtlUt z!L$^gr=$&Ho3L9MkE<9^31UlH<5{J5nQj!@(j~h8)n+OpFEtzrL=li~!um$j*J`7z zW112<(@E)5v&!A}He3P>sph!^3vVOqDx;6GSk`iM?*3sJWqU{s29fj`j~pCe1kkoDxt~KmWm%f?M$5_|Gh$KrUbm2$qSr8y%q+~gf58gAz_sGVHLf7fI%pUi z<+H-z&iB1vViMz`_sCQD)?Gg(de=zIfsEU zH0n^K%r8hs6M+7fHwA_dh#X62_^wRRUW~VvfVNpb@Ivt)IH&{-n^ruI z+Pi7Sv!qS{Wu}R0E*D5LoCTG9#L~QIPk5E>Eyl9DUeaG<(Fc55lUqtaqw0FE);>vb z_qMNuxT+U2Mle+(-g>>%KC27ubEgLr#urHA*BpO~C&snfRy1{qRA38ISrjr-5gS6P z@F1iHaxMQnSi>syn_;{lj3u*D(-$7t(MuL8TdCwVCJM0^NzApHFGl_hLlzr-exe+! z$}-%*eIh%!iQkZw@lpDi&loooi2^xVF%>FmqlA13{!jge%uhTsTeP>tFImRd_bSOU z$ofL;b85qDH2bmB*UGqOcpND}d4)^%Yny9a64`zVi}nd3BW2ZfMtjBM|3B)bYp4Pq zL8E#97p7}@^Jq?*kL6LhKMZ);yEJbPsnf^f{SWn?+P?7Y_6wJnNhnhV!^6;!m|+ld zp<^!XAvIRhh1K6Ut`g=Qzp(CGE%erK%i>J^ib{9Y6enh7&b(9Dh$AOHq(4nw}d0x zX02M@3w$m5)V?dGN1<3-yVsV|*uiRY?~^;dd^@ZCgZFBQft#!xaW{|FZWDm_G`UH7 z)BfWK8M`#dJ$YEXKX>s z{rtJL_Bzf0hANA`9GsfP;uV#rvzy9;0Rf>lS`1m$WPjHq5n=e-e<_Per3~D4M;oFE zz_r-5NpdK^SkP+BE1#!gZrpIES&`t3wfAWFB8jeoUecLUd*)0mjABo@6Zc1Qfq97O z{^b$uPHK6Vn>X_kqxTc2Wp|#&cV6W|k7*1WFZ?d<`H9Q5nhS<;CJ-J5c=qf;Q=2$i zF-Hy%tk$Zg@pYVqs$hIwLT3h)4C;Wv`r5g+FN#gJWDe)5(=o=fPU^U6k^z;|=I5Z7 zG#_z;L88kv<^CBt_d}+nI(4biDB2ox$Cx^#O57pbF`kt*WiqHPRcUe#5dA|>G-Vic z^jDKYxK>T=>90cy%^QyO_n%w4yq&HLYjNgZ7M#6jNqBa7Fo;ehIxyl0ql3w`ee&va zJDI@!tTN(UCdvh&78Dl#4B`RH|J3$<>SrVzl~aw*WA*kQ$Muk;SKqoujE(cRq~^I; z!*Ef)6MbnN`QL!*WFHv-_TM`EBP&uw|iecaJT37#Ld226UF|4{#PefCXP=$I?*%n>G;p~-yQ#8 z|NG-V9)At*fXBuM#y=hV0N;SAzOBAj#%}kokDVHOd;h}N>-(q19vJ&*^!R}t-w*KE z=%c;+M$e2qHd2gy(068kap3Oot%INTzJpJ|%fp}b9~+(;ey4xm@SUM|hu#``nfw4J zh92&14c!}jwg2_OOM{OLKHuLn_~D_q54~~d`k_|;sY6G|3UF^=$36j`a`#&o0$m7n zA<%_D7Xn=fbRp1%Koc2`K<6r*ZOP{@>zYr!Va}O%et-0!K1WNR;B&M?Y?U}x z=9*e+xp>KmK6T(!n~vos4Lrc6EZg4o9_>|SyWXP}ooQ!yiYx8)jpfRKH$!4S#W&1E@Db)Y z<=@vOj5^|@Um^gXD2vuCFH0X`Tig{=>N@y1+cj(17cjYo?ZEl&(bYum(th8I_|| znqiF7IN&1M#daGXp#j36fTl5QBN%CC6im!pd?d?p81rY*#wuctz)$NleE+P+QoGNh zscQ)$bVrve$OPCz(;~_d4|dYm;tSj7&Xp)>D$Rdm<79=<;%)ha)YGplSiud&F4&%t_>?c#E zTNoEhf?QS@tUPd7BN!}>KzzcY=6}pu_og>t%Q?In&^k{0E(+7YOKoMpPH8b%bYMq? zX{%q$DwQkYtdg{+tIQ>1FkxG-+gUHA(v5m_$w-hP8oqEE{VlGR9t#TT2Pp8X*GS3y z;-&15pwu=HjfHfzHk)r=%03CYnpky^Zwlb%OYr}yet9X`c;x&>v2?)UEykZpDo(?g zx*Mu*y)yN~s28+g+`N=51q-Sg>$vA|EnB^2=Ok(!(Ap|C)Ry=Qp&goMR%iDlBPSAB zB&WI(zSwqb`}fM?1%m<36-(uKXd}kc^qfosIY3(J{t?o$BGDQ)-M9f)(ju`bh=o7hQ^Bmo1nM@MD$y5Bm9rH(a@(UA7x1DM>LztO|$MxxsRr!%yR5S({oU~ z-9{Q7-`Cx4ZV>z{N3a+|8Py#7Xr9z`TUwc1OKbg$+d*VxG9Trgi=A@Z@zp#;hyG#4(yF0!wZkveIh(ln7Nu zHh`mdjh`%@j361#r(JfV)@~3TjX<=3!wzli>ye&F4}hYc08HGWTG{&duZ^JKtr`M07H78wtm8k4JNgot(Ua^itp_aI~~cGCF)baWx8L zEFiq0;2NJ@S=`*dnB`6+tI*jg_7!D@q#51gRDE7%MN%4g^NZK!oC<0I6tkZ#E}nOp zd6oe11S-*&KaW1LV+XI$uht{41KXEYIBb3=ET!4{SK0^-Le+H}_ph7ngHA>++<1)J zm+BWr_{678lHloaZeLu}j`N9&TkZ4gno}dwP>@7}?Tpcm)BL1-d5g<^iHaO0%XZwc|Z$b^eRVIj0937zv|Uc36>Y(iz)0+H*!vh>)ml+Tp6? zCy#A!{qWkD_S{9M!d=I{H(G3HBcl}hv7vI30_`Crpq!1{ACzed5x@nTZA4{684KF@Amg6qx|_jsIlq^|95lV`KriF?xOU`O$m*kBt_i zJ0ow6TpfA6U$OoVjNBc5zwe#l*U9>SdU$&HjoyLb&xh^~y)*Qt?fS=u?jAfcw9vnA zXrT9O|IY^B>iJ;s$Ai}gpC5d`=W5Tw;3qu~58gYpLl%IoL#MFve?D+$fXRG;UqAgy zG=6>Vkbk@NuLk_{tb`F?Q~q8QH&VSnPCqZbJ?!5;T8;PqGeG}&?Rkjz^Yi!l{RGb~ zoE-zrk1k&N0na&}OFWl({u_^k<*)I4$a9zH4-xqPoabNg{7ar!5wy1@ zY`b5@G2p7NDSt1D>;Ek9A~1??SJIR5H}#i;#^2cwqTXMEJ3Sds!i{huTy($w8XzDi zM!u%}9lHLV;4gvye)ucy3>kk@=L3JAfjd3lg%19Nr-v7x!HQP}tCJ!{vh$++5Z~+b)z?aX8dLRo_(g1|NeSr z@vGBIUdN_(P5GyZ|9XBW`0CaF82gZnpUj`1{M#r$_CNdmMs6p!`+Kx|o#%a?pY!|( zB9Lvs9jY=MD+^SIB-Ly@cOi zT2boewetRyhHXkz3Y)*}ZH&J&;u)Q=Bc9R76Cy3Wj@f8wWo2>Wg>#!WPNeMjS-dpXmUHBFgEY5l%RUzBS8*V_d@Qg%p7LMjbmV!wNP=++lEPK*f#+4azhDd&c&ysEV338)an!;vGd#HR6(;$YX)J8c10$ zuKl-RRgRSi6*66~oZ8CsQdS6`v#d`|v{QxkEBUKDF&Z;Ei~YLh1U=wH@WRGQHj3jY zF{5DUqc$Q6stgMn#Z`f?(uQL?THHF9jn6c#xfjr}>R@W);AT6-nKHf~qdyv0KqYJ8 zj7*G7*oG5%xyU4rO@p3fQ;?mQwlvO6*NhW3$UajvM29Wm5gJvWqPrr~NE+81jx1_S z!Bx0MSQlE-uxY6y9v}5ygzodbRq(3XeO&FvTv~Z9gjX?IS&(OfLHJYI5R_L}T2o); z&1cwMQOe5Zpc(rPgL1rYRqZx&yK)u6s45e+GG3Un_=A=*dFk3@`T>mC)og_lP^l`q zvhh^5Z$5(WMVFo5Wk8o#587*IPeC!tC$NuftOUyQmqPgH7~?7=DJ#`{v2sN`@S7U1 z!aNgfTQ=+P%@!nKF#V%T{}XTsnUa3nJcwVzdELdkw2p7vk~|1f*r|>0M^w*x=)Lul zrWg7UKQMGep4%0EioNwyKyIVb^)H>15d^KSJz=u7U`~uBops2*&N`teZ^6KLqRmNz z8*(k=mFq@A-?Ft?%c@#isXSQ?WZ?&%GMcK-T06cZ0}q|DB`LjkjjODaT9Ydltzqy; zZJFmwIhsu`h%oR`oErE_)#u8}Q|%X%{R*p)?R?N%>NB;|Q78J8<*4O5gHczpyEy1qnTh%jdcpZt(<%GuKASvc{JfA4H(Lq?zdmfiR50=3V1DQ}b z$Bgh=oVl?6%%$b$me#k+JIm$l!N1BT8+DSo1341KK395fKS*%F=+99D|UVNHF~?ynYNV%(OP^y{~3`{FAewCZXl*$?*4h2RX^G;PK-H6SiK?Vc0v8;+^ zd5w;}aLXS-=_>G1fUmU3D9=17)6~5kWj&P33z(E+9+g;WRF36NKjeC9yQ5Yw%H|8) zt<9HQSw z&Z%!VDD|m9>KYtotx2#6z#l@S%rc5w2ut!@fdhovv0brjC-X?l4pbrrHVHCgdY`5J zdZt$xAO@$cSaHNTqjFo6HNwThE4@ZtJ{q^$u|E*~YV+b806^MHG^i0O0EpF|bC{YQh^x~ZK!#^$-S7PY1YqL%!K4Xot9rMi~M zv{zJB|OILE27LOlf+%KsU=KDt6fnwR}y<=K*CD5R`>0-03_02LT z%7d?26u33BT3cNfgKr>E*afD4P8Z|3L>ukR3y*U*Wt&U=GTF&r3>hML1WbJ9@sTo#K_AyN6%teV6?I1BdSs^Z(}L^~qIi{f|!G>fJZ_vx%LF8xybeT%R~Q z@fflFcgEitKY8fY@##aS#uvtW#`pDoJoe$(J7c%{-W*#WyV`$pY;FeRCq|BpJm1?h@-gxJJHu~a|35i=WVjfAyE{Lb?9?ZV%oXe0A{cftA5yME36+{P@sq?EUZdygKmmp%Z;a`aT@^xc9Zb^}gF8 zk-u?YSwB>KE$N5;B=mqQ%tX&0C;igd^!cT~i1NR26a8B@;Im0DJC)|ObFcz2y-cN^rk@M^=xnBSBIC~& z0rB^#$?zMv{bJZx(reuxUor%~DP-~W*?$VX`=v9#KtA3H{C+aj#a}?|%l*ES*0rfL zj9Y(J@t^i1CI2Nk`_cc2@~J-x`^lF(`fl&pZZ(xak?|t=S;t@AzfAvW;4k|>b|LuX zLf|89{P~*l_o8_HTF6D=Pb{~G|CVC?+nGpq=Ja0o$CnF%e13TSK*GZgIfV{iTK*}v$VG^Oc1aXIcCK=Br4 za|O{$+bIPn`4SWvhF%Kem~|f3(>=Ws?_|5MqD@grV$xRPeb-x4+fOcSZLVD{TW;t| zTK^+5i5ArbT0Yx1u{o&(eV{&+)|g-8hvMh>keE=ZTGfbtHvJm5DiloItEeI4Be5a9 zjL{Z-OC4$h&{26)sB>vJxo1Ioq&|(&H*-IZjIE4T}y?RPO((EW374h7U-=yw$GX`<`r;7wg#`2zv!OMdOIJ*uquu z^hK^vO|3VHv;wYLMr><&`a=7BzRQ7$3K+HwGgZ;F=XnYpT6$e66NvXl*I#@?<;LkL zG*e}{w7k@iVxRjh)Q@$yPd@$B(+Xr)xZkWJ(tMP2*;C&?c`_F@Y_K;S$hW3=^78Tz z&po%6{VK3Bpi7sxflM5bfE45Mi&3CD=5`9PpP(nlpn?U94`uX@3>z{upv)#52&)?T zT&Sfgw1Kf)Nm8f?>}6oU-onlIwo?$Vi50lS`@2N>K140IcTJ136R1Oe}m-nA!`~z_cO8+WyK|Ge@1R5YNFE@ZT^$0{mN$o zKa&=ej7g5h89ca^_jvLIp66TsB6Jpw=@FoRht=r5Mci}v`INfQS|;#B0JdRhziDxLDpSw=w7QhuTxEYx^L;9p z$s)uI86O#TspU8^5UeQ*!?OL!)4*(H;Jfc2+JC|{h zIB1e=k&#Ma^64+ra;eMSGv`p$H}O}J?ZbNJr%#>!&dH~Lu=xFFzrFaxQ%{_J;+rQI z^+Bm~h8WN}OKoI*uwb;_-l}fcp8Lk0vhtSgT2A-;Uel@58>oo)wi)TyT_&6u<~5w8 z#Lwa;{>42PmM!%8B+5VjhO_xZpE?(S+X7i2iP!dlJ@7zby8Q3D%TLjvKqPXt_i#u zrmR0#n^?t?^{k;vNFyOoG-110t`i0dq`FvNpr>vN1K#{}WqXRcr6o}2WSVig<##Sm z#twExnr3pnG%m!Ik~X^?`y%G@u&%N8I@DbrM080_cuurnUkK{5z9Yv1 z{BoB|23|>c)0paJctqV8Aihn=%P@tij_E7flgZKcTqRcCRmNT-EHNbWd0b@~j_CgKHVmNP67l?A^E-5PS^ZmsGICn=JKMw`CaV;GES5AQ!Z*=E#xhyZIkK*Un!f5 zt;`sVC2op_$|iU;jx>=5?jy~Xt^T}?$j1TUs1TvLU(IzBy|PYa54CXw7u!3BSMEQSygnA~#Z|STr+)US*1af;{eE>e%DV%==Kd|O z$Yrwi-Hna&>+KrTNt#5RXk%BM@c6RuE@`)Zt_b=j*-p)lTrW>lseBmse?Ls?f|d9z zpIb8Dz!SSZkCju>MsQ8_m(I8Cq;P3tGhequ8WzeWBh3dsH=WwtSZ-gwys?>A*qONe z1@RBjsBXad0`2Xon=X{mDjR)UmUp&W4jYOU9r#KHq>7nK<2}O@XSlNc#2hSE2ok=r z1i}MO8>loal85*SLQc>ca+FOwyKShQLjosGd~@uyz#I7KBC_y1pV7vA)_X$pU+0-I SVN8-P_Ko+clVyoz_WuP?r(n_m literal 0 HcmV?d00001 diff --git a/GTAGS b/GTAGS new file mode 100644 index 0000000000000000000000000000000000000000..d31c9ce87344d789e39f8fceed7d58d5d5c83a54 GIT binary patch literal 32768 zcmeHPJ#ZUGmL6Huk0na{{|Cw0vX&>iSTcSIfYxbAla$0;Qshz)Pgk{7&=4~q#{y=s zg8?nAU3I=H1sgbU;NSw)l>-I3DhCb}Y~Z@OK!F1X3KS^Vz<~n?4s=x;C~$D!dou&f z3G$>P*RNl{?tcBYV#kD|0{)5gU-&PSKq!Gw0-*##34{^|B@jv= zlt3tfPy(R@LJ5Qt2qh3oAe2BTflvaW1VRaf5(p&_N+6U#D1lG{p#(w+gcA6_D1jp` zrs?yr@O1L8lTRn_P2TVS^VrqMlhI0K@yz#K_l`e1wbJ?Z@bi)N5wojfL>&I}$YR&S z@rx7U3gR?JJs3weBjm_!qJ;Y?+@M^`sP^X zHzRL8=(uy@;<3+<&Gnz__`WYWR2i;}3CC}q>NvgA@%-#( zN1sL>p7`SA&EuUX@AXZe8XnCYe|kbbd2`^&k>NKUz0p7LY43xc?|LIW&yPOqp6y{4W6?}Z+|^ZCT&)&v7dKUXc6LrXU(mIxDav|ntE5bcrdn3s76hs=PT19!V6n{p zz(j)S#&dx_R18H{3+6TDLuS)vEt`t(2_~DeDAnq!E-NJ)B$J8m2|~>S zjL?TCLpWQKS=d=F*VYZjQ?+deExEt;<+t>jCciCy#4>a9mNRPRj;SxN-f$q}`;$Ky z9PseRNKG83Gl_k*x51$xbOCpQ+Fg)Jg(bt#4R<%B6G3f;I&j;Th_0*`q{^DCmlvc` zX-g{Hw%az7Zq1A)?dHqX?dt3-5kg1rE|AWxo5qxg-${070XRFGa!t(G2ey=6(0wv|Je)t?{|7Sdy~^nKLAfo(81IxOI8kGKy!i0UbmV zCSI7PU?{Lk8+vtl%^^yqTM&VU^mG!BP|_uNT{0x7jbb=GI+JNdNikzIwgqj+u4X0) z+jqV)r*Vg$!mA;AUnUcn-&J+H7&Ko)CDn>lP(VPsaU2ip#*(_NI7T!vBi5?W5l`v& z*e;C{uB=u~-7wd6RWpTTl>JG-kCfOXMD zdx?obNA{E_1=t=}jSfP}9>-J_%QB#>!(G}^Z!XGw4HI9i)A=1qlS_)>6is?sO#_JJ z$>m-0v>KggDtiKDxy`z^3=I+zS(c8^CSD_pu9!SESz|Py4x2RER0s7x_gP_AK$l|H zizfm*4CRO)m|8(__({9@S*Ag4+g#o7a}~ssMc&L4$>&|FXIn9U3p2F^n|rsU)Q7MD zodTmXGpr@6l8JBjtZe8onpF2y?g(*8+nMHmckniqLTtv5l(O_~ZzM}M9IQIb!lunq zBDqhNn5V4W#yXBQZmibDk|KYdon5Y4mMkIf(M}qFtXnlxEmdunV)3Rv59$k+Xe`!L z_82hVAwJ8JstK@$Z`*KUyb2eQfK_r)9 z*J_4B8g0!%Ii+e&i`B}GVknCkn})iD2I!Xk@!1rMfp{`dMX64jdu?9Xhf62-WR-bM z6P?8?)1^ot=b;fhH%OQkkAJAaBpVudTdkE$wNg?G>@Qlwj&Tw`oMaG=p<8}tPXApU z@g*3n5UqL1=^Hkl3xj=8S9{_#wp(Ld!_3GI0YUg>ZaKrjGVe!wY3>nLuP^-ea(;7p zEq`roacN~Km%l2+S*x<*TXUqkLJx|a6F*wL_m-FmhmMRxvPhbjMbGco7RWYSS$&@aMmY7-2 zmkAS8%}{gNzL4DfK*7jIU|sQ5YU5+Ib8vl6XdOr^fN>CIsIc7#KJ33!3wOO-cGZ{$MR!cZ}egk z^Kp=x2LeREhEE5VSLd%R<<~dX7M8AGU)!MZZ*{RV{>k49q`5kOZE3e>dBSvIH0*c7W;z&2nkd{Du>FR5$C`9q7**KG z(!91^QX00~!HB(_4_be*{cG^yokwgOj0rC*-sbQ;GY2f;cc#8+)1qS2($seoHew5*y1ft^(P7Vq6KGE0`LxkaXzT~IZOooFI2LZLn-~O3 zn7kj-z^u<%HYU~rHYV@cV7dzcB12E(_BVKh>TT?*+$~$XKsE~8UoU5FJ@5$9YIXur z{Lq?s1N*Q;=&uw#Xt z5)7P7CTT77Y>VXDuUe33>1!(16gVXoq;kGWKUA4{nw#hF2o3Cct+1nGvb2yfmU&Ad z5Lq!KES%w}vpdA#(-5O6UL*Y)#Is4*7UH^kM=9lWKkINlJ^ckm^VH>95UN_f0*{l; z8`>36J5|fR@LTkuV(1^hA~-eXbc@JtS{h@=wPf_v$~M}X>xwHiY4EE z`IPP62U0=bLf6>k;~9Gg%U#|x^czdEDKVEXMDqm|P9Lp57_R_N(ZQOrH;@j9MYd!X zkFY9L9IurJVmy_#9Hfe*IxeV>6W+q@aV^OSX%48WYzd_Z+Z$aUei zUZfADhMeQrpn?Zccji1*)zNJK&T`Q%k*|CbPm-`~f;v}TxH-*fAo0evQ03(5F$}kn z*I%BBTcY_n@_^O%Duc1Gu<;CA_LRv!v;VkO!8lR>$YX24cImkDup0Ebbj&mFF9+2_ zyIW?U>&P;5`>J*;v4!KUAZSivRxsAx`*$=i2CU^SFU{v}Y%Gy4$5i#{i1x>P^Orq! z3D_MV$G+ji)%`xdhuByt#&~PXMMKz@uj`q&1RSCexV_=$8Ut)A90ggJU*+$OAwV{W z71vd+H5e(F7Xvef%No7M`p@H&_21^oyOwd))IjrVKiME2mrivKQ!l{^Qh#b6>%Obj zU?_Lnnuihb!+Pc82M(J@v8ew^UxcRl*RC}|U1YDWp0AsBAvDpIXq+?na;`tm~@-XIBC@ zzRo_`HqvR<>nj`co6EU>$4-cU z`*>_+?5ELZqYp;cMUz=jN%vm{z8t6wLzu%wh@9h7&?@r%hUw7a4y};g z&$?wLE68bCkc$~XRtkcgNel9fBFKd(KI8ZJ79RX2Cj>c>#P4w&3~5CjzoN7}gJ1Ah zLWwQ>EfztI-!dtDm+%+pNg!l_G!sJ&K#=h+NpwJr((j-tfDF9Ex`wZS^QEw4AF-R#2>;m3z0WXfh;Nb+;S%2@7m@+$ z%aC00P6GB8XtF3@#OEw%iol8C*+Q8p=n_;vXtGoe-=nBE4IUKyO%R{-9q*z=jDmyz z`=8qu%@6$Ncsz_BtSW!5;A198DngAT7Oz|R& zBgsyP#@bdD;WE5Y=fsi5V$RWs6THGFjDoT@4tgt|U>|YyF&vaPb77sZc;f2C*AjQ` zi*K?-+#*hu_PqFf=>X?!bPmL_IAr`4(YGUZj&OKS*u}?T;?Z7LY}`TP+Gu}`_wab% z&^Z<3xTmIAu9xgTw%kj62~8~el0C~;-4mvId4t)rSK8{J!@YlRZFBq2CJ3(`X90xs zOW8LkU7o|!u9z2!)3fXq#Sn6w(t+ZiUM0@&pmASR{&mF>(0(z^Lwm&>9_DP5^oQmr z!I>-?F_8(}U&4mI^8&H{cD&}R#)uL<+v@|yIJEn`ozBO6=Or!AM+44X8z-Z_7}~?c z4IT6xgXHJq-G6f1me@Yh4_`gN^~SLV$fo6)bU-^_dPYTk^lLnajcaXZj4kng(DTv# zo_!tW{QY&r?9$xL^Y;PH;K*0o>uj>!XR3AkY&+vl@UcBl1jYfg?|i@*4mvBG!Q;fe zSkVjhc$MZjA3j#Lu1{Dqzc(g_@Oa)2;=>=DLgjco_{#b22)Mjte|UtYy<|%_@%*jhO=&(Oy?3AayFWYU z|7dD zA_KtmJp7wW) zeLH%w@6Kp<_nj`eH##~!&@uXK+?dBQoH}Gk1nxbkFttsV6yn?(Bmz z-wply&DggfzAy7h4=Cp5_+374H|jAGeoo%t&gn9diL zHz)&?lOre=3rZ_RS3EaW+5=#^ic!;$f62Ph@_wyYq>MGz?ngFd-<4}@&Eq01(HC^u z@4QU)VtFcWC^Ay%NE&iaSvd7G;%qD^ebla%o5!_yzceKp<$S@phniPqzKXi3(pdxZ zi8E|{S6RVrI(tW`UPl^x0?jAL#3rthQr@K-#3wEsBl8gD?r@ryPRFhLhMwkGC(qVJ z-tB z`zujuGwuo>K2M%vQv2LEd~4$umD4UJ(*7RnM&Tei?Pj^}EM5Qql+#WvlfB`+>VxLA zL-0#|pPO%I`?AOVd^zntqB-JyJZ;WtxBWNEX@|36(o;S^K3Gn>B<+P#pSH+pXVO_O z&1L(@Y4=gPb7Rms!Z6jnM~*JC`(#UmHs`COjaWYOe1$wFr1QTa-rAf$qt4sUoa=?= z&I{DOHsl(i^ZIt=d?B7V4y(ZvPJ~`9rwZxeTX=r1{1`O0rm6lzy2i%>vbMrOl$qOjkGWSbB*_QbkgUZ^sXq)cu2b8t)V`5FW+Vvb~l(T*tuH+sqs19OQ+`e)IUf)1b3*L+vwHVLR^f6W{I1i9u(f zoJ-^bClc6F)5GQ_q4|()b|b%!lMcr|X8&d84x+h+=S%jl_RgzG%5(Y6&3TX>IF~T_hwSg~Bj+uRXd$mv5q`RmGq^uNDy1L^<( literal 0 HcmV?d00001 diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index ce090ae..fff17b8 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -148,6 +148,14 @@ struct Feature { inline bool initializePosition( const CamStateServer& cam_states); + cv::Point2f pixelDistanceAt( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const CameraCalibration& cam, + Eigen::Vector3d& in_p) const; + + + /* * @brief project PositionToCamera Takes a 3d position in a world frame * and projects it into the passed camera frame using pinhole projection @@ -737,6 +745,37 @@ float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const return ((float)image.at(pose.y, pose.x))/255; } +cv::Point2f Feature::pixelDistanceAt( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const CameraCalibration& cam, + Eigen::Vector3d& in_p) const +{ + + cv::Point2f cam_p = projectPositionToCamera(cam_state, cam_state_id, cam, in_p); + + // create vector of patch in pixel plane + std::vector surroundingPoints; + surroundingPoints.push_back(cv::Point2f(cam_p.x+1, cam_p.y)); + surroundingPoints.push_back(cv::Point2f(cam_p.x-1, cam_p.y)); + surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y+1)); + surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y-1)); + + std::vector pure; + image_handler::undistortPoints(surroundingPoints, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs, + pure); + + // returns the absolute pixel distance at pixels one metres away + cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y)); + + return distance; +} + + + cv::Point2f Feature::projectPositionToCamera( const CAMState& cam_state, const StateIDType& cam_state_id, diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 9e103b7..1d458d2 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,13 +18,13 @@ output="screen"> - + - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index b233848..d56d1a9 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1225,6 +1225,9 @@ void MsckfVio::PhotometricMeasurementJacobian( const CAMState& cam_state = state_server.cam_states[cam_state_id]; const Feature& feature = map_server[feature_id]; + const StateIDType anchor_state_id = feature.observations.begin()->first; + const CAMState anchor_state = state_server.cam_states[anchor_state_id]; + // Cam0 pose. Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Vector3d& t_c0_w = cam_state.position; @@ -1263,7 +1266,7 @@ void MsckfVio::PhotometricMeasurementJacobian( Eigen::MatrixXd H_pA(N*N, 6); auto frame = cam0.moving_window.find(cam_state_id)->second.image; - + auto anchor_frame = cam0.moving_window.find(anchor_state_id)->second.image; //observation const Vector4d& z = feature.observations.find(cam_state_id)->second; @@ -1291,8 +1294,10 @@ void MsckfVio::PhotometricMeasurementJacobian( for (auto point : feature.anchorPatch_3d) { + //cout << "____feature-measurement_____\n" << endl; Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); //add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); @@ -1300,13 +1305,17 @@ void MsckfVio::PhotometricMeasurementJacobian( //calculate photom. residual photo_r.push_back(photo_z[count] - estimate_photo_z[count]); - // add jacobians + //cout << "residual: " << photo_r.back() << endl; + // add jacobians + cv::Point2f pixelDistance = feature.pixelDistanceAt(anchor_state, anchor_state_id, cam0, point); + + // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame); - dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame); - dI_dhj(0, 0) = dx; - dI_dhj(0, 1) = dy; + dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); + dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); + dI_dhj(0, 0) = dx/pixelDistance.x; + dI_dhj(0, 1) = dy/pixelDistance.y; gradientVector.x += dx; gradientVector.y += dy; @@ -1332,8 +1341,9 @@ void MsckfVio::PhotometricMeasurementJacobian( dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta; dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC; - //d{}^Gp_P{ij} / \rho_i + // d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; + // Isometry T_anchor_w takes a vector in anchor frame to world frame dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); @@ -1345,13 +1355,18 @@ void MsckfVio::PhotometricMeasurementJacobian( // Intermediate Jakobians H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1 - H_plj = dI_dhj * dh_dXplj; // 1 x 6 - H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 + H_plj = dI_dhj * dh_dXplj; // 1 x 6 + H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; H_pA.block<1, 6>(count, 0) = H_pAj; + //cout << "H_pl\n" << H_plj << endl; + + //cout << "H_pA\n" << H_pAj << endl; + + //cout << "H_rho\n" << H_rhoj << endl; count++; } @@ -1491,15 +1506,18 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd A = svd_helper.matrixU().rightCols(jacobian_row_size - singularValues.size()); */ + H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; -/* - ofstream myfile; - myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl; - myfile.close(); - cout << "---------- LOGGED -------- " << endl; -*/ + + if(PRINTIMAGES) + { + ofstream myfile; + myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); + myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; + myfile.close(); + cout << "---------- LOGGED -------- " << endl; + } if(PRINTIMAGES) { std::cout << "resume playback" << std::endl; @@ -1642,7 +1660,7 @@ void MsckfVio::featureJacobian( ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.ldlt().solve(r) << endl; + myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; myfile.close(); cout << "---------- LOGGED -------- " << endl; return; @@ -1866,7 +1884,7 @@ void MsckfVio::removeLostFeatures() { else featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) { + if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); @@ -2028,7 +2046,7 @@ void MsckfVio::pruneCamStateBuffer() { else featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) { + if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); From 0d544c5361f57099cd12468c4d35458c32ed32f9 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 24 May 2019 17:22:02 +0200 Subject: [PATCH 40/86] minor changes, no improvements --- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 1d458d2..a28de3f 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,7 +18,7 @@ output="screen"> - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index d56d1a9..16a5951 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1314,8 +1314,8 @@ void MsckfVio::PhotometricMeasurementJacobian( // frame derivative calculated convoluting with kernel [-1, 0, 1] dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); - dI_dhj(0, 0) = dx/pixelDistance.x; - dI_dhj(0, 1) = dy/pixelDistance.y; + dI_dhj(0, 0) = dx/(pixelDistance.x*N*N); + dI_dhj(0, 1) = dy/(pixelDistance.y*N*N); gradientVector.x += dx; gradientVector.y += dy; From 5d36a123a709ac34ee1e29b7897738062e8d12e8 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 28 May 2019 11:04:21 +0200 Subject: [PATCH 41/86] stash --- launch/msckf_vio_tum.launch | 4 ++-- src/msckf_vio.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index a28de3f..eca1ebc 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -21,10 +21,10 @@ - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 16a5951..174be4a 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1303,7 +1303,7 @@ void MsckfVio::PhotometricMeasurementJacobian( photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); //calculate photom. residual - photo_r.push_back(photo_z[count] - estimate_photo_z[count]); + photo_r.push_back((photo_z[count] - estimate_photo_z[count])); //cout << "residual: " << photo_r.back() << endl; @@ -1314,8 +1314,8 @@ void MsckfVio::PhotometricMeasurementJacobian( // frame derivative calculated convoluting with kernel [-1, 0, 1] dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); - dI_dhj(0, 0) = dx/(pixelDistance.x*N*N); - dI_dhj(0, 1) = dy/(pixelDistance.y*N*N); + dI_dhj(0, 0) = dx/(pixelDistance.x); + dI_dhj(0, 1) = dy/(pixelDistance.y); gradientVector.x += dx; gradientVector.y += dy; From 2a16fb2fc5091b5e00bd00b98713fc1a266621e8 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 31 May 2019 11:38:49 +0200 Subject: [PATCH 42/86] added sobel kernel for image gradient calcualtion --- .vscode/c_cpp_properties.json | 16 ++++++++++ .../ipch/778a17e566a4909e/mmap_address.bin | Bin 0 -> 8 bytes .../ipch/ccf983af1f87ec2b/mmap_address.bin | Bin 0 -> 8 bytes .../ipch/e40aedd19a224f8d/mmap_address.bin | Bin 0 -> 8 bytes .vscode/settings.json | 6 ++++ include/msckf_vio/feature.hpp | 30 ++++++++++++++++++ launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 9 ++++-- 8 files changed, 60 insertions(+), 3 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/ipch/778a17e566a4909e/mmap_address.bin create mode 100644 .vscode/ipch/ccf983af1f87ec2b/mmap_address.bin create mode 100644 .vscode/ipch/e40aedd19a224f8d/mmap_address.bin create mode 100644 .vscode/settings.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..3d22904 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,16 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "cStandard": "c11", + "cppStandard": "c++14", + "intelliSenseMode": "clang-x64" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/ipch/778a17e566a4909e/mmap_address.bin b/.vscode/ipch/778a17e566a4909e/mmap_address.bin new file mode 100644 index 0000000000000000000000000000000000000000..42269a4348b4a6e1aa772ddb798bcf093c10412f GIT binary patch literal 8 PcmZQzV92>$&%gix2Mqzo literal 0 HcmV?d00001 diff --git a/.vscode/ipch/ccf983af1f87ec2b/mmap_address.bin b/.vscode/ipch/ccf983af1f87ec2b/mmap_address.bin new file mode 100644 index 0000000000000000000000000000000000000000..bea14df956f2662a8833d5673c14976bdf2072f3 GIT binary patch literal 8 PcmZQzU|4Xvo`C@X2hjoR literal 0 HcmV?d00001 diff --git a/.vscode/ipch/e40aedd19a224f8d/mmap_address.bin b/.vscode/ipch/e40aedd19a224f8d/mmap_address.bin new file mode 100644 index 0000000000000000000000000000000000000000..3e2f12af61ae10c427911503226fc90588481369 GIT binary patch literal 8 PcmZQzVDLz)XJ7yT1a$!_ literal 0 HcmV?d00001 diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..2086c47 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "files.associations": { + "core": "cpp", + "sparsecore": "cpp" + } +} \ No newline at end of file diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index fff17b8..2699104 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -168,6 +168,11 @@ struct Feature { const CameraCalibration& cam, Eigen::Vector3d& in_p) const; + double Kernel( + const cv::Point2f pose, + const cv::Mat frame, + std::string type) const; + /* * @brief IrradianceAnchorPatch_Camera returns irradiance values * of the Anchor Patch position in a camera frame @@ -387,6 +392,31 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const else return false; } +double Feature::Kernel( + const cv::Point2f pose, + const cv::Mat frame, + std::string type) const +{ +Eigen::Matrix kernel = Eigen::Matrix::Zero(); +if(type == "Sobel_x") + kernel << -1., 0., 1.,-2., 0., 2. , -1., 0., 1.; +else if(type == "Sobel_y") + kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.; + +double delta = 0; +int offs = (int)(kernel.rows()-1)/2; +for(int i = 0; i < kernel.rows(); i++){ + + for(int j = 0; j < kernel.cols(); j++) + { + std::cout << "i: " << i << ":" << "j: " << j << ":" << kernel(i,j) << std::endl; + std::cout <<"pose: " << pose.y+i-offs << " : " << pose.x+j-offs << std::endl; + delta += ((float)frame.at(pose.y+i-offs , pose.x+j-offs))/255 * (float)kernel(i,j); + } +} +std::cout << "delta " << delta << std::endl; +return delta; +} bool Feature::estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index eca1ebc..911f36b 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -24,7 +24,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 174be4a..203603b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1312,8 +1312,13 @@ void MsckfVio::PhotometricMeasurementJacobian( // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); - dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); + + dx = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_x"); + dy = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_y"); + + // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); + // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); + dI_dhj(0, 0) = dx/(pixelDistance.x); dI_dhj(0, 1) = dy/(pixelDistance.y); From 5f6bcd178465df26fdc99dd14632a108913255b7 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 4 Jun 2019 17:38:11 +0200 Subject: [PATCH 43/86] added direct levenberg marqhart estimation for rho, was previously calculated from feature position --- include/msckf_vio/feature.hpp | 265 +++++++++++++++++++++++++++++++--- launch/msckf_vio_tum.launch | 6 +- src/msckf_vio.cpp | 65 ++++++--- 3 files changed, 296 insertions(+), 40 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 2699104..a47c142 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -70,6 +70,11 @@ struct Feature { position(Eigen::Vector3d::Zero()), is_initialized(false), is_anchored(false) {} + +void Rhocost(const Eigen::Isometry3d& T_c0_ci, + const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2, + double& e) const; + /* * @brief cost Compute the cost of the camera observations * @param T_c0_c1 A rigid body transformation takes @@ -82,6 +87,13 @@ struct Feature { const Eigen::Vector3d& x, const Eigen::Vector2d& z, double& e) const; + bool initializeRho(const CamStateServer& cam_states); + + inline void RhoJacobian(const Eigen::Isometry3d& T_c0_ci, + const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2, + Eigen::Matrix& J, Eigen::Vector2d& r, + double& w) const; + /* * @brief jacobian Compute the Jacobian of the camera observation * @param T_c0_c1 A rigid body transformation takes @@ -97,6 +109,10 @@ struct Feature { Eigen::Matrix& J, Eigen::Vector2d& r, double& w) const; + inline double generateInitialDepth( + const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1, + const Eigen::Vector2d& z2) const; + /* * @brief generateInitialGuess Compute the initial guess of * the feature's 3d position using only two views. @@ -194,7 +210,7 @@ bool MarkerGeneration( const CAMState& cam_state, const StateIDType& cam_state_id, CameraCalibration& cam0, - const std::vector photo_r, + const Eigen::VectorXd& photo_r, std::stringstream& ss, cv::Point2f gradientVector, cv::Point2f residualVector) const; @@ -263,6 +279,26 @@ typedef std::map, Eigen::aligned_allocator< std::pair > > MapServer; +void Feature::Rhocost(const Eigen::Isometry3d& T_c0_ci, + const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2, + double& e) const +{ + // Compute hi1, hi2, and hi3 as Equation (37). + const double& rho = x; + + Eigen::Vector3d h = T_c0_ci.linear()* + Eigen::Vector3d(z1(0), z1(1), 1.0) + rho*T_c0_ci.translation(); + double& h1 = h(0); + double& h2 = h(1); + double& h3 = h(2); + + // Predict the feature observation in ci frame. + Eigen::Vector2d z_hat(h1/h3, h2/h3); + + // Compute the residual. + e = (z_hat-z2).squaredNorm(); + return; +} void Feature::cost(const Eigen::Isometry3d& T_c0_ci, const Eigen::Vector3d& x, const Eigen::Vector2d& z, @@ -275,9 +311,9 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci, Eigen::Vector3d h = T_c0_ci.linear()* Eigen::Vector3d(alpha, beta, 1.0) + rho*T_c0_ci.translation(); - double& h1 = h(0); - double& h2 = h(1); - double& h3 = h(2); + double h1 = h(0); + double h2 = h(1); + double h3 = h(2); // Predict the feature observation in ci frame. Eigen::Vector2d z_hat(h1/h3, h2/h3); @@ -287,6 +323,43 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci, return; } +void Feature::RhoJacobian(const Eigen::Isometry3d& T_c0_ci, + const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2, + Eigen::Matrix& J, Eigen::Vector2d& r, + double& w) const +{ + + // Compute hi1, hi2, and hi3 as Equation (37). + const double& rho = x; + + Eigen::Vector3d h = T_c0_ci.linear()* + Eigen::Vector3d(z1(0), z2(1), 1.0) + rho*T_c0_ci.translation(); + double& h1 = h(0); + double& h2 = h(1); + double& h3 = h(2); + + // Compute the Jacobian. + Eigen::Matrix3d W; + W.leftCols<2>() = T_c0_ci.linear().leftCols<2>(); + W.rightCols<1>() = T_c0_ci.translation(); + + J(0,0) = -h1/(h3*h3); + J(1,0) = -h2/(h3*h3); + + // Compute the residual. + Eigen::Vector2d z_hat(h1/h3, h2/h3); + r = z_hat - z2; + + // Compute the weight based on the residual. + double e = r.norm(); + if (e <= optimization_config.huber_epsilon) + w = 1.0; + else + w = optimization_config.huber_epsilon / (2*e); + + return; +} + void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci, const Eigen::Vector3d& x, const Eigen::Vector2d& z, Eigen::Matrix& J, Eigen::Vector2d& r, @@ -326,9 +399,9 @@ void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci, return; } -void Feature::generateInitialGuess( +double Feature::generateInitialDepth( const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1, - const Eigen::Vector2d& z2, Eigen::Vector3d& p) const + const Eigen::Vector2d& z2) const { // Construct a least square problem to solve the depth. Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0); @@ -343,6 +416,15 @@ void Feature::generateInitialGuess( // Solve for the depth. double depth = (A.transpose() * A).inverse() * A.transpose() * b; + return depth; +} + + +void Feature::generateInitialGuess( + const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1, + const Eigen::Vector2d& z2, Eigen::Vector3d& p) const +{ + double depth = generateInitialDepth(T_c1_c2, z1, z2); p(0) = z1(0) * depth; p(1) = z1(1) * depth; p(2) = depth; @@ -405,16 +487,11 @@ else if(type == "Sobel_y") double delta = 0; int offs = (int)(kernel.rows()-1)/2; -for(int i = 0; i < kernel.rows(); i++){ +for(int i = 0; i < kernel.rows(); i++) for(int j = 0; j < kernel.cols(); j++) - { - std::cout << "i: " << i << ":" << "j: " << j << ":" << kernel(i,j) << std::endl; - std::cout <<"pose: " << pose.y+i-offs << " : " << pose.x+j-offs << std::endl; - delta += ((float)frame.at(pose.y+i-offs , pose.x+j-offs))/255 * (float)kernel(i,j); - } -} -std::cout << "delta " << delta << std::endl; + delta += ((float)frame.at(pose.y+j-offs , pose.x+i-offs))/255 * (float)kernel(j,i); + return delta; } bool Feature::estimate_FrameIrradiance( @@ -572,7 +649,7 @@ bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, CameraCalibration& cam0, - const std::vector photo_r, + const Eigen::VectorXd& photo_r, std::stringstream& ss, cv::Point2f gradientVector, cv::Point2f residualVector) const @@ -685,17 +762,17 @@ bool Feature::VisualizePatch( for(int i = 0; i0) + if(photo_r(i*N+j)>0) cv::rectangle(irradianceFrame, cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), - cv::Scalar(255 - photo_r[i*N+j]*255, 255 - photo_r[i*N+j]*255, 255), + cv::Scalar(255 - photo_r(i*N+j)*255, 255 - photo_r(i*N+j)*255, 255), CV_FILLED); else cv::rectangle(irradianceFrame, cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), - cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), + cv::Scalar(255, 255 + photo_r(i*N+j)*255, 255 + photo_r(i*N+j)*255), CV_FILLED); // gradient arrow @@ -911,9 +988,160 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) return true; } +bool Feature::initializeRho(const CamStateServer& cam_states) { + + // Organize camera poses and feature observations properly. + std::vector > cam_poses(0); + std::vector > measurements(0); + + for (auto& m : observations) { + auto cam_state_iter = cam_states.find(m.first); + if (cam_state_iter == cam_states.end()) continue; + + // Add the measurement. + measurements.push_back(m.second.head<2>()); + measurements.push_back(m.second.tail<2>()); + + // This camera pose will take a vector from this camera frame + // to the world frame. + Eigen::Isometry3d cam0_pose; + cam0_pose.linear() = quaternionToRotation( + cam_state_iter->second.orientation).transpose(); + cam0_pose.translation() = cam_state_iter->second.position; + + Eigen::Isometry3d cam1_pose; + cam1_pose = cam0_pose * CAMState::T_cam0_cam1.inverse(); + + cam_poses.push_back(cam0_pose); + cam_poses.push_back(cam1_pose); + } + + // All camera poses should be modified such that it takes a + // vector from the first camera frame in the buffer to this + // camera frame. + Eigen::Isometry3d T_c0_w = cam_poses[0]; + T_anchor_w = T_c0_w; + for (auto& pose : cam_poses) + pose = pose.inverse() * T_c0_w; + + // Generate initial guess + double initial_depth = 0; + initial_depth = generateInitialDepth(cam_poses[cam_poses.size()-1], measurements[0], + measurements[measurements.size()-1]); + + double solution = 1.0/initial_depth; + + // Apply Levenberg-Marquart method to solve for the 3d position. + double lambda = optimization_config.initial_damping; + int inner_loop_cntr = 0; + int outer_loop_cntr = 0; + bool is_cost_reduced = false; + double delta_norm = 0; + + // Compute the initial cost. + double total_cost = 0.0; + for (int i = 0; i < cam_poses.size(); ++i) { + double this_cost = 0.0; + Rhocost(cam_poses[i], solution, measurements[0], measurements[i], this_cost); + total_cost += this_cost; + } + + // Outer loop. + do { + Eigen::Matrix A = Eigen::Matrix::Zero(); + Eigen::Matrix b = Eigen::Matrix::Zero(); + + for (int i = 0; i < cam_poses.size(); ++i) { + Eigen::Matrix J; + Eigen::Vector2d r; + double w; + + RhoJacobian(cam_poses[i], solution, measurements[0], measurements[i], J, r, w); + + if (w == 1) { + A += J.transpose() * J; + b += J.transpose() * r; + } else { + double w_square = w * w; + A += w_square * J.transpose() * J; + b += w_square * J.transpose() * r; + } + } + + // Inner loop. + // Solve for the delta that can reduce the total cost. + do { + Eigen::Matrix damper = lambda*Eigen::Matrix::Identity(); + Eigen::Matrix delta = (A+damper).ldlt().solve(b); + double new_solution = solution - delta(0,0); + delta_norm = delta.norm(); + + double new_cost = 0.0; + for (int i = 0; i < cam_poses.size(); ++i) { + double this_cost = 0.0; + Rhocost(cam_poses[i], new_solution, measurements[0], measurements[i], this_cost); + new_cost += this_cost; + } + + if (new_cost < total_cost) { + is_cost_reduced = true; + solution = new_solution; + total_cost = new_cost; + lambda = lambda/10 > 1e-10 ? lambda/10 : 1e-10; + } else { + is_cost_reduced = false; + lambda = lambda*10 < 1e12 ? lambda*10 : 1e12; + } + + } while (inner_loop_cntr++ < + optimization_config.inner_loop_max_iteration && !is_cost_reduced); + + inner_loop_cntr = 0; + + } while (outer_loop_cntr++ < + optimization_config.outer_loop_max_iteration && + delta_norm > optimization_config.estimation_precision); + + // Covert the feature position from inverse depth + // representation to its 3d coordinate. + Eigen::Vector3d final_position(measurements[0](0)/solution, + measurements[0](1)/solution, 1.0/solution); + + // Check if the solution is valid. Make sure the feature + // is in front of every camera frame observing it. + bool is_valid_solution = true; + for (const auto& pose : cam_poses) { + Eigen::Vector3d position = + pose.linear()*final_position + pose.translation(); + if (position(2) <= 0) { + is_valid_solution = false; + break; + } + } + + //save inverse depth distance from camera + anchor_rho = solution; + + // Convert the feature position to the world frame. + position = T_c0_w.linear()*final_position + T_c0_w.translation(); + + if (is_valid_solution) + is_initialized = true; + + return is_valid_solution; +} + + bool Feature::initializePosition(const CamStateServer& cam_states) { // Organize camera poses and feature observations properly. + + +initializeRho(cam_states); +std::cout << "init rho is: " << anchor_rho << std::endl; + std::vector > cam_poses(0); std::vector - + - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 203603b..4a03598 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1240,9 +1240,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //photometric observation std::vector photo_z; - - std::vector photo_r; - + VectorXd r_photo = VectorXd::Zero(N*N); // individual Jacobians Matrix dI_dhj = Matrix::Zero(); Matrix dh_dCpij = Matrix::Zero(); @@ -1303,8 +1301,7 @@ void MsckfVio::PhotometricMeasurementJacobian( photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); //calculate photom. residual - photo_r.push_back((photo_z[count] - estimate_photo_z[count])); - + r_photo(count) = photo_z[count] - estimate_photo_z[count]; //cout << "residual: " << photo_r.back() << endl; // add jacobians @@ -1325,9 +1322,10 @@ void MsckfVio::PhotometricMeasurementJacobian( gradientVector.x += dx; gradientVector.y += dy; - residualVector.x += dx * photo_r[count]; - residualVector.y += dy * photo_r[count]; - res_sum += photo_r[count]; + + residualVector.x += dx * r_photo(count); + residualVector.y += dy * r_photo(count); + res_sum += r_photo(count); //dh / d{}^Cp_{ij} dh_dCpij(0, 0) = 1 / p_c0(2); @@ -1348,7 +1346,6 @@ void MsckfVio::PhotometricMeasurementJacobian( // d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; - // Isometry T_anchor_w takes a vector in anchor frame to world frame dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); @@ -1363,6 +1360,12 @@ void MsckfVio::PhotometricMeasurementJacobian( H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 + /* + cout << endl; + std::cout << H_plj << endl; + cout << r_photo.segment(count, 1) << endl; + std::cout << H_plj.colPivHouseholderQr().solve(r_photo.segment(count, 1)) << std::endl; + */ H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; H_pA.block<1, 6>(count, 0) = H_pAj; @@ -1375,6 +1378,14 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } + /* + std::cout << "\n\n frame change through patch" << std::endl; + std::cout << H_pl << std::endl; + std::cout << r_photo << std::endl; + std::cout << endl; + std::cout << H_pl.colPivHouseholderQr().solve(r_photo) << std::endl; + */ + MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); @@ -1408,16 +1419,14 @@ void MsckfVio::PhotometricMeasurementJacobian( H_y = H_yl; //TODO make this more fluent as well - count = 0; - for(auto data : photo_r) - r[count++] = data; + r = r_photo; std::stringstream ss; ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss, gradientVector, residualVector); + //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); } return; @@ -1463,6 +1472,8 @@ void MsckfVio::PhotometricFeatureJacobian( if (feature.observations.find(cam_id) == feature.observations.end()) continue; + if (feature.observations.find(cam_id) == feature.observations.begin()) + continue; valid_cam_state_ids.push_back(cam_id); } @@ -1519,6 +1530,7 @@ void MsckfVio::PhotometricFeatureJacobian( { ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); + myfile << "Hxi\n" << H_xi << "ri\n" << r_i << "Hyi\n" << H_yi << endl; myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; myfile.close(); cout << "---------- LOGGED -------- " << endl; @@ -1601,6 +1613,13 @@ void MsckfVio::featureJacobian( const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { + // stop playing bagfile if printing images + if(PRINTIMAGES) + { + std::cout << "stopped playpack" << std::endl; + nh.setParam("/play_bag", false); + } + const auto& feature = map_server[feature_id]; @@ -1662,12 +1681,20 @@ void MsckfVio::featureJacobian( H_x = A.transpose() * H_xj; r = A.transpose() * r_j; - - ofstream myfile; - myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; - myfile.close(); - cout << "---------- LOGGED -------- " << endl; + + // stop playing bagfile if printing images + if(PRINTIMAGES) + { + + ofstream myfile; + myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); + myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; + myfile.close(); + cout << "---------- LOGGED -------- " << endl; + std::cout << "stopped playpack" << std::endl; + nh.setParam("/play_bag", true); + } + return; } From 9b4bf1284644834906c6b77389be9f3b8f50f416 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 11 Jun 2019 09:53:43 +0200 Subject: [PATCH 44/86] removed incorrect prints --- include/msckf_vio/feature.hpp | 5 - include/msckf_vio/msckf_vio.h | 2 + src/msckf_vio.cpp | 168 +++++++++++++++++++++++++++++----- 3 files changed, 146 insertions(+), 29 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index a47c142..f5a0d1e 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -329,7 +329,6 @@ void Feature::RhoJacobian(const Eigen::Isometry3d& T_c0_ci, double& w) const { - // Compute hi1, hi2, and hi3 as Equation (37). const double& rho = x; Eigen::Vector3d h = T_c0_ci.linear()* @@ -1138,10 +1137,6 @@ bool Feature::initializePosition(const CamStateServer& cam_states) { // Organize camera poses and feature observations properly. - -initializeRho(cam_states); -std::cout << "init rho is: " << anchor_rho << std::endl; - std::vector > cam_poses(0); std::vector& rm_cam_state_ids); + + void pruneLastCamStateBuffer(); void pruneCamStateBuffer(); // Reset the system online if the uncertainty is too large. void onlineReset(); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 4a03598..74b689d 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -475,7 +475,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); start_time = ros::Time::now(); - pruneCamStateBuffer(); + pruneLastCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); @@ -1360,31 +1360,13 @@ void MsckfVio::PhotometricMeasurementJacobian( H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 - /* - cout << endl; - std::cout << H_plj << endl; - cout << r_photo.segment(count, 1) << endl; - std::cout << H_plj.colPivHouseholderQr().solve(r_photo.segment(count, 1)) << std::endl; - */ H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; H_pA.block<1, 6>(count, 0) = H_pAj; - //cout << "H_pl\n" << H_plj << endl; - - //cout << "H_pA\n" << H_pAj << endl; - - //cout << "H_rho\n" << H_rhoj << endl; count++; } - /* - std::cout << "\n\n frame change through patch" << std::endl; - std::cout << H_pl << std::endl; - std::cout << r_photo << std::endl; - std::cout << endl; - std::cout << H_pl.colPivHouseholderQr().solve(r_photo) << std::endl; - */ MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); @@ -1426,7 +1408,7 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); + feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); } return; @@ -1530,9 +1512,10 @@ void MsckfVio::PhotometricFeatureJacobian( { ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "Hxi\n" << H_xi << "ri\n" << r_i << "Hyi\n" << H_yi << endl; - myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; + myfile << "Hxi\n" << H_xi << "ri\n" << r_i << "Hyi\n" << H_yi << endl; + myfile << "kernel\n" << A_null_space << endl; myfile.close(); + myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; cout << "---------- LOGGED -------- " << endl; } if(PRINTIMAGES) @@ -1860,7 +1843,7 @@ void MsckfVio::removeLostFeatures() { invalid_feature_ids.push_back(feature.id); continue; } else { - if(!feature.initializePosition(state_server.cam_states)) { + if(!feature.initializeRho(state_server.cam_states)) { invalid_feature_ids.push_back(feature.id); continue; } @@ -1989,6 +1972,143 @@ void MsckfVio::findRedundantCamStates(vector& rm_cam_state_ids) { return; } +void MsckfVio::pruneLastCamStateBuffer() +{ + if (state_server.cam_states.size() < max_cam_state_size) + return; + + auto rm_cam_state_id = state_server.cam_states.begin()->first; + + // Set the size of the Jacobian matrix. + int jacobian_row_size = 0; + int augmentationSize = 6; + if(PHOTOMETRIC) + augmentationSize = 7; + + //initialize all the features which are going to be removed + for (auto& item : map_server) { + auto& feature = item.second; + + // check if feature is involved, if not continue + if (feature.observations.find(rm_cam_state_id) == + feature.observations.end()) + continue; + + + if (!feature.is_initialized) { + + // Check if the feature can be initialized + if (!feature.checkMotion(state_server.cam_states)) { + + // remove any features that can't be initialized + feature.observations.erase(rm_cam_state_id); + continue; + } else { + if(!feature.initializeRho(state_server.cam_states)) { + feature.observations.erase(rm_cam_state_id); + continue; + } + } + } + if(!feature.is_anchored) + { + if(!feature.initializeAnchor(cam0, N)) + { + feature.observations.erase(rm_cam_state_id); + continue; + } + } + + if(PHOTOMETRIC) + //just use max. size, as gets shrunken down after anyway + jacobian_row_size += N*N*feature.observations.size(); + else + jacobian_row_size += 4*feature.observations.size() - 3; + + } + + MatrixXd H_xj; + VectorXd r_j; + MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size()); + VectorXd r = VectorXd::Zero(jacobian_row_size); + int stack_cntr = 0; + + for (auto& item : map_server) { + auto& feature = item.second; + + // check if feature is involved, if not continue + if (feature.observations.find(rm_cam_state_id) == + feature.observations.end()) + continue; + + + vector involved_cam_state_ids(0); + for (const auto& cam_state : state_server.cam_states) + involved_cam_state_ids.push_back(cam_state.first); + + if(PHOTOMETRIC) + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + else + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + + if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { + H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; + r.segment(stack_cntr, r_j.rows()) = r_j; + stack_cntr += H_xj.rows(); + cout << "passed" << endl; + } + else + { + cout << "rejected" << endl; + } + for (const auto& cam_id : involved_cam_state_ids) + feature.observations.erase(cam_id); + } + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + + // Perform measurement update. + measurementUpdate(H_x, r); + + //reduction + int cam_sequence = std::distance(state_server.cam_states.begin(), + state_server.cam_states.find(rm_cam_state_id)); + int cam_state_start = 21 + augmentationSize*cam_sequence; + int cam_state_end = cam_state_start + augmentationSize; + + + // Remove the corresponding rows and columns in the state + // covariance matrix. + if (cam_state_end < state_server.state_cov.rows()) { + state_server.state_cov.block(cam_state_start, 0, + state_server.state_cov.rows()-cam_state_end, + state_server.state_cov.cols()) = + state_server.state_cov.block(cam_state_end, 0, + state_server.state_cov.rows()-cam_state_end, + state_server.state_cov.cols()); + + state_server.state_cov.block(0, cam_state_start, + state_server.state_cov.rows(), + state_server.state_cov.cols()-cam_state_end) = + state_server.state_cov.block(0, cam_state_end, + state_server.state_cov.rows(), + state_server.state_cov.cols()-cam_state_end); + + state_server.state_cov.conservativeResize( + state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + } else { + state_server.state_cov.conservativeResize( + state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + } + + // Remove this camera state in the state vector. + state_server.cam_states.erase(rm_cam_state_id); + cam0.moving_window.erase(rm_cam_state_id); + cam1.moving_window.erase(rm_cam_state_id); + + return; +} + void MsckfVio::pruneCamStateBuffer() { if (state_server.cam_states.size() < max_cam_state_size) @@ -2030,7 +2150,7 @@ void MsckfVio::pruneCamStateBuffer() { feature.observations.erase(cam_id); continue; } else { - if(!feature.initializePosition(state_server.cam_states)) { + if(!feature.initializeRho(state_server.cam_states)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); continue; From 44fffa19a252244650fbc2ef3075d9b66bb22573 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 11 Jun 2019 10:59:41 +0200 Subject: [PATCH 45/86] minor scale change in distance between pixels --- include/msckf_vio/feature.hpp | 10 ++++++++-- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 32 ++++---------------------------- 3 files changed, 13 insertions(+), 31 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index f5a0d1e..63cfd60 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -874,8 +874,14 @@ cv::Point2f Feature::pixelDistanceAt( cam.distortion_coeffs, pure); - // returns the absolute pixel distance at pixels one metres away - cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y)); + // transfrom position to camera frame + // to get distance multiplier + Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); + const Eigen::Vector3d& t_c0_w = cam_state.position; + Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); + + // returns the distance between the pixel points in space + cv::Point2f distance(fabs(pure[0].x - pure[1].x)*p_c0[2], fabs(pure[2].y - pure[3].y)*p_c0[2]); return distance; } diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 1d458d2..a28de3f 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,7 +18,7 @@ output="screen"> - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 74b689d..a6313b3 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1427,23 +1427,6 @@ void MsckfVio::PhotometricFeatureJacobian( nh.setParam("/play_bag", false); } - - // Errstate - calcErrorState(); - /* - std::cout << "--- error state: ---\n " << std::endl; - std::cout << "imu orientation:\n " << err_state_server.imu_state.orientation << std::endl; - std::cout << "imu position\n" << err_state_server.imu_state.position << std::endl; - - int count = 0; - for(auto cam_state : err_state_server.cam_states) - { - std::cout << " - cam " << count++ << " - \n" << std::endl; - std::cout << "orientation: " << cam_state.second.orientation(0) << cam_state.second.orientation(1) << " " << cam_state.second.orientation(2) << " " << std::endl; - std::cout << "position:" << cam_state.second.position(0) << " " << cam_state.second.position(1) << " " << cam_state.second.position(2) << std::endl; - } - */ - const auto& feature = map_server[feature_id]; @@ -1493,21 +1476,12 @@ void MsckfVio::PhotometricFeatureJacobian( // get Nullspace FullPivLU lu(H_yi.transpose()); MatrixXd A_null_space = lu.kernel(); - /* - JacobiSVD svd_helper(H_yi, ComputeFullU | ComputeThinV); - - int sv_size = 0; - Eigen::VectorXd singularValues = svd_helper.singularValues(); - for(int i = 0; i < singularValues.size(); i++) - if(singularValues[i] > 1e-12) - sv_size++; - - MatrixXd A = svd_helper.matrixU().rightCols(jacobian_row_size - singularValues.size()); - */ H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; + cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; + if(PRINTIMAGES) { ofstream myfile; @@ -1665,6 +1639,8 @@ void MsckfVio::featureJacobian( r = A.transpose() * r_j; + cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; + // stop playing bagfile if printing images if(PRINTIMAGES) { From acca4ab0187b3d121b1187f26557d05ec4f23aec Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 12 Jun 2019 18:39:40 +0200 Subject: [PATCH 46/86] added kernel visualization --- include/msckf_vio/feature.hpp | 48 ++++++++++++++++++-- launch/msckf_vio_debug_tum.launch | 4 +- launch/msckf_vio_tum.launch | 2 +- log | 73 +++++++++++++++++++++++++++++++ src/msckf_vio.cpp | 44 ++++++++++++++++--- 5 files changed, 157 insertions(+), 14 deletions(-) create mode 100644 log diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 63cfd60..b094d49 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -206,6 +206,11 @@ bool MarkerGeneration( ros::Publisher& marker_pub, const CamStateServer& cam_states) const; +bool VisualizeKernel( + const CAMState& cam_state, + const StateIDType& cam_state_id, + CameraCalibration& cam0) const; + bool VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, @@ -480,16 +485,16 @@ double Feature::Kernel( { Eigen::Matrix kernel = Eigen::Matrix::Zero(); if(type == "Sobel_x") - kernel << -1., 0., 1.,-2., 0., 2. , -1., 0., 1.; + kernel << -3., 0., 3.,-10., 0., 10. , -3., 0., 3.; else if(type == "Sobel_y") - kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.; + kernel << -3., -10., -3., 0., 0., 0., 3., 10., 3.; double delta = 0; int offs = (int)(kernel.rows()-1)/2; for(int i = 0; i < kernel.rows(); i++) for(int j = 0; j < kernel.cols(); j++) - delta += ((float)frame.at(pose.y+j-offs , pose.x+i-offs))/255 * (float)kernel(j,i); + delta += ((float)frame.at(pose.y+j-offs , pose.x+i-offs))/255. * (float)kernel(j,i); return delta; } @@ -644,6 +649,41 @@ bool Feature::MarkerGeneration( marker_pub.publish(ma); } + +bool Feature::VisualizeKernel( + const CAMState& cam_state, + const StateIDType& cam_state_id, + CameraCalibration& cam0) const +{ + auto anchor = observations.begin(); + cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image; + + cv::Mat xderImage; + cv::Mat yderImage; + + cv::Sobel(anchorImage, xderImage, CV_8UC1, 1, 0, 3); + cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3); + + + cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, yderImage.type()); + cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, yderImage.type()); + + cv::imshow("xder", xderImage); + //cv::imshow("yder", yderImage); + + for(int i = 1; i < anchorImage.rows-1; i++) + for(int j = 1; j < anchorImage.cols-1; j++) + xderImage2.at(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_x"); + + for(int i = 1; i < anchorImage.rows-1; i++) + for(int j = 1; j < anchorImage.cols-1; j++) + yderImage2.at(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_y"); + cv::imshow("anchor", anchorImage); + cv::imshow("xder2", xderImage2); + //cv::imshow("yder2", yderImage2); + + cvWaitKey(0); +} bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, @@ -848,7 +888,7 @@ bool Feature::VisualizePatch( float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const { - return ((float)image.at(pose.y, pose.x))/255; + return ((float)image.at(pose.y, pose.x))/255.; } cv::Point2f Feature::pixelDistanceAt( diff --git a/launch/msckf_vio_debug_tum.launch b/launch/msckf_vio_debug_tum.launch index c4e12e7..4796014 100644 --- a/launch/msckf_vio_debug_tum.launch +++ b/launch/msckf_vio_debug_tum.launch @@ -22,10 +22,10 @@ - + - + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index a28de3f..0fc464a 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -21,7 +21,7 @@ - + diff --git a/log b/log new file mode 100644 index 0000000..1e2f876 --- /dev/null +++ b/log @@ -0,0 +1,73 @@ +# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST +# name: Hx +# type: matrix +# rows: 18 +# columns: 49 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 313.795 -58.7912 139.778 -46.7616 144.055 86.9644 0 -314.123 55.6434 -140.648 46.7616 -144.055 -86.9644 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 441.06 -94.50069999999999 174.424 -53.7653 204.822 120.248 0 -441.685 90.1101 -175.657 53.7653 -204.822 -120.248 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 225.35 -54.5629 77.60599999999999 -21.1425 105.886 60.3706 0 -225.756 52.3373 -78.2406 21.1425 -105.886 -60.3706 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 175.128 20.6203 175.127 -79.63939999999999 73.245 62.1868 0 -174.573 -22.5235 -175.576 79.63939999999999 -73.245 -62.1868 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 296.962 43.5469 311.307 -143.667 123.399 108.355 0 -295.905 -46.7952 -312.063 143.667 -123.399 -108.355 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 126.283 117.889 311.864 -161.264 38.8521 71.8019 0 -124.464 -119.541 -312.118 161.264 -38.8521 -71.8019 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 49.2502 63.7166 155.071 -80.81950000000001 12.7732 32.1826 0 -48.2934 -64.4113 -155.157 80.81950000000001 -12.7732 -32.1826 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 69.59699999999999 154.579 335.384 -179.355 9.212580000000001 62.0364 0 -67.35599999999999 -155.735 -335.462 179.355 -9.212580000000001 -62.0364 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -66.6965 304.947 500.218 -285.589 -71.31010000000001 55.5058 0 70.8009 -305.077 -499.831 285.589 71.31010000000001 -55.5058 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 323.404 -62.2043 141.092 -46.6015 148.737 89.1108 0 0 0 0 0 0 0 0 -324.336 57.8552 -141.991 46.6015 -148.737 -89.1108 1 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 454.208 -99.3095 175.986 -53.4094 211.276 123.174 0 0 0 0 0 0 0 0 -455.779 93.0992 -177.158 53.4094 -211.276 -123.174 1 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 231.884 -57.0266 78.2559 -20.8926 109.118 61.8183 0 0 0 0 0 0 0 0 -232.824 53.8025 -78.80719999999999 20.8926 -109.118 -61.8183 1 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 181.715 18.8525 177.045 -80.08499999999999 76.3716 63.8254 0 0 0 0 0 0 0 0 -181.07 -20.839 -177.959 80.08499999999999 -76.3716 -63.8254 1 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 308.249 40.5812 314.711 -144.494 128.766 111.207 0 0 0 0 0 0 0 0 -306.972 -43.8825 -316.328 144.494 -128.766 -111.207 1 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 133.309 116.865 315.474 -162.598 42.0763 73.8353 0 0 0 0 0 0 0 0 -130.603 -117.454 -316.931 162.598 -42.0763 -73.8353 1 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 52.4426 63.3607 156.902 -81.5288 14.2139 33.1222 0 0 0 0 0 0 0 0 -50.9976 -63.4393 -157.607 81.5288 -14.2139 -33.1222 1 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 75.5508 154.234 339.369 -180.995 11.859 63.8956 0 0 0 0 0 0 0 0 -72.1041 -153.816 -340.865 180.995 -11.859 -63.8956 1 0 0 0 0 0 0 0 + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -62.0551 306.357 506.351 -288.542 -69.50409999999999 57.4562 0 0 0 0 0 0 0 0 68.6262 -303.028 -508.423 288.542 69.50409999999999 -57.4562 1 0 0 0 0 0 0 0 + + +# name: Hy +# type: matrix +# rows: 18 +# columns: 14 + 1.56267 0 0 0 0 0 0 0 0 0 0.252873 0 0 0.110387 + 0 1.56267 0 0 0 0 0 0 0 0 0.453168 0 0 0.162961 + 0 0 1.56267 0 0 0 0 0 0 0 0.638441 0 0 0.0873758 + 0 0 0 1.56267 0 0 0 0 0 0 0.21031 0 0 0.0321517 + 0 0 0 0 1.56267 0 0 0 0 0 0.375554 0 0 0.0505151 + 0 0 0 0 0 1.56267 0 0 0 0 0.638441 0 0 -0.0336034 + 0 0 0 0 0 0 1.56267 0 0 0 0.157733 0 0 -0.0232704 + 0 0 0 0 0 0 0 1.56267 0 0 0.212814 0 0 -0.0688343 + 0 0 0 0 0 0 0 0 1.56267 0 0.360532 0 0 -0.187745 + 1.56267 0 0 0 0 0 0 0 0 0 0 0.252873 0 0.322811 + 0 1.56267 0 0 0 0 0 0 0 0 0 0.453168 0 0.467319 + 0 0 1.56267 0 0 0 0 0 0 0 0 0.638441 0 0.245907 + 0 0 0 1.56267 0 0 0 0 0 0 0 0.21031 0 0.135725 + 0 0 0 0 1.56267 0 0 0 0 0 0 0.375554 0 0.225277 + 0 0 0 0 0 1.56267 0 0 0 0 0 0.638441 0 0.012926 + 0 0 0 0 0 0 1.56267 0 0 0 0 0.157733 0 -0.0108962 + 0 0 0 0 0 0 0 1.56267 0 0 0 0.212814 0 -0.06974909999999999 + 0 0 0 0 0 0 0 0 1.56267 0 0 0.360532 0 -0.318846 + + +# name: r +# type: matrix +# rows: 18 +# columns: 1 + 0.0354809 + 0.0153183 + 0.0570191 + -0.0372801 + 0.0878601 + 0.06811780000000001 + -0.00426164 + 5.162985999041026e-321 + 6.927779999999998e-310 + 0 + 2.121999999910509e-314 + 0 + 0 + 0 + 3.6073900000086e-313 + 4.446590812571219e-323 + 3.952525166729972e-323 + 3.952525166729972e-323 + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index a6313b3..a0442fc 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1290,6 +1290,10 @@ void MsckfVio::PhotometricMeasurementJacobian( cv::Point2f residualVector(0,0); double res_sum = 0; + + ofstream myfile; + myfile.open ("/home/raphael/dev/MSCKF_ws/log_jacobi.txt"); + for (auto point : feature.anchorPatch_3d) { //cout << "____feature-measurement_____\n" << endl; @@ -1316,8 +1320,8 @@ void MsckfVio::PhotometricMeasurementJacobian( // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); - dI_dhj(0, 0) = dx/(pixelDistance.x); - dI_dhj(0, 1) = dy/(pixelDistance.y); + dI_dhj(0, 0) = dx;// /(pixelDistance.x); + dI_dhj(0, 1) = dy;// /(pixelDistance.y); gradientVector.x += dx; gradientVector.y += dy; @@ -1359,6 +1363,16 @@ void MsckfVio::PhotometricMeasurementJacobian( H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1 H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 + myfile << " --------- \n" << endl; + myfile << "H_rhoj\n" << H_rhoj << endl; + myfile << "H_plj\n" << H_plj << endl; + myfile << "H_pAj\n" << H_pAj << endl; + myfile << "\n" << endl; + myfile << "dI_dhj\n" << dI_dhj << endl; + myfile << "dh_dGpij\n" << dh_dGpij << endl; + myfile << "dGpj_XpAj\n" << dGpj_XpAj << endl; + + // myfile << "pixel pos change based on residual:\n" << dI_dhj.colPivHouseholderQr().solve(r_photo(count)) << endl; H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; @@ -1367,6 +1381,7 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } + myfile.close(); MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); @@ -1393,7 +1408,7 @@ void MsckfVio::PhotometricMeasurementJacobian( H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N); // TODO make this calculation more fluent - for(int i = 0; i< N*N; i++) + for(int i = 0; i< N*N; i++) H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho; @@ -1408,7 +1423,8 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); + //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); + feature.VisualizeKernel(cam_state, cam_state_id, cam0); } return; @@ -1486,10 +1502,24 @@ void MsckfVio::PhotometricFeatureJacobian( { ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "Hxi\n" << H_xi << "ri\n" << r_i << "Hyi\n" << H_yi << endl; - myfile << "kernel\n" << A_null_space << endl; + myfile << "\nHxi" << endl; + for(int i = 0; i < H_xi.rows(); i++) + myfile << H_xi.block(i, 0, 1, H_xi.cols()) << ";"; + + myfile << "\nr" << endl; + for(int i = 0; i < r_i.rows(); i++) + myfile << r.segment(i, 1) << ";"; + + myfile << "\nHyi" << endl; + for(int i = 0; i < H_yi.rows(); i++) + myfile << H_yi.block(i, 0, 1, H_yi.cols()) << ";"; + + myfile << "A_null_space" << endl; + for(int i = 0; i < A_null_space.rows(); i++) + myfile << A_null_space.block(i, 0, 1, A_null_space.cols()) << ";"; + + myfile.close(); - myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; cout << "---------- LOGGED -------- " << endl; } if(PRINTIMAGES) From 07f4927128b68043e9bc1397fae7a3139e517c93 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Thu, 13 Jun 2019 16:20:37 +0200 Subject: [PATCH 47/86] added kernel calculation visualization; changed sobel filter to cv implementation, added octave export --- include/msckf_vio/feature.hpp | 78 ++++++++++++++++++++++++++++------- src/msckf_vio.cpp | 42 +++++++++++++++---- 2 files changed, 98 insertions(+), 22 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index b094d49..218157b 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -184,6 +184,10 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, const CameraCalibration& cam, Eigen::Vector3d& in_p) const; + double cvKernel( + const cv::Point2f pose, + std::string type) const; + double Kernel( const cv::Point2f pose, const cv::Mat frame, @@ -269,6 +273,14 @@ inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p, bool is_initialized; bool is_anchored; + + cv::Mat abs_xderImage; + cv::Mat abs_yderImage; + + cv::Mat xderImage; + cv::Mat yderImage; + + cv::Mat anchorImage_blurred; cv::Point2f anchor_center_pos; cv::Point2f undist_anchor_center_pos; // Noise for a normalized feature measurement. @@ -478,6 +490,27 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const else return false; } +double Feature::cvKernel( + const cv::Point2f pose, + std::string type) const +{ + + double delta = 0; + + if(type == "Sobel_x") + { + std::cout << "image value x" << ((double)xderImage.at(pose.y, pose.x))/255. << std::endl; + delta = ((double)xderImage.at(pose.y, pose.x))/255.; + } + else if (type == "Sobel_y") + { + std::cout << "image value y" << ((double)yderImage.at(pose.y, pose.x))/255. << std::endl; + delta = ((double)yderImage.at(pose.y, pose.x))/255.; + } + return delta; + +} + double Feature::Kernel( const cv::Point2f pose, const cv::Mat frame, @@ -485,9 +518,9 @@ double Feature::Kernel( { Eigen::Matrix kernel = Eigen::Matrix::Zero(); if(type == "Sobel_x") - kernel << -3., 0., 3.,-10., 0., 10. , -3., 0., 3.; + kernel << -1., 0., 1.,-2., 0., 2. , -1., 0., 1.; else if(type == "Sobel_y") - kernel << -3., -10., -3., 0., 0., 0., 3., 10., 3.; + kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.; double delta = 0; int offs = (int)(kernel.rows()-1)/2; @@ -658,29 +691,29 @@ bool Feature::VisualizeKernel( auto anchor = observations.begin(); cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image; - cv::Mat xderImage; - cv::Mat yderImage; + //cv::Mat xderImage; + //cv::Mat yderImage; - cv::Sobel(anchorImage, xderImage, CV_8UC1, 1, 0, 3); - cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3); + //cv::Sobel(anchorImage, xderImage, CV_8UC1, 1, 0, 3); + //cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3); - cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, yderImage.type()); - cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, yderImage.type()); + cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type()); + cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type()); - cv::imshow("xder", xderImage); - //cv::imshow("yder", yderImage); + cv::imshow("xder", abs_xderImage); + cv::imshow("yder", abs_yderImage); for(int i = 1; i < anchorImage.rows-1; i++) for(int j = 1; j < anchorImage.cols-1; j++) - xderImage2.at(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_x"); + xderImage2.at(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_x")); for(int i = 1; i < anchorImage.rows-1; i++) for(int j = 1; j < anchorImage.cols-1; j++) - yderImage2.at(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_y"); + yderImage2.at(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_y")); cv::imshow("anchor", anchorImage); cv::imshow("xder2", xderImage2); - //cv::imshow("yder2", yderImage2); + cv::imshow("yder2", yderImage2); cvWaitKey(0); } @@ -824,12 +857,13 @@ bool Feature::VisualizePatch( */ // residual gradient direction + /* cv::arrowedLine(irradianceFrame, cv::Point(40+scale*(N+N/2+0.5), 15+scale*((N-0.5))), cv::Point(40+scale*(N+N/2+0.5)+scale*residualVector.x, 15+scale*(N-0.5)+scale*residualVector.y), cv::Scalar(0, 255, 175), 3); - + */ cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); @@ -921,7 +955,7 @@ cv::Point2f Feature::pixelDistanceAt( Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); // returns the distance between the pixel points in space - cv::Point2f distance(fabs(pure[0].x - pure[1].x)*p_c0[2], fabs(pure[2].y - pure[3].y)*p_c0[2]); + cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y)); return distance; } @@ -976,6 +1010,8 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal bool Feature::initializeAnchor(const CameraCalibration& cam, int N) { + + //initialize patch Size int n = (int)(N-1)/2; @@ -984,6 +1020,18 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) return false; cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image; + cv::Mat anchorImage_deeper; + anchorImage.convertTo(anchorImage_deeper,CV_16S); + //TODO remove this? + + + cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3); + cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3); + + cv::convertScaleAbs(xderImage, abs_xderImage); + cv::convertScaleAbs(yderImage, abs_yderImage); + + cv::GaussianBlur(anchorImage, anchorImage_blurred, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT); auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2]; auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3]; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index a0442fc..5b3c065 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1314,14 +1314,14 @@ void MsckfVio::PhotometricMeasurementJacobian( // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_x"); - dy = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_y"); + dx = feature.cvKernel(p_in_anchor, "Sobel_x"); + dy = feature.cvKernel(p_in_anchor, "Sobel_y"); // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); - dI_dhj(0, 0) = dx;// /(pixelDistance.x); - dI_dhj(0, 1) = dy;// /(pixelDistance.y); + dI_dhj(0, 0) = dx/(pixelDistance.x); + dI_dhj(0, 1) = dy/(pixelDistance.y); gradientVector.x += dx; gradientVector.y += dy; @@ -1423,8 +1423,8 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); - feature.VisualizeKernel(cam_state, cam_state_id, cam0); + feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); + // feature.VisualizeKernel(cam_state, cam_state_id, cam0); } return; @@ -1496,7 +1496,7 @@ void MsckfVio::PhotometricFeatureJacobian( H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; - cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; + //cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; if(PRINTIMAGES) { @@ -1519,8 +1519,34 @@ void MsckfVio::PhotometricFeatureJacobian( myfile << A_null_space.block(i, 0, 1, A_null_space.cols()) << ";"; + myfile.close(); + + //octave + myfile.open("/home/raphael/dev/octave/log2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: Hx\n" + << "# type: matrix\n" + << "# rows: " << H_xi.rows() << "\n" + << "# columns: " << H_xi.cols() << "\n" + << H_xi << endl; + + myfile << "# name: Hy\n" + << "# type: matrix\n" + << "# rows: " << H_yi.rows() << "\n" + << "# columns: " << H_yi.cols() << "\n" + << H_yi << endl; + + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r_i.rows() << "\n" + << "# columns: " << 1 << "\n" + << r_i << endl; + myfile.close(); cout << "---------- LOGGED -------- " << endl; + cvWaitKey(0); + } if(PRINTIMAGES) { @@ -1808,6 +1834,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) cout << dof << " " << gamma << " " << chi_squared_test_table[dof] << endl; + if (chi_squared_test_table[dof] == 0) + return true; if (gamma < chi_squared_test_table[dof]) { //cout << "passed" << endl; return true; From 1b29047451bca3217a5884723dccf02bece8acaa Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Thu, 13 Jun 2019 18:09:06 +0200 Subject: [PATCH 48/86] sobel normalization added --- include/msckf_vio/feature.hpp | 9 ++++++++- src/msckf_vio.cpp | 6 +++--- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 218157b..6d51e4f 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -701,7 +701,11 @@ bool Feature::VisualizeKernel( cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type()); cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type()); - cv::imshow("xder", abs_xderImage); + + cv::Mat norm_abs_xderImage; + cv::normalize(abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); + + cv::imshow("xder", norm_abs_xderImage); cv::imshow("yder", abs_yderImage); for(int i = 1; i < anchorImage.rows-1; i++) @@ -1028,6 +1032,9 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3); cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3); + xderImage/=8; + yderImage/=8; + cv::convertScaleAbs(xderImage, abs_xderImage); cv::convertScaleAbs(yderImage, abs_yderImage); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 5b3c065..0877890 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1320,8 +1320,8 @@ void MsckfVio::PhotometricMeasurementJacobian( // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); - dI_dhj(0, 0) = dx/(pixelDistance.x); - dI_dhj(0, 1) = dy/(pixelDistance.y); + dI_dhj(0, 0) = dx;// /(pixelDistance.x); + dI_dhj(0, 1) = dy;// /(pixelDistance.y); gradientVector.x += dx; gradientVector.y += dy; @@ -1424,7 +1424,7 @@ void MsckfVio::PhotometricMeasurementJacobian( { feature.MarkerGeneration(marker_pub, state_server.cam_states); feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); - // feature.VisualizeKernel(cam_state, cam_state_id, cam0); + feature.VisualizeKernel(cam_state, cam_state_id, cam0); } return; From dcbc69a1c4eaf060a0ff2a7259adc5e10bf22565 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Tue, 18 Jun 2019 11:03:52 +0200 Subject: [PATCH 49/86] added fix to feature projection --- include/msckf_vio/feature.hpp | 6 ------ launch/msckf_vio_tum.launch | 4 ++-- src/msckf_vio.cpp | 22 +++++++--------------- 3 files changed, 9 insertions(+), 23 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 6d51e4f..466d1e9 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -498,15 +498,9 @@ double Feature::cvKernel( double delta = 0; if(type == "Sobel_x") - { - std::cout << "image value x" << ((double)xderImage.at(pose.y, pose.x))/255. << std::endl; delta = ((double)xderImage.at(pose.y, pose.x))/255.; - } else if (type == "Sobel_y") - { - std::cout << "image value y" << ((double)yderImage.at(pose.y, pose.x))/255. << std::endl; delta = ((double)yderImage.at(pose.y, pose.x))/255.; - } return delta; } diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 0fc464a..f6602c1 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -21,10 +21,10 @@ - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 0877890..977a95b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -212,7 +212,7 @@ bool MsckfVio::loadParameters() { utils::getTransformEigen(nh, "T_imu_body").inverse(); // Maximum number of camera states to be stored - nh.param("max_cam_state_size", max_cam_state_size, 30); + nh.param("max_cam_state_size", max_cam_state_size, 20); //cam_state_size = max_cam_state_size; ROS_INFO("==========================================="); ROS_INFO("fixed frame id: %s", fixed_frame_id.c_str()); @@ -1320,16 +1320,8 @@ void MsckfVio::PhotometricMeasurementJacobian( // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); - dI_dhj(0, 0) = dx;// /(pixelDistance.x); - dI_dhj(0, 1) = dy;// /(pixelDistance.y); - - gradientVector.x += dx; - gradientVector.y += dy; - - - residualVector.x += dx * r_photo(count); - residualVector.y += dy * r_photo(count); - res_sum += r_photo(count); + dI_dhj(0, 0) = dx * cam0.intrinsics[0]; + dI_dhj(0, 1) = dy * cam0.intrinsics[1]; //dh / d{}^Cp_{ij} dh_dCpij(0, 0) = 1 / p_c0(2); @@ -1424,7 +1416,7 @@ void MsckfVio::PhotometricMeasurementJacobian( { feature.MarkerGeneration(marker_pub, state_server.cam_states); feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); - feature.VisualizeKernel(cam_state, cam_state_id, cam0); + //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } return; @@ -1767,7 +1759,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { // Compute the error of the state. VectorXd delta_x = K * r; - // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1823,6 +1814,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { } bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { + return 1; MatrixXd P1 = H * state_server.state_cov * H.transpose(); @@ -2085,7 +2077,7 @@ void MsckfVio::pruneLastCamStateBuffer() else featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { + if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); @@ -2232,7 +2224,7 @@ void MsckfVio::pruneCamStateBuffer() { else featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { + if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); From 2d045660c2f97036d6374692808c47581f94c61e Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 19 Jun 2019 09:13:46 +0200 Subject: [PATCH 50/86] added some debug output, added qr decomp --- include/msckf_vio/msckf_vio.h | 1 + launch/msckf_vio_tum.launch | 7 +- src/msckf_vio.cpp | 116 ++++++++++++++-------------------- 3 files changed, 55 insertions(+), 69 deletions(-) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 032e885..ad20d49 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -236,6 +236,7 @@ class MsckfVio { bool PHOTOMETRIC; // debug flag + bool STREAMPAUSE; bool PRINTIMAGES; bool GROUNDTRUTH; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index f6602c1..54e6c7e 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,13 +18,14 @@ output="screen"> - + + - + @@ -71,4 +72,6 @@ + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 977a95b..a6fe1f9 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -69,6 +69,7 @@ bool MsckfVio::loadParameters() { //Photometry Flag nh.param("PHOTOMETRIC", PHOTOMETRIC, false); nh.param("PrintImages", PRINTIMAGES, false); + nh.param("StreamPause", STREAMPAUSE, false); nh.param("GroundTruth", GROUNDTRUTH, false); // Frame id @@ -304,10 +305,10 @@ bool MsckfVio::initialize() { // Initialize the chi squared test table with confidence // level 0.95. - for (int i = 1; i < 100; ++i) { + for (int i = 1; i < 1000; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.2); + boost::math::quantile(chi_squared_dist, 0.05); } if (!createRosIO()) return false; @@ -407,6 +408,10 @@ void MsckfVio::imageCallback( // Return if the gravity vector has not been set. if (!is_gravity_set) return; + // stop playing bagfile if printing images + if(STREAMPAUSE) + nh.setParam("/play_bag", false); + // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -443,6 +448,7 @@ void MsckfVio::imageCallback( double imu_processing_time = ( ros::Time::now()-start_time).toSec(); + //cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); if(PHOTOMETRIC) @@ -455,6 +461,8 @@ void MsckfVio::imageCallback( double state_augmentation_time = ( ros::Time::now()-start_time).toSec(); + + //cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -462,23 +470,29 @@ void MsckfVio::imageCallback( double add_observations_time = ( ros::Time::now()-start_time).toSec(); + + //cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); double manage_moving_window_time = ( ros::Time::now()-start_time).toSec(); + + //cout << "4" << endl; // Perform measurement update if necessary. start_time = ros::Time::now(); removeLostFeatures(); double remove_lost_features_time = ( ros::Time::now()-start_time).toSec(); + //cout << "5" << endl; start_time = ros::Time::now(); - pruneLastCamStateBuffer(); + pruneCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); + //cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -509,6 +523,8 @@ void MsckfVio::imageCallback( publish_time, publish_time/processing_time); } + if(STREAMPAUSE) + nh.setParam("/play_bag", true); return; } @@ -1428,15 +1444,7 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd& H_x, VectorXd& r) { - // stop playing bagfile if printing images - if(PRINTIMAGES) - { - std::cout << "stopped playpack" << std::endl; - nh.setParam("/play_bag", false); - } - const auto& feature = map_server[feature_id]; - // Check how many camera states in the provided camera // id camera has actually seen this feature. @@ -1538,13 +1546,8 @@ void MsckfVio::PhotometricFeatureJacobian( myfile.close(); cout << "---------- LOGGED -------- " << endl; cvWaitKey(0); + } - } - if(PRINTIMAGES) - { - std::cout << "resume playback" << std::endl; - nh.setParam("/play_bag", true); - } return; } @@ -1619,12 +1622,6 @@ void MsckfVio::featureJacobian( MatrixXd& H_x, VectorXd& r) { // stop playing bagfile if printing images - if(PRINTIMAGES) - { - std::cout << "stopped playpack" << std::endl; - nh.setParam("/play_bag", false); - } - const auto& feature = map_server[feature_id]; @@ -1686,9 +1683,6 @@ void MsckfVio::featureJacobian( H_x = A.transpose() * H_xj; r = A.transpose() * r_j; - - cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; - // stop playing bagfile if printing images if(PRINTIMAGES) { @@ -1698,8 +1692,6 @@ void MsckfVio::featureJacobian( myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; myfile.close(); cout << "---------- LOGGED -------- " << endl; - std::cout << "stopped playpack" << std::endl; - nh.setParam("/play_bag", true); } return; @@ -1708,8 +1700,8 @@ void MsckfVio::featureJacobian( void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { - if (H.rows() == 0 || r.rows() == 0) return; - + if (H.rows() == 0 || r.rows() == 0) + return; // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -1718,7 +1710,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { if(PHOTOMETRIC) augmentationSize = 7; - /* + // QR decomposition to make stuff faster if (H.rows() > H.cols()) { // Convert H to a sparse matrix. SparseMatrix H_sparse = H.sparseView(); @@ -1736,29 +1728,24 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { H_thin = H_temp.topRows(21+state_server.cam_states.size()*augmentationSize); r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize); - //HouseholderQR qr_helper(H); - //MatrixXd Q = qr_helper.householderQ(); - //MatrixXd Q1 = Q.leftCols(21+state_server.cam_states.size()*6); - - //H_thin = Q1.transpose() * H; - //r_thin = Q1.transpose() * r; } else { H_thin = H; r_thin = r; } - */ + // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; - MatrixXd S = H*P*H.transpose() + + MatrixXd S = H_thin*P*H_thin.transpose() + Feature::observation_noise*MatrixXd::Identity( - H.rows(), H.rows()); + H_thin.rows(), H_thin.rows()); //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); - MatrixXd K_transpose = S.ldlt().solve(H*P); + cout << "inverting: " << S.rows() << ":" << S.cols() << endl; + MatrixXd K_transpose = S.ldlt().solve(H_thin*P); MatrixXd K = K_transpose.transpose(); - // Compute the error of the state. VectorXd delta_x = K * r; + cout << "update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1800,7 +1787,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { } // Update state covariance. - MatrixXd I_KH = MatrixXd::Identity(K.rows(), H.cols()) - K*H; + MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin; //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() + // K*K.transpose()*Feature::observation_noise; state_server.state_cov = I_KH*state_server.state_cov; @@ -1814,7 +1801,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { } bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - return 1; MatrixXd P1 = H * state_server.state_cov * H.transpose(); @@ -1827,7 +1813,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) chi_squared_test_table[dof] << endl; if (chi_squared_test_table[dof] == 0) - return true; + return false; if (gamma < chi_squared_test_table[dof]) { //cout << "passed" << endl; return true; @@ -1929,11 +1915,6 @@ void MsckfVio::removeLostFeatures() { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); - cout << "passed" << endl; - } - else - { - cout << "rejected" << endl; } // Put an upper bound on the row size of measurement Jacobian, @@ -1941,6 +1922,8 @@ void MsckfVio::removeLostFeatures() { if (stack_cntr > 1500) break; } + cout << "processed features: " << processed_feature_ids.size() << endl; + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2000,6 +1983,7 @@ void MsckfVio::findRedundantCamStates(vector& rm_cam_state_ids) { void MsckfVio::pruneLastCamStateBuffer() { + if (state_server.cam_states.size() < max_cam_state_size) return; @@ -2058,7 +2042,7 @@ void MsckfVio::pruneLastCamStateBuffer() MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; - + int pruned_cntr = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2077,25 +2061,27 @@ void MsckfVio::pruneLastCamStateBuffer() else featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { + if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); - cout << "passed" << endl; - } - else - { - cout << "rejected" << endl; + pruned_cntr++; } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } - H_x.conservativeResize(stack_cntr, H_x.cols()); - r.conservativeResize(stack_cntr); - // Perform measurement update. - measurementUpdate(H_x, r); - + if(pruned_cntr != 0) + { + cout << "pruned features: " << pruned_cntr << endl; + + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + + // Perform measurement update. + measurementUpdate(H_x, r); + } + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -2137,6 +2123,7 @@ void MsckfVio::pruneLastCamStateBuffer() void MsckfVio::pruneCamStateBuffer() { + if (state_server.cam_states.size() < max_cam_state_size) return; @@ -2228,11 +2215,6 @@ void MsckfVio::pruneCamStateBuffer() { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); - cout << "passed" << endl; - } - else - { - cout << "rejected" << endl; } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); From ecab936c627a84678cc5a494dc2aa5041132998a Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 19 Jun 2019 09:59:22 +0200 Subject: [PATCH 51/86] minor changes in launch file and edited augmentationsize --- launch/msckf_vio_tum.launch | 4 ++-- src/msckf_vio.cpp | 26 +++++++++----------------- 2 files changed, 11 insertions(+), 19 deletions(-) diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 54e6c7e..ebf0946 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -21,7 +21,7 @@ - + @@ -72,6 +72,6 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index a6fe1f9..02b2879 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -451,13 +451,9 @@ void MsckfVio::imageCallback( //cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); - if(PHOTOMETRIC) - { - truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); - PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); - } - else - stateAugmentation(feature_msg->header.stamp.toSec()); + //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); + PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); + double state_augmentation_time = ( ros::Time::now()-start_time).toSec(); @@ -1706,9 +1702,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; + int augmentationSize = 7; // QR decomposition to make stuff faster if (H.rows() > H.cols()) { @@ -1779,11 +1773,13 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { auto cam_state_iter = state_server.cam_states.begin(); for (int i = 0; i < state_server.cam_states.size(); ++i, ++cam_state_iter) { - const VectorXd& delta_x_cam = delta_x.segment(21+i*augmentationSize, augmentationSize); + const VectorXd& delta_x_cam = delta_x.segment(21+i*augmentationSize, 6); const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>()); cam_state_iter->second.orientation = quaternionMultiplication( dq_cam, cam_state_iter->second.orientation); cam_state_iter->second.position += delta_x_cam.tail<3>(); + if(PHOTOMETRIC) + cam_state_iter->second.illumination.frame_bias += delta_x(21+i*augmentationSize+6); } // Update state covariance. @@ -1827,9 +1823,7 @@ void MsckfVio::removeLostFeatures() { // Remove the features that lost track. // BTW, find the size the final Jacobian matrix and residual vector. int jacobian_row_size = 0; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; + int augmentationSize = 7; vector invalid_feature_ids(0); @@ -2133,9 +2127,7 @@ void MsckfVio::pruneCamStateBuffer() { // Find the size of the Jacobian matrix. int jacobian_row_size = 0; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; + int augmentationSize = 7; for (auto& item : map_server) { auto& feature = item.second; From c79fc173b382dc56e2d4956a157b13885d84747a Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 19 Jun 2019 09:59:53 +0200 Subject: [PATCH 52/86] minor changes in launch file and edited augmentationsize --- src/msckf_vio.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 02b2879..330bd54 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -451,9 +451,13 @@ void MsckfVio::imageCallback( //cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); - //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); - PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); - + if(PHOTOMETRIC) + { + truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); + PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); + } + else + stateAugmentation(feature_msg->header.stamp.toSec()); double state_augmentation_time = ( ros::Time::now()-start_time).toSec(); @@ -1702,7 +1706,9 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; - int augmentationSize = 7; + int augmentationSize = 6; + if(PHOTOMETRIC) + augmentationSize = 7; // QR decomposition to make stuff faster if (H.rows() > H.cols()) { @@ -1823,7 +1829,9 @@ void MsckfVio::removeLostFeatures() { // Remove the features that lost track. // BTW, find the size the final Jacobian matrix and residual vector. int jacobian_row_size = 0; - int augmentationSize = 7; + int augmentationSize = 6; + if(PHOTOMETRIC) + augmentationSize = 7; vector invalid_feature_ids(0); @@ -2127,7 +2135,9 @@ void MsckfVio::pruneCamStateBuffer() { // Find the size of the Jacobian matrix. int jacobian_row_size = 0; - int augmentationSize = 7; + int augmentationSize = 6; + if(PHOTOMETRIC) + augmentationSize = 7; for (auto& item : map_server) { auto& feature = item.second; From 02b9e0bc78cad4d8d00cc7ef0ae959f386c3e145 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Wed, 19 Jun 2019 18:32:44 +0200 Subject: [PATCH 53/86] added scaling for images --- config/camchain-imucam-tum-scaled.yaml | 36 +++ include/msckf_vio/msckf_vio.h | 2 + launch/image_processor_tinytum.launch | 34 +++ launch/msckf_vio_tinytum.launch | 78 ++++++ launch/msckf_vio_tum.launch | 10 +- src/msckf_vio.cpp | 319 ++++++++++++++++--------- src/shrinkImage.py | 75 ++++++ 7 files changed, 439 insertions(+), 115 deletions(-) create mode 100644 config/camchain-imucam-tum-scaled.yaml create mode 100644 launch/image_processor_tinytum.launch create mode 100644 launch/msckf_vio_tinytum.launch create mode 100755 src/shrinkImage.py diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml new file mode 100644 index 0000000..529828c --- /dev/null +++ b/config/camchain-imucam-tum-scaled.yaml @@ -0,0 +1,36 @@ +cam0: + T_cam_imu: + [-0.9995378259923383, 0.02917807204183088, -0.008530798463872679, 0.047094247958417004, + 0.007526588843243184, -0.03435493139706542, -0.9993813532126198, -0.04788273017221637, + -0.029453096117288798, -0.9989836729399656, 0.034119442089241274, -0.0697294754693238, + 0.0, 0.0, 0.0, 1.0] + camera_model: pinhole + distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, + -0.001662284667857643] + distortion_model: equidistant + intrinsics: [95.2026071784, 95.2029854486, 127.573663262, 128.582615763] + resolution: [256, 256] + rostopic: /cam0/image_raw +cam1: + T_cam_imu: + [-0.9995240747493029, 0.02986739485347808, -0.007717688852024281, -0.05374086123613335, + 0.008095979457928231, 0.01256553460985914, -0.9998882749870535, -0.04648588412432889, + -0.02976708103202316, -0.9994748851595197, -0.0128013601698453, -0.07333210787623645, + 0.0, 0.0, 0.0, 1.0] + T_cn_cnm1: + [0.9999994317488622, -0.0008361847221513937, -0.0006612844045898121, -0.10092123225528335, + 0.0008042457277382264, 0.9988989443471681, -0.04690684567228134, -0.001964540595211977, + 0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572, + 0.0, 0.0, 0.0, 1.0] + camera_model: pinhole + distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, + -0.002347858896562788] + distortion_model: equidistant + intrinsics: [94.8217471066, 94.8164593555, 126.391667581, 127.571024044] + resolution: [256, 256] + rostopic: /cam1/image_raw +T_imu_body: + [1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000] \ No newline at end of file diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index ad20d49..9e9975d 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -219,6 +219,7 @@ class MsckfVio { const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); void measurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); bool gatingTest(const Eigen::MatrixXd& H, @@ -252,6 +253,7 @@ class MsckfVio { // State vector StateServer state_server; + StateServer photometric_state_server; // Ground truth state vector StateServer true_state_server; diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch new file mode 100644 index 0000000..6fe2855 --- /dev/null +++ b/launch/image_processor_tinytum.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch new file mode 100644 index 0000000..12fcaee --- /dev/null +++ b/launch/msckf_vio_tinytum.launch @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index ebf0946..390ab03 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,14 +18,14 @@ output="screen"> - + - - + + - + @@ -72,6 +72,6 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 330bd54..323e791 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -451,13 +451,8 @@ void MsckfVio::imageCallback( //cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); - if(PHOTOMETRIC) - { - truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); - PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); - } - else - stateAugmentation(feature_msg->header.stamp.toSec()); + //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); + PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); double state_augmentation_time = ( ros::Time::now()-start_time).toSec(); @@ -488,7 +483,7 @@ void MsckfVio::imageCallback( //cout << "5" << endl; start_time = ros::Time::now(); - pruneCamStateBuffer(); + pruneLastCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); @@ -1306,10 +1301,6 @@ void MsckfVio::PhotometricMeasurementJacobian( cv::Point2f residualVector(0,0); double res_sum = 0; - - ofstream myfile; - myfile.open ("/home/raphael/dev/MSCKF_ws/log_jacobi.txt"); - for (auto point : feature.anchorPatch_3d) { //cout << "____feature-measurement_____\n" << endl; @@ -1324,9 +1315,6 @@ void MsckfVio::PhotometricMeasurementJacobian( r_photo(count) = photo_z[count] - estimate_photo_z[count]; //cout << "residual: " << photo_r.back() << endl; - // add jacobians - cv::Point2f pixelDistance = feature.pixelDistanceAt(anchor_state, anchor_state_id, cam0, point); - // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] @@ -1371,16 +1359,6 @@ void MsckfVio::PhotometricMeasurementJacobian( H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1 H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 - myfile << " --------- \n" << endl; - myfile << "H_rhoj\n" << H_rhoj << endl; - myfile << "H_plj\n" << H_plj << endl; - myfile << "H_pAj\n" << H_pAj << endl; - myfile << "\n" << endl; - myfile << "dI_dhj\n" << dI_dhj << endl; - myfile << "dh_dGpij\n" << dh_dGpij << endl; - myfile << "dGpj_XpAj\n" << dGpj_XpAj << endl; - - // myfile << "pixel pos change based on residual:\n" << dI_dhj.colPivHouseholderQr().solve(r_photo(count)) << endl; H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; @@ -1389,8 +1367,6 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } - myfile.close(); - MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); @@ -1639,7 +1615,7 @@ void MsckfVio::featureJacobian( jacobian_row_size = 4 * valid_cam_state_ids.size(); MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size, - 21+state_server.cam_states.size()*6); + 21+state_server.cam_states.size()*7); MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3); VectorXd r_j = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1656,7 +1632,7 @@ void MsckfVio::featureJacobian( state_server.cam_states.begin(), cam_state_iter); // Stack the Jacobians. - H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi; + H_xj.block<4, 6>(stack_cntr, 21+7*cam_state_cntr) = H_xi; H_fj.block<4, 3>(stack_cntr, 0) = H_fi; r_j.segment<4>(stack_cntr) = r_i; stack_cntr += 4; @@ -1706,10 +1682,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; - + // QR decomposition to make stuff faster if (H.rows() > H.cols()) { // Convert H to a sparse matrix. @@ -1725,14 +1698,13 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp); (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp); - H_thin = H_temp.topRows(21+state_server.cam_states.size()*augmentationSize); - r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize); + H_thin = H_temp.topRows(21+state_server.cam_states.size()*7); + r_thin = r_temp.head(21+state_server.cam_states.size()*7); } else { H_thin = H; r_thin = r; - } - + } // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; @@ -1740,13 +1712,16 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { Feature::observation_noise*MatrixXd::Identity( H_thin.rows(), H_thin.rows()); //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); - cout << "inverting: " << S.rows() << ":" << S.cols() << endl; MatrixXd K_transpose = S.ldlt().solve(H_thin*P); MatrixXd K = K_transpose.transpose(); // Compute the error of the state. VectorXd delta_x = K * r; - cout << "update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; + cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. + + if(PHOTOMETRIC) return; + const VectorXd& delta_x_imu = delta_x.head<21>(); if (//delta_x_imu.segment<3>(0).norm() > 0.15 || @@ -1779,13 +1754,12 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { auto cam_state_iter = state_server.cam_states.begin(); for (int i = 0; i < state_server.cam_states.size(); ++i, ++cam_state_iter) { - const VectorXd& delta_x_cam = delta_x.segment(21+i*augmentationSize, 6); + const VectorXd& delta_x_cam = delta_x.segment(21+i*7, 6); const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>()); cam_state_iter->second.orientation = quaternionMultiplication( dq_cam, cam_state_iter->second.orientation); cam_state_iter->second.position += delta_x_cam.tail<3>(); - if(PHOTOMETRIC) - cam_state_iter->second.illumination.frame_bias += delta_x(21+i*augmentationSize+6); + cam_state_iter->second.illumination.frame_bias += delta_x(21+i*7+6); } // Update state covariance. @@ -1802,6 +1776,110 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { return; } + +void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { + + if (H.rows() == 0 || r.rows() == 0) + return; + // Decompose the final Jacobian matrix to reduce computational + // complexity as in Equation (28), (29). + MatrixXd H_thin; + VectorXd r_thin; + + // QR decomposition to make stuff faster + if (H.rows() > H.cols()) { + // Convert H to a sparse matrix. + SparseMatrix H_sparse = H.sparseView(); + + // Perform QR decompostion on H_sparse. + SPQR > spqr_helper; + spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); + spqr_helper.compute(H_sparse); + + MatrixXd H_temp; + VectorXd r_temp; + (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp); + (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp); + + H_thin = H_temp.topRows(21+state_server.cam_states.size()*7); + r_thin = r_temp.head(21+state_server.cam_states.size()*7); + + } else { + H_thin = H; + r_thin = r; + } + + + // Compute the Kalman gain. + const MatrixXd& P = state_server.state_cov; + MatrixXd S = H_thin*P*H_thin.transpose() + + Feature::observation_noise*MatrixXd::Identity( + H_thin.rows(), H_thin.rows()); + //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); + MatrixXd K_transpose = S.ldlt().solve(H_thin*P); + MatrixXd K = K_transpose.transpose(); + // Compute the error of the state. + + VectorXd delta_x = K * r; + cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; + cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // Update the IMU state. + if (not PHOTOMETRIC) return; + + const VectorXd& delta_x_imu = delta_x.head<21>(); + + if (//delta_x_imu.segment<3>(0).norm() > 0.15 || + //delta_x_imu.segment<3>(3).norm() > 0.15 || + delta_x_imu.segment<3>(6).norm() > 0.5 || + //delta_x_imu.segment<3>(9).norm() > 0.5 || + delta_x_imu.segment<3>(12).norm() > 1.0) { + printf("delta velocity: %f\n", delta_x_imu.segment<3>(6).norm()); + printf("delta position: %f\n", delta_x_imu.segment<3>(12).norm()); + ROS_WARN("Update change is too large."); + //return; + } + + const Vector4d dq_imu = + smallAngleQuaternion(delta_x_imu.head<3>()); + state_server.imu_state.orientation = quaternionMultiplication( + dq_imu, state_server.imu_state.orientation); + state_server.imu_state.gyro_bias += delta_x_imu.segment<3>(3); + state_server.imu_state.velocity += delta_x_imu.segment<3>(6); + state_server.imu_state.acc_bias += delta_x_imu.segment<3>(9); + state_server.imu_state.position += delta_x_imu.segment<3>(12); + + const Vector4d dq_extrinsic = + smallAngleQuaternion(delta_x_imu.segment<3>(15)); + state_server.imu_state.R_imu_cam0 = quaternionToRotation( + dq_extrinsic) * state_server.imu_state.R_imu_cam0; + state_server.imu_state.t_cam0_imu += delta_x_imu.segment<3>(18); + + // Update the camera states. + auto cam_state_iter = state_server.cam_states.begin(); + for (int i = 0; i < state_server.cam_states.size(); + ++i, ++cam_state_iter) { + const VectorXd& delta_x_cam = delta_x.segment(21+i*7, 6); + const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>()); + cam_state_iter->second.orientation = quaternionMultiplication( + dq_cam, cam_state_iter->second.orientation); + cam_state_iter->second.position += delta_x_cam.tail<3>(); + cam_state_iter->second.illumination.frame_bias += delta_x(21+i*7+6); + } + + // Update state covariance. + MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin; + //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() + + // K*K.transpose()*Feature::observation_noise; + state_server.state_cov = I_KH*state_server.state_cov; + + // Fix the covariance to be symmetric + MatrixXd state_cov_fixed = (state_server.state_cov + + state_server.state_cov.transpose()) / 2.0; + state_server.state_cov = state_cov_fixed; + return; +} + + bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { MatrixXd P1 = H * state_server.state_cov * H.transpose(); @@ -1811,8 +1889,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - cout << dof << " " << gamma << " " << - chi_squared_test_table[dof] << endl; + //cout << "gate: " << dof << " " << gamma << " " << + //chi_squared_test_table[dof] << endl; if (chi_squared_test_table[dof] == 0) return false; @@ -1829,10 +1907,7 @@ void MsckfVio::removeLostFeatures() { // Remove the features that lost track. // BTW, find the size the final Jacobian matrix and residual vector. int jacobian_row_size = 0; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; - + int pjacobian_row_size = 0; vector invalid_feature_ids(0); vector processed_feature_ids(0); @@ -1872,11 +1947,8 @@ void MsckfVio::removeLostFeatures() { } } - if(PHOTOMETRIC) - //just use max. size, as gets shrunken down after anyway - jacobian_row_size += N*N*feature.observations.size(); - else - jacobian_row_size += 4*feature.observations.size() - 3; + pjacobian_row_size += N*N*feature.observations.size(); + jacobian_row_size += 4*feature.observations.size() - 3; processed_feature_ids.push_back(feature.id); } @@ -1893,10 +1965,15 @@ void MsckfVio::removeLostFeatures() { if (processed_feature_ids.size() == 0) return; MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, - 21+augmentationSize*state_server.cam_states.size()); + 21+7*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; + MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, + 21+7*state_server.cam_states.size()); + VectorXd pr = VectorXd::Zero(pjacobian_row_size); + int pstack_cntr = 0; + // Process the features which lose track. for (const auto& feature_id : processed_feature_ids) { auto& feature = map_server[feature_id]; @@ -1907,30 +1984,36 @@ void MsckfVio::removeLostFeatures() { MatrixXd H_xj; VectorXd r_j; + MatrixXd pH_xj; + VectorXd pr_j; - if(PHOTOMETRIC) - PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j); - else - featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - + PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); + featureJacobian(feature.id, cam_state_ids, H_xj, r_j); + if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); + } + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; + pr.segment(pstack_cntr, pr_j.rows()) = pr_j; + pstack_cntr += pH_xj.rows(); } // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. - if (stack_cntr > 1500) break; + //if (stack_cntr > 1500) break; } - cout << "processed features: " << processed_feature_ids.size() << endl; - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + // Perform the measurement update step. measurementUpdate(H_x, r); + photometricMeasurementUpdate(pH_x, pr); // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) @@ -1988,14 +2071,13 @@ void MsckfVio::pruneLastCamStateBuffer() if (state_server.cam_states.size() < max_cam_state_size) return; - + auto rm_cam_state_id = state_server.cam_states.begin()->first; // Set the size of the Jacobian matrix. int jacobian_row_size = 0; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; + int pjacobian_row_size = 0; + //initialize all the features which are going to be removed for (auto& item : map_server) { @@ -2031,20 +2113,23 @@ void MsckfVio::pruneLastCamStateBuffer() } } - if(PHOTOMETRIC) - //just use max. size, as gets shrunken down after anyway - jacobian_row_size += N*N*feature.observations.size(); - else - jacobian_row_size += 4*feature.observations.size() - 3; + pjacobian_row_size += N*N*feature.observations.size(); + jacobian_row_size += 4*feature.observations.size() - 3; } MatrixXd H_xj; VectorXd r_j; - MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size()); + MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+7*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); + MatrixXd pH_xj; + VectorXd pr_j; + MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size()); + VectorXd pr = VectorXd::Zero(pjacobian_row_size); int stack_cntr = 0; int pruned_cntr = 0; + int pstack_cntr = 0; + for (auto& item : map_server) { auto& feature = item.second; @@ -2058,38 +2143,41 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - if(PHOTOMETRIC) - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - else + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { + if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); pruned_cntr++; + } + if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { + pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; + pr.segment(pstack_cntr, pr_j.rows()) = pr_j; + pstack_cntr += pH_xj.rows(); } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } - if(pruned_cntr != 0) - { - cout << "pruned features: " << pruned_cntr << endl; - H_x.conservativeResize(stack_cntr, H_x.cols()); - r.conservativeResize(stack_cntr); - - // Perform measurement update. - measurementUpdate(H_x, r); - } + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + // Perform measurement update. + + + measurementUpdate(H_x, r); + photometricMeasurementUpdate(pH_x, pr); + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); - int cam_state_start = 21 + augmentationSize*cam_sequence; - int cam_state_end = cam_state_start + augmentationSize; - + int cam_state_start = 21 + 7*cam_sequence; + int cam_state_end = cam_state_start + 7; // Remove the corresponding rows and columns in the state // covariance matrix. @@ -2109,12 +2197,13 @@ void MsckfVio::pruneLastCamStateBuffer() state_server.state_cov.cols()-cam_state_end); state_server.state_cov.conservativeResize( - state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + state_server.state_cov.rows()-7, state_server.state_cov.cols()-7); } else { state_server.state_cov.conservativeResize( - state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + state_server.state_cov.rows()-7, state_server.state_cov.cols()-7); } + // Remove this camera state in the state vector. state_server.cam_states.erase(rm_cam_state_id); cam0.moving_window.erase(rm_cam_state_id); @@ -2125,7 +2214,6 @@ void MsckfVio::pruneLastCamStateBuffer() void MsckfVio::pruneCamStateBuffer() { - if (state_server.cam_states.size() < max_cam_state_size) return; @@ -2135,9 +2223,7 @@ void MsckfVio::pruneCamStateBuffer() { // Find the size of the Jacobian matrix. int jacobian_row_size = 0; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; + int pjacobian_row_size = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2181,10 +2267,9 @@ void MsckfVio::pruneCamStateBuffer() { continue; } } - if(PHOTOMETRIC) - jacobian_row_size += N*N*involved_cam_state_ids.size(); - else - jacobian_row_size += 4*involved_cam_state_ids.size() - 3; + + pjacobian_row_size += N*N*involved_cam_state_ids.size(); + jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } //cout << "jacobian row #: " << jacobian_row_size << endl; @@ -2192,9 +2277,14 @@ void MsckfVio::pruneCamStateBuffer() { // Compute the Jacobian and residual. MatrixXd H_xj; VectorXd r_j; - MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size()); + MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+7*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; + MatrixXd pH_xj; + VectorXd pr_j; + MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size()); + VectorXd pr = VectorXd::Zero(pjacobian_row_size); + int pstack_cntr = 0; for (auto& item : map_server) { auto& feature = item.second; // Check how many camera states to be removed are associated @@ -2207,33 +2297,42 @@ void MsckfVio::pruneCamStateBuffer() { } if (involved_cam_state_ids.size() == 0) continue; - - if(PHOTOMETRIC) - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - else - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - + + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); } + + if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { + pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; + pr.segment(pstack_cntr, pr_j.rows()) = pr_j; + pstack_cntr += pH_xj.rows(); + } + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); // Perform measurement update. measurementUpdate(H_x, r); + photometricMeasurementUpdate(pH_x, pr); //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_id)); - int cam_state_start = 21 + augmentationSize*cam_sequence; - int cam_state_end = cam_state_start + augmentationSize; + int cam_state_start = 21 + 7*cam_sequence; + int cam_state_end = cam_state_start + 7; // Remove the corresponding rows and columns in the state @@ -2254,10 +2353,10 @@ void MsckfVio::pruneCamStateBuffer() { state_server.state_cov.cols()-cam_state_end); state_server.state_cov.conservativeResize( - state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + state_server.state_cov.rows()-7, state_server.state_cov.cols()-7); } else { state_server.state_cov.conservativeResize( - state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + state_server.state_cov.rows()-7, state_server.state_cov.cols()-7); } // Remove this camera state in the state vector. diff --git a/src/shrinkImage.py b/src/shrinkImage.py new file mode 100755 index 0000000..89ca55d --- /dev/null +++ b/src/shrinkImage.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python +from __future__ import print_function + +import sys +import rospy +import cv2 +from std_msgs.msg import String +from sensor_msgs.msg import Image +from cv_bridge import CvBridge, CvBridgeError + +class image_converter: + + def __init__(self): + self.image0_pub = rospy.Publisher("/cam0/new_image_raw",Image, queue_size=10) + self.image1_pub = rospy.Publisher("/cam1/new_image_raw",Image, queue_size=10) + + self.bridge = CvBridge() + self.image0_sub = rospy.Subscriber("/cam0/image_raw",Image,self.callback_cam0) + self.image1_sub = rospy.Subscriber("/cam1/image_raw",Image,self.callback_cam1) + + def callback_cam0(self,data): + try: + cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") + except CvBridgeError as e: + print(e) + + imgScale = 0.25 + newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale + newimg = cv2.resize(cv_image,(int(newX),int(newY))) + + newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8") + newdata = data + newdata.height = newpub.height + newdata.width = newpub.width + newdata.step = newpub.step + newdata.data = newpub.data + try: + self.image0_pub.publish(newdata) + except CvBridgeError as e: + print(e) + + def callback_cam1(self,data): + try: + cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") + except CvBridgeError as e: + print(e) + + imgScale = 0.25 + newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale + newimg = cv2.resize(cv_image,(int(newX),int(newY))) + + newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8") + newdata = data + newdata.height = newpub.height + newdata.width = newpub.width + newdata.step = newpub.step + newdata.data = newpub.data + + try: + self.image1_pub.publish(newdata) + except CvBridgeError as e: + print(e) + + +def main(args): + ic = image_converter() + rospy.init_node('image_converter', anonymous=True) + try: + rospy.spin() + except KeyboardInterrupt: + print("Shutting down") + cv2.destroyAllWindows() + +if __name__ == '__main__': + main(sys.argv) \ No newline at end of file From bcf948bcc12055c2a975d50757b5e92a1b1e96aa Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Wed, 19 Jun 2019 18:56:30 +0200 Subject: [PATCH 54/86] added bagcontrol here --- launch/msckf_vio_tinytum.launch | 3 +- src/control.py | 64 +++++++++++++++++++++++++++++++++ 2 files changed, 66 insertions(+), 1 deletion(-) create mode 100755 src/control.py diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 12fcaee..94deb7c 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -72,7 +72,8 @@ - + + diff --git a/src/control.py b/src/control.py new file mode 100755 index 0000000..3292b5b --- /dev/null +++ b/src/control.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python +import rosbag +import rospy +from sensor_msgs.msg import Imu, Image +from geometry_msgs.msg import TransformStamped +import time +import signal +import sys + + +def signal_handler(sig, frame): + print('gracefully exiting the program.') + bag.close() + sys.exit(0) + +def main(): + global bag + + cam0_topic = '/cam0/image_raw' + cam1_topic = '/cam1/image_raw' + imu0_topic = '/imu0' + grnd_topic = '/vrpn_client/raw_transform' + + rospy.init_node('controlbag') + rospy.set_param('play_bag', False) + + cam0_pub = rospy.Publisher(cam0_topic, Image, queue_size=10) + cam1_pub = rospy.Publisher(cam1_topic, Image, queue_size=10) + imu0_pub = rospy.Publisher(imu0_topic, Imu, queue_size=10) + grnd_pub = rospy.Publisher(grnd_topic, TransformStamped, queue_size=10) + + signal.signal(signal.SIGINT, signal_handler) + + bag = rosbag.Bag('/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag') + for topic, msg, t in bag.read_messages(topics=[cam0_topic, cam1_topic, imu0_topic, grnd_topic]): + + # pause if parameter set to false + flag = False + while not rospy.get_param('/play_bag'): + time.sleep(0.01) + if not flag: + print("stopped playback") + flag = not flag + + if flag: + print("resume playback") + + if topic == cam0_topic: + cam0_pub.publish(msg) + + elif topic == cam1_topic: + cam1_pub.publish(msg) + + elif topic == imu0_topic: + imu0_pub.publish(msg) + + elif topic ==grnd_topic: + grnd_pub.publish(msg) + + #print msg + bag.close() + +if __name__== "__main__": + main() \ No newline at end of file From c565033d7ff5a910a7c297874d5a55c1b12d0bb3 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Wed, 19 Jun 2019 16:59:32 +0000 Subject: [PATCH 55/86] Add new file --- src/bagcontrol.py | 64 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 src/bagcontrol.py diff --git a/src/bagcontrol.py b/src/bagcontrol.py new file mode 100644 index 0000000..3292b5b --- /dev/null +++ b/src/bagcontrol.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python +import rosbag +import rospy +from sensor_msgs.msg import Imu, Image +from geometry_msgs.msg import TransformStamped +import time +import signal +import sys + + +def signal_handler(sig, frame): + print('gracefully exiting the program.') + bag.close() + sys.exit(0) + +def main(): + global bag + + cam0_topic = '/cam0/image_raw' + cam1_topic = '/cam1/image_raw' + imu0_topic = '/imu0' + grnd_topic = '/vrpn_client/raw_transform' + + rospy.init_node('controlbag') + rospy.set_param('play_bag', False) + + cam0_pub = rospy.Publisher(cam0_topic, Image, queue_size=10) + cam1_pub = rospy.Publisher(cam1_topic, Image, queue_size=10) + imu0_pub = rospy.Publisher(imu0_topic, Imu, queue_size=10) + grnd_pub = rospy.Publisher(grnd_topic, TransformStamped, queue_size=10) + + signal.signal(signal.SIGINT, signal_handler) + + bag = rosbag.Bag('/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag') + for topic, msg, t in bag.read_messages(topics=[cam0_topic, cam1_topic, imu0_topic, grnd_topic]): + + # pause if parameter set to false + flag = False + while not rospy.get_param('/play_bag'): + time.sleep(0.01) + if not flag: + print("stopped playback") + flag = not flag + + if flag: + print("resume playback") + + if topic == cam0_topic: + cam0_pub.publish(msg) + + elif topic == cam1_topic: + cam1_pub.publish(msg) + + elif topic == imu0_topic: + imu0_pub.publish(msg) + + elif topic ==grnd_topic: + grnd_pub.publish(msg) + + #print msg + bag.close() + +if __name__== "__main__": + main() \ No newline at end of file From 6f7f8b78928577c8985d3e2f93084251b5018429 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Tue, 25 Jun 2019 12:03:08 +0200 Subject: [PATCH 56/86] added tiny tum --- config/camchain-imucam-tum-scaled.yaml | 16 ++++++------- launch/image_processor_tinytum.launch | 4 ++-- launch/msckf_vio_tum.launch | 6 ++--- src/msckf_vio.cpp | 33 ++++++++++++++++++++------ 4 files changed, 39 insertions(+), 20 deletions(-) diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index 529828c..01f1983 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -5,11 +5,11 @@ cam0: -0.029453096117288798, -0.9989836729399656, 0.034119442089241274, -0.0697294754693238, 0.0, 0.0, 0.0, 1.0] camera_model: pinhole - distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, - -0.001662284667857643] + distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, + 0.00020293673591811182] distortion_model: equidistant - intrinsics: [95.2026071784, 95.2029854486, 127.573663262, 128.582615763] - resolution: [256, 256] + intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] + resolution: [512, 512] rostopic: /cam0/image_raw cam1: T_cam_imu: @@ -23,11 +23,11 @@ cam1: 0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572, 0.0, 0.0, 0.0, 1.0] camera_model: pinhole - distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, - -0.002347858896562788] + distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, + 0.0003299517423931039] distortion_model: equidistant - intrinsics: [94.8217471066, 94.8164593555, 126.391667581, 127.571024044] - resolution: [256, 256] + intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] + resolution: [512, 512] rostopic: /cam1/image_raw T_imu_body: [1.0000, 0.0000, 0.0000, 0.0000, diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 6fe2855..53dd99a 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -25,8 +25,8 @@ - - + + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 390ab03..0240de3 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,11 +18,11 @@ output="screen"> - + - + @@ -72,6 +72,6 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 323e791..b6245e2 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1664,10 +1664,29 @@ void MsckfVio::featureJacobian( { ofstream myfile; - myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; + myfile.open("/home/raphael/dev/octave/org2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: Hx\n" + << "# type: matrix\n" + << "# rows: " << H_xj.rows() << "\n" + << "# columns: " << H_xj.cols() << "\n" + << H_xj << endl; + + myfile << "# name: Hy\n" + << "# type: matrix\n" + << "# rows: " << H_fj.rows() << "\n" + << "# columns: " << H_fj.cols() << "\n" + << H_fj << endl; + + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r_j.rows() << "\n" + << "# columns: " << 1 << "\n" + << r_j << endl; + myfile.close(); - cout << "---------- LOGGED -------- " << endl; + cout << "---------- LOGGED ORG -------- " << endl; } return; @@ -1716,8 +1735,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { MatrixXd K = K_transpose.transpose(); // Compute the error of the state. VectorXd delta_x = K * r; - cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; + // cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. if(PHOTOMETRIC) return; @@ -1821,8 +1840,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // Compute the error of the state. VectorXd delta_x = K * r; - cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; + // cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. if (not PHOTOMETRIC) return; From 1d6100ed13c4e5280bc55bf39f7f4fab6f3b0754 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Tue, 25 Jun 2019 19:05:53 +0200 Subject: [PATCH 57/86] added live toggle for photometric --- include/msckf_vio/feature.hpp | 2 -- launch/msckf_vio_euroc.launch | 10 ++++++ launch/msckf_vio_tinytum.launch | 17 +++++----- launch/msckf_vio_tum.launch | 4 +-- src/msckf_vio.cpp | 55 +++++++++++++++++++++------------ 5 files changed, 55 insertions(+), 33 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 466d1e9..405f2d4 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -1008,8 +1008,6 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal bool Feature::initializeAnchor(const CameraCalibration& cam, int N) { - - //initialize patch Size int n = (int)(N-1)/2; diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 896feff..c9e9923 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -17,6 +17,16 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> + + + + + + + + + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 94deb7c..44b3312 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -6,7 +6,7 @@ default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/> - + @@ -18,14 +18,14 @@ output="screen"> - + - - + + - + @@ -64,16 +64,13 @@ - - + + - - - diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 0240de3..a5b65c7 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,10 +18,10 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index b6245e2..a3d5dd6 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -412,6 +412,8 @@ void MsckfVio::imageCallback( if(STREAMPAUSE) nh.setParam("/play_bag", false); + + nh.param("PHOTOMETRIC", PHOTOMETRIC, false); // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -448,7 +450,7 @@ void MsckfVio::imageCallback( double imu_processing_time = ( ros::Time::now()-start_time).toSec(); - //cout << "1" << endl; + // cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); @@ -457,7 +459,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "2" << endl; + // cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -466,7 +468,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "3" << endl; + // cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); @@ -474,20 +476,20 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "4" << endl; + // cout << "4" << endl; // Perform measurement update if necessary. start_time = ros::Time::now(); removeLostFeatures(); double remove_lost_features_time = ( ros::Time::now()-start_time).toSec(); - //cout << "5" << endl; + // cout << "5" << endl; start_time = ros::Time::now(); - pruneLastCamStateBuffer(); + pruneCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); - //cout << "6" << endl; + // cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -1368,7 +1370,9 @@ void MsckfVio::PhotometricMeasurementJacobian( } MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); + //MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); + + MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1); // set anchor Jakobi // get position of anchor in cam states @@ -1389,13 +1393,17 @@ void MsckfVio::PhotometricMeasurementJacobian( H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); // set irradiance error Block - H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N); + //H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N); // TODO make this calculation more fluent + /* for(int i = 0; i< N*N; i++) H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho; - + */ + H_yl.block(0, 0, N*N, N*N) = Eigen::MatrixXd::Identity(N*N, N*N); + H_yl.block(0, N*N, N*N, 1) = -H_rho; + H_x = H_xl; H_y = H_yl; @@ -1439,7 +1447,10 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, 21+state_server.cam_states.size()*7); - MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1); + + // MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1); + MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+1); + VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1460,6 +1471,8 @@ void MsckfVio::PhotometricFeatureJacobian( H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl; r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; + + } // Project the residual and Jacobians onto the nullspace @@ -1798,8 +1811,10 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { - if (H.rows() == 0 || r.rows() == 0) + if (H.rows() == 0 || r.rows() == 0) + { return; + } // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -1840,10 +1855,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // Compute the error of the state. VectorXd delta_x = K * r; - // cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - // cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. + + if (not PHOTOMETRIC) return; + + cout << "msc veloci: " << delta_x[6] << ", " << delta_x[7] << ", " << delta_x[8] << endl; + cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1900,6 +1918,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { + return 1; MatrixXd P1 = H * state_server.state_cov * H.transpose(); @@ -1908,8 +1927,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - //cout << "gate: " << dof << " " << gamma << " " << - //chi_squared_test_table[dof] << endl; + cout << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; if (chi_squared_test_table[dof] == 0) return false; @@ -2008,12 +2027,12 @@ void MsckfVio::removeLostFeatures() { PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); } + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; @@ -2180,7 +2199,6 @@ void MsckfVio::pruneLastCamStateBuffer() feature.observations.erase(cam_id); } - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2188,7 +2206,6 @@ void MsckfVio::pruneLastCamStateBuffer() pr.conservativeResize(pstack_cntr); // Perform measurement update. - measurementUpdate(H_x, r); photometricMeasurementUpdate(pH_x, pr); From 273bbf8a9434bff1d617380d760bf4b56d8cb0ce Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Tue, 25 Jun 2019 19:49:04 +0200 Subject: [PATCH 58/86] added fov camera model --- src/image_handler.cpp | 73 ++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 68 insertions(+), 5 deletions(-) diff --git a/src/image_handler.cpp b/src/image_handler.cpp index cb98426..d5e62ec 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -45,7 +45,22 @@ void undistortPoint( } else if (distortion_model == "equidistant") { cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else { + } + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + float omega = distortion_coeffs[0]; + float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float ru = tan(rd * omega)/(2 * tan(omega / 2)); + + cv::Point2f newPoint( + ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd), + ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd)); + + pts_out.push_back(newPoint); + } + } + else { ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", distortion_model.c_str()); cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, @@ -79,10 +94,26 @@ void undistortPoints( if (distortion_model == "radtan") { cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else if (distortion_model == "equidistant") { + } + else if (distortion_model == "equidistant") { cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else { + } + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + float omega = distortion_coeffs[0]; + float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float ru = tan(rd * omega)/(2 * tan(omega / 2)); + + cv::Point2f newPoint( + ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd), + ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd)); + + pts_out.push_back(newPoint); + } + } + else { ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", distortion_model.c_str()); cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, @@ -110,7 +141,23 @@ std::vector distortPoints( distortion_coeffs, pts_out); } else if (distortion_model == "equidistant") { cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs); - } else { + } + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + // based on 'straight lines have to be straight' + float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float omega = distortion_coeffs[0]; + float rd = 1 / (omega)*atan(2*ru*tan(omega / 2)); + + cv::Point2f newPoint( + pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2], + pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]); + + pts_out.push_back(newPoint); + } + } + else { ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", distortion_model.c_str()); std::vector homogenous_pts; @@ -143,7 +190,23 @@ cv::Point2f distortPoint( distortion_coeffs, pts_out); } else if (distortion_model == "equidistant") { cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs); - } else { + } + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + // based on 'straight lines have to be straight' + float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float omega = distortion_coeffs[0]; + float rd = 1 / (omega)*atan(2*ru*tan(omega / 2)); + + cv::Point2f newPoint( + pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2], + pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]); + + pts_out.push_back(newPoint); + } + } + else { ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", distortion_model.c_str()); std::vector homogenous_pts; From ebc73c0c5ec382de12ae22025d4b197acf4f6e82 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Wed, 26 Jun 2019 19:23:50 +0200 Subject: [PATCH 59/86] not working, added stop and go to image processing, added undistorted image to image processing --- config/camchain-imucam-tum-scaled.yaml | 4 +- include/msckf_vio/feature.hpp | 138 ++++++++++++++++++++++--- include/msckf_vio/image_handler.h | 3 + include/msckf_vio/image_processor.h | 2 + include/msckf_vio/msckf_vio.h | 2 +- launch/image_processor_tinytum.launch | 3 + launch/image_processor_tum.launch | 3 + launch/msckf_vio_tinytum.launch | 4 +- src/image_handler.cpp | 56 +++++++++- src/image_processor.cpp | 31 +++++- src/msckf_vio.cpp | 58 ++++++++--- 11 files changed, 260 insertions(+), 44 deletions(-) diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index 01f1983..40b2dc0 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] resolution: [512, 512] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] resolution: [512, 512] rostopic: /cam1/image_raw diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 405f2d4..54157e7 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -137,6 +137,9 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, inline bool checkMotion( const CamStateServer& cam_states) const; + + bool initializeAnchorUndistort(const CameraCalibration& cam, int N); + /* * @brief InitializeAnchor generates the NxN patch around the * feature in the Anchor image @@ -171,6 +174,12 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, Eigen::Vector3d& in_p) const; +cv::Point2f projectPositionToCameraUndistorted( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const CameraCalibration& cam, + Eigen::Vector3d& in_p) const; + /* * @brief project PositionToCamera Takes a 3d position in a world frame @@ -692,26 +701,22 @@ bool Feature::VisualizeKernel( //cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3); - cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type()); - cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type()); - - - cv::Mat norm_abs_xderImage; - cv::normalize(abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); - - cv::imshow("xder", norm_abs_xderImage); - cv::imshow("yder", abs_yderImage); + // cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type()); + // cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type()); + /* for(int i = 1; i < anchorImage.rows-1; i++) for(int j = 1; j < anchorImage.cols-1; j++) xderImage2.at(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_x")); + for(int i = 1; i < anchorImage.rows-1; i++) for(int j = 1; j < anchorImage.cols-1; j++) yderImage2.at(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_y")); - cv::imshow("anchor", anchorImage); - cv::imshow("xder2", xderImage2); - cv::imshow("yder2", yderImage2); + */ + //cv::imshow("anchor", anchorImage); + cv::imshow("xder2", xderImage); + cv::imshow("yder2", yderImage); cvWaitKey(0); } @@ -752,7 +757,7 @@ bool Feature::VisualizePatch( std::vector projectionPatch; for(auto point : anchorPatch_3d) { - cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_c0 = projectPositionToCameraUndistorted(cam_state, cam_state_id, cam0, point); projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image)); // visu - feature cv::Point xs(p_in_c0.x, p_in_c0.y); @@ -807,10 +812,11 @@ bool Feature::VisualizePatch( cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); - cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1)); + cv::Point2f p_f(observations.find(cam_state_id)->second(0)*cam0.intrinsics[0] + cam0.intrinsics[2],observations.find(cam_state_id)->second(1)*cam0.intrinsics[1] + cam0.intrinsics[3]); // move to real pixels - p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); - + + // without distort + // p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); for(int i = 0; ifirst) + //printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y); + + cv::Point2f my_p(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]); + + // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); + // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); + // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); + + return my_p; +} cv::Point2f Feature::projectPositionToCamera( const CAMState& cam_state, @@ -1080,6 +1113,79 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) return true; } +//@test center projection must always be initial feature projection +bool Feature::initializeAnchorUndistort(const CameraCalibration& cam, int N) +{ + + //initialize patch Size + int n = (int)(N-1)/2; + + auto anchor = observations.begin(); + if(cam.moving_window.find(anchor->first) == cam.moving_window.end()) + return false; + + cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image; + cv::Mat anchorImage_deeper; + anchorImage.convertTo(anchorImage_deeper,CV_16S); + //TODO remove this? + + + cv::Mat hen = cv::Mat::zeros(cv::Size(3, 3), CV_16S); + hen.at(1,0) = 1; + + //sobel test + /* + cv::Mat newhen; + cv::Sobel(hen, newhen, -1, 1, 0, 3); + std::cout << "newhen" << newhen << std::endl; + */ + + cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3); + cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3); + + xderImage/=4; + yderImage/=4; + + cv::convertScaleAbs(xderImage, abs_xderImage); + cv::convertScaleAbs(yderImage, abs_yderImage); + + cvWaitKey(0); + + auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2]; + auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3]; + + //project onto pixel plane + undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]); + + // create vector of patch in pixel plane + for(double u_run = -n; u_run <= n; u_run++) + for(double v_run = -n; v_run <= n; v_run++) + anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run)); + + //project back into u,v + for(int i = 0; i < N*N; i++) + anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1])); + + + // save anchor position for later visualisaztion + anchor_center_pos = anchorPatch_real[(N*N-1)/2]; + + // save true pixel Patch position + for(auto point : anchorPatch_real) + if(point.x - n < 0 || point.x + n >= cam.resolution(0)-1 || point.y - n < 0 || point.y + n >= cam.resolution(1)-1) + return false; + + for(auto point : anchorPatch_real) + anchorPatch.push_back(PixelIrradiance(point, anchorImage)); + + // project patch pixel to 3D space in camera coordinate system + for(auto point : anchorPatch_ideal) + anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam)); + + is_anchored = true; + return true; +} + bool Feature::initializeRho(const CamStateServer& cam_states) { // Organize camera poses and feature observations properly. diff --git a/include/msckf_vio/image_handler.h b/include/msckf_vio/image_handler.h index d664be6..eae27c3 100644 --- a/include/msckf_vio/image_handler.h +++ b/include/msckf_vio/image_handler.h @@ -16,6 +16,9 @@ namespace msckf_vio { */ namespace image_handler { +cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics); +cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics); + void undistortPoints( const std::vector& pts_in, const cv::Vec4d& intrinsics, diff --git a/include/msckf_vio/image_processor.h b/include/msckf_vio/image_processor.h index 638fbcd..7fb3943 100644 --- a/include/msckf_vio/image_processor.h +++ b/include/msckf_vio/image_processor.h @@ -320,6 +320,8 @@ private: return; } + bool STREAMPAUSE; + // Indicate if this is the first image message. bool is_first_img; diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 9e9975d..3394a0e 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -207,7 +207,7 @@ class MsckfVio { Eigen::MatrixXd& H_x, Eigen::VectorXd& r); - void PhotometricMeasurementJacobian( + bool PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, Eigen::MatrixXd& H_x, diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 53dd99a..7500ff2 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -11,6 +11,9 @@ output="screen" > + + + diff --git a/launch/image_processor_tum.launch b/launch/image_processor_tum.launch index d2a02e4..135af70 100644 --- a/launch/image_processor_tum.launch +++ b/launch/image_processor_tum.launch @@ -11,6 +11,9 @@ output="screen" > + + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 44b3312..beaaef4 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -21,11 +21,11 @@ - + - + diff --git a/src/image_handler.cpp b/src/image_handler.cpp index d5e62ec..bd2a699 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -14,6 +14,16 @@ namespace msckf_vio { namespace image_handler { +cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics) +{ + return cv::Point2f(p_in.x * intrinsics[0] + intrinsics[2], p_in.y * intrinsics[1] + intrinsics[3]); +} + +cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics) +{ + return cv::Point2f((p_in.x - intrinsics[2])/intrinsics[0], (p_in.y - intrinsics[3])/intrinsics[1]); +} + void undistortPoint( const cv::Point2f& pt_in, const cv::Vec4d& intrinsics, @@ -42,10 +52,13 @@ void undistortPoint( if (distortion_model == "radtan") { cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else if (distortion_model == "equidistant") { + } + // equidistant + else if (distortion_model == "equidistant") { cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); } + // fov else if (distortion_model == "fov") { for(int i = 0; i < pts_in.size(); i++) { @@ -59,7 +72,15 @@ void undistortPoint( pts_out.push_back(newPoint); } - } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + } else { ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", distortion_model.c_str()); @@ -112,7 +133,16 @@ void undistortPoints( pts_out.push_back(newPoint); } - } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + + } else { ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", distortion_model.c_str()); @@ -156,7 +186,15 @@ std::vector distortPoints( pts_out.push_back(newPoint); } - } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + temp_pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + } else { ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", distortion_model.c_str()); @@ -205,7 +243,15 @@ cv::Point2f distortPoint( pts_out.push_back(newPoint); } - } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + } else { ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", distortion_model.c_str()); diff --git a/src/image_processor.cpp b/src/image_processor.cpp index d35bb16..da8494e 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -42,6 +42,9 @@ ImageProcessor::~ImageProcessor() { } bool ImageProcessor::loadParameters() { + + // debug parameters + nh.param("StreamPause", STREAMPAUSE, false); // Camera calibration parameters nh.param("cam0/distortion_model", cam0.distortion_model, string("radtan")); @@ -211,7 +214,9 @@ void ImageProcessor::stereoCallback( const sensor_msgs::ImageConstPtr& cam0_img, const sensor_msgs::ImageConstPtr& cam1_img) { - //cout << "==================================" << endl; + // stop playing bagfile if printing images + if(STREAMPAUSE) + nh.setParam("/play_bag_image", false); // Get the current image. cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img, @@ -219,6 +224,18 @@ void ImageProcessor::stereoCallback( cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img, sensor_msgs::image_encodings::MONO8); + + const cv::Matx33d K0(cam0.intrinsics[0], 0.0, cam0.intrinsics[2], + 0.0, cam0.intrinsics[1], cam0.intrinsics[3], + 0.0, 0.0, 1.0); + const cv::Matx33d K1(cam1.intrinsics[0], 0.0, cam1.intrinsics[2], + 0.0, cam1.intrinsics[1], cam1.intrinsics[3], + 0.0, 0.0, 1.0); + + + cv::fisheye::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, K0, cam0.distortion_coeffs, K0); + cv::fisheye::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, K1, cam1.distortion_coeffs, K1); + // Build the image pyramids once since they're used at multiple places createImagePyramids(); @@ -245,6 +262,7 @@ void ImageProcessor::stereoCallback( // Add new features into the current image. start_time = ros::Time::now(); addNewFeatures(); + //ROS_INFO("Addition time: %f", // (ros::Time::now()-start_time).toSec()); @@ -267,16 +285,19 @@ void ImageProcessor::stereoCallback( // (ros::Time::now()-start_time).toSec()); // Publish features in the current image. + ros::Time start_time = ros::Time::now(); publish(); //ROS_INFO("Publishing: %f", // (ros::Time::now()-start_time).toSec()); // Update the previous image and previous features. + cam0_prev_img_ptr = cam0_curr_img_ptr; prev_features_ptr = curr_features_ptr; std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_); + // Initialize the current features to empty vectors. curr_features_ptr.reset(new GridFeatures()); for (int code = 0; code < @@ -284,6 +305,10 @@ void ImageProcessor::stereoCallback( (*curr_features_ptr)[code] = vector(0); } + // stop playing bagfile if printing images + if(STREAMPAUSE) + nh.setParam("/play_bag_image", true); + return; } @@ -580,6 +605,7 @@ void ImageProcessor::trackFeatures() { ++after_ransac; } + // Compute the tracking rate. int prev_feature_num = 0; for (const auto& item : *prev_features_ptr) @@ -659,6 +685,8 @@ void ImageProcessor::stereoMatch( // Further remove outliers based on the known // essential matrix. + + vector cam0_points_undistorted(0); vector cam1_points_undistorted(0); image_handler::undistortPoints( @@ -668,6 +696,7 @@ void ImageProcessor::stereoMatch( cam1_points, cam1.intrinsics, cam1.distortion_model, cam1.distortion_coeffs, cam1_points_undistorted); + double norm_pixel_unit = 4.0 / ( cam0.intrinsics[0]+cam0.intrinsics[1]+ cam1.intrinsics[0]+cam1.intrinsics[1]); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index a3d5dd6..e5f5b3f 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -450,7 +450,7 @@ void MsckfVio::imageCallback( double imu_processing_time = ( ros::Time::now()-start_time).toSec(); - // cout << "1" << endl; + //cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); @@ -459,7 +459,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - // cout << "2" << endl; + //cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -468,7 +468,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - // cout << "3" << endl; + //cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); @@ -476,20 +476,20 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - // cout << "4" << endl; + //cout << "4" << endl; // Perform measurement update if necessary. start_time = ros::Time::now(); removeLostFeatures(); double remove_lost_features_time = ( ros::Time::now()-start_time).toSec(); - // cout << "5" << endl; + //cout << "5" << endl; start_time = ros::Time::now(); pruneCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); - // cout << "6" << endl; + //cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -552,6 +552,20 @@ void MsckfVio::manageMovingWindow( cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone(); cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); + + // undistort Images + + const cv::Matx33d K(cam0.intrinsics[0], 0.0, cam0.intrinsics[2], + 0.0, cam0.intrinsics[1], cam0.intrinsics[3], + 0.0, 0.0, 1.0); + + cv::Mat undistortedCam0; + cv::fisheye::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, K, cam0.distortion_coeffs, K); + //cv::imshow("anchor", undistortedCam0); + //cvWaitKey(0); + cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone(); + cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); + //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) while(cam0.moving_window.size() > 100) { @@ -1228,7 +1242,7 @@ void MsckfVio::addFeatureObservations( return; } -void MsckfVio::PhotometricMeasurementJacobian( +bool MsckfVio::PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) @@ -1305,21 +1319,25 @@ void MsckfVio::PhotometricMeasurementJacobian( for (auto point : feature.anchorPatch_3d) { + //cout << "____feature-measurement_____\n" << endl; Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); - cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); - cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); + cv::Point2f p_in_c0 = feature.projectPositionToCameraUndistorted(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_anchor = feature.projectPositionToCameraUndistorted(anchor_state, anchor_state_id, cam0, point); + if( p_in_c0.x < 0 || p_in_c0.x >= cam0.resolution(0) || p_in_c0.y < 0 || p_in_c0.y >= cam0.resolution(1) ) + { + cout << "skip" << endl; + return false; + } //add observation - photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - + photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); //calculate photom. residual r_photo(count) = photo_z[count] - estimate_photo_z[count]; //cout << "residual: " << photo_r.back() << endl; // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.cvKernel(p_in_anchor, "Sobel_x"); dy = feature.cvKernel(p_in_anchor, "Sobel_y"); @@ -1329,6 +1347,7 @@ void MsckfVio::PhotometricMeasurementJacobian( dI_dhj(0, 0) = dx * cam0.intrinsics[0]; dI_dhj(0, 1) = dy * cam0.intrinsics[1]; + //dh / d{}^Cp_{ij} dh_dCpij(0, 0) = 1 / p_c0(2); dh_dCpij(1, 1) = 1 / p_c0(2); @@ -1369,6 +1388,7 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } + MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); //MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); @@ -1419,7 +1439,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } - return; + return true; } void MsckfVio::PhotometricFeatureJacobian( @@ -1460,7 +1480,8 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd H_yl; Eigen::VectorXd r_l = VectorXd::Zero(N*N); - PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); + if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) + continue; auto cam_state_iter = state_server.cam_states.find(cam_id); int cam_state_cntr = std::distance( @@ -1475,6 +1496,9 @@ void MsckfVio::PhotometricFeatureJacobian( } + + H_xi.conservativeResize(stack_cntr, H_xi.cols()); + H_yi.conservativeResize(stack_cntr, H_yi.cols()); // Project the residual and Jacobians onto the nullspace // of H_yj. @@ -1978,7 +2002,7 @@ void MsckfVio::removeLostFeatures() { } if(!feature.is_anchored) { - if(!feature.initializeAnchor(cam0, N)) + if(!feature.initializeAnchorUndistort(cam0, N)) { invalid_feature_ids.push_back(feature.id); continue; @@ -2144,7 +2168,7 @@ void MsckfVio::pruneLastCamStateBuffer() } if(!feature.is_anchored) { - if(!feature.initializeAnchor(cam0, N)) + if(!feature.initializeAnchorUndistort(cam0, N)) { feature.observations.erase(rm_cam_state_id); continue; @@ -2296,7 +2320,7 @@ void MsckfVio::pruneCamStateBuffer() { } if(!feature.is_anchored) { - if(!feature.initializeAnchor(cam0, N)) + if(!feature.initializeAnchorUndistort(cam0, N)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); From b3e86b3e64daab8827cdd70130c153ab009a1039 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 15:57:24 +0200 Subject: [PATCH 60/86] changed structure for image undistort into image_handler --- include/msckf_vio/feature.hpp | 187 ++++++++---------------------- include/msckf_vio/image_handler.h | 7 ++ launch/msckf_vio_tum.launch | 6 +- src/image_handler.cpp | 18 +++ src/image_processor.cpp | 22 ++-- src/msckf_vio.cpp | 74 +++++++----- 6 files changed, 128 insertions(+), 186 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 54157e7..839f564 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -138,8 +138,6 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, const CamStateServer& cam_states) const; - bool initializeAnchorUndistort(const CameraCalibration& cam, int N); - /* * @brief InitializeAnchor generates the NxN patch around the * feature in the Anchor image @@ -174,13 +172,6 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, Eigen::Vector3d& in_p) const; -cv::Point2f projectPositionToCameraUndistorted( - const CAMState& cam_state, - const StateIDType& cam_state_id, - const CameraCalibration& cam, - Eigen::Vector3d& in_p) const; - - /* * @brief project PositionToCamera Takes a 3d position in a world frame * and projects it into the passed camera frame using pinhole projection @@ -757,7 +748,7 @@ bool Feature::VisualizePatch( std::vector projectionPatch; for(auto point : anchorPatch_3d) { - cv::Point2f p_in_c0 = projectPositionToCameraUndistorted(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image)); // visu - feature cv::Point xs(p_in_c0.x, p_in_c0.y); @@ -964,35 +955,6 @@ cv::Point2f Feature::pixelDistanceAt( return distance; } -cv::Point2f Feature::projectPositionToCameraUndistorted( - const CAMState& cam_state, - const StateIDType& cam_state_id, - const CameraCalibration& cam, - Eigen::Vector3d& in_p) const -{ - Eigen::Isometry3d T_c0_w; - - cv::Point2f out_p; - - // transfrom position to camera frame - Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); - const Eigen::Vector3d& t_c0_w = cam_state.position; - Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); - - out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); - - // if(cam_state_id == observations.begin()->first) - //printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y); - - cv::Point2f my_p(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]); - - // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); - // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); - // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); - - return my_p; -} - cv::Point2f Feature::projectPositionToCamera( const CAMState& cam_state, const StateIDType& cam_state_id, @@ -1009,15 +971,17 @@ cv::Point2f Feature::projectPositionToCamera( Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); + cv::Point2f my_p; - // if(cam_state_id == observations.begin()->first) - //printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y); - - cv::Point2f my_p = image_handler::distortPoint(out_p, + // test is prewarped or not + if (cam.distortion_model.substr(0,3) == "pre-") + my_p = cv::Point2f(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]); + else + my_p = image_handler::distortPoint(out_p, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); - + // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); @@ -1068,121 +1032,64 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2]; auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3]; - //testing - undist_anchor_center_pos = cv::Point2f(u,v); - //for NxN patch pixels around feature - int count = 0; + // check if image has been pre-undistorted + if(cam.distortion_model.substr(0,3) == "pre-") + { + std::cout << "is a pre" << std::endl; + //project onto pixel plane + undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]); - // get feature in undistorted pixel space - // this only reverts from 'pure' space into undistorted pixel space using camera matrix - cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs); - // create vector of patch in pixel plane - for(double u_run = -n; u_run <= n; u_run++) - for(double v_run = -n; v_run <= n; v_run++) - anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); + // create vector of patch in pixel plane + for(double u_run = -n; u_run <= n; u_run++) + for(double v_run = -n; v_run <= n; v_run++) + anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run)); + + //project back into u,v + for(int i = 0; i < N*N; i++) + anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1])); + } + + else + { + // get feature in undistorted pixel space + // this only reverts from 'pure' space into undistorted pixel space using camera matrix + cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); + // create vector of patch in pixel plane + for(double u_run = -n; u_run <= n; u_run++) + for(double v_run = -n; v_run <= n; v_run++) + anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); - //create undistorted pure points - image_handler::undistortPoints(anchorPatch_real, - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs, - anchorPatch_ideal); + //create undistorted pure points + image_handler::undistortPoints(anchorPatch_real, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs, + anchorPatch_ideal); + } // save anchor position for later visualisaztion anchor_center_pos = anchorPatch_real[(N*N-1)/2]; - - // save true pixel Patch position - for(auto point : anchorPatch_real) - if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) - return false; - - for(auto point : anchorPatch_real) - anchorPatch.push_back(PixelIrradiance(point, anchorImage)); - - // project patch pixel to 3D space in camera coordinate system - for(auto point : anchorPatch_ideal) - anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam)); - - is_anchored = true; - return true; -} - -//@test center projection must always be initial feature projection -bool Feature::initializeAnchorUndistort(const CameraCalibration& cam, int N) -{ - - //initialize patch Size - int n = (int)(N-1)/2; - - auto anchor = observations.begin(); - if(cam.moving_window.find(anchor->first) == cam.moving_window.end()) - return false; - - cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image; - cv::Mat anchorImage_deeper; - anchorImage.convertTo(anchorImage_deeper,CV_16S); - //TODO remove this? - - - cv::Mat hen = cv::Mat::zeros(cv::Size(3, 3), CV_16S); - hen.at(1,0) = 1; - - //sobel test - /* - cv::Mat newhen; - cv::Sobel(hen, newhen, -1, 1, 0, 3); - std::cout << "newhen" << newhen << std::endl; - */ - - cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3); - cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3); - - xderImage/=4; - yderImage/=4; - - cv::convertScaleAbs(xderImage, abs_xderImage); - cv::convertScaleAbs(yderImage, abs_yderImage); - - cvWaitKey(0); - - auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2]; - auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3]; - - //project onto pixel plane - undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]); - - // create vector of patch in pixel plane - for(double u_run = -n; u_run <= n; u_run++) - for(double v_run = -n; v_run <= n; v_run++) - anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run)); - - //project back into u,v - for(int i = 0; i < N*N; i++) - anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1])); - - - // save anchor position for later visualisaztion - anchor_center_pos = anchorPatch_real[(N*N-1)/2]; - + // save true pixel Patch position for(auto point : anchorPatch_real) if(point.x - n < 0 || point.x + n >= cam.resolution(0)-1 || point.y - n < 0 || point.y + n >= cam.resolution(1)-1) return false; - + for(auto point : anchorPatch_real) anchorPatch.push_back(PixelIrradiance(point, anchorImage)); - + // project patch pixel to 3D space in camera coordinate system for(auto point : anchorPatch_ideal) anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam)); is_anchored = true; + return true; } diff --git a/include/msckf_vio/image_handler.h b/include/msckf_vio/image_handler.h index eae27c3..3cf2ed2 100644 --- a/include/msckf_vio/image_handler.h +++ b/include/msckf_vio/image_handler.h @@ -19,6 +19,13 @@ namespace image_handler { cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics); cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics); +void undistortImage( + cv::InputArray src, + cv::OutputArray dst, + const std::string& distortion_model, + const cv::Vec4d& intrinsics, + const cv::Vec4d& distortion_coeffs); + void undistortPoints( const std::vector& pts_in, const cv::Vec4d& intrinsics, diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index a5b65c7..1c2c689 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,11 +18,11 @@ output="screen"> - + - - + + diff --git a/src/image_handler.cpp b/src/image_handler.cpp index bd2a699..324ffdb 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -24,6 +24,24 @@ cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsic return cv::Point2f((p_in.x - intrinsics[2])/intrinsics[0], (p_in.y - intrinsics[3])/intrinsics[1]); } +void undistortImage( + cv::InputArray src, + cv::OutputArray dst, + const std::string& distortion_model, + const cv::Vec4d& intrinsics, + const cv::Vec4d& distortion_coeffs) +{ + + const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2], + 0.0, intrinsics[1], intrinsics[3], + 0.0, 0.0, 1.0); + + if (distortion_model == "pre-equidistant") + cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K); + else if (distortion_model == "equidistant") + src.copyTo(dst); +} + void undistortPoint( const cv::Point2f& pt_in, const cv::Vec4d& intrinsics, diff --git a/src/image_processor.cpp b/src/image_processor.cpp index da8494e..0e01957 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -215,8 +215,8 @@ void ImageProcessor::stereoCallback( const sensor_msgs::ImageConstPtr& cam1_img) { // stop playing bagfile if printing images - if(STREAMPAUSE) - nh.setParam("/play_bag_image", false); + //if(STREAMPAUSE) + // nh.setParam("/play_bag_image", false); // Get the current image. cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img, @@ -225,17 +225,13 @@ void ImageProcessor::stereoCallback( sensor_msgs::image_encodings::MONO8); - const cv::Matx33d K0(cam0.intrinsics[0], 0.0, cam0.intrinsics[2], - 0.0, cam0.intrinsics[1], cam0.intrinsics[3], - 0.0, 0.0, 1.0); - const cv::Matx33d K1(cam1.intrinsics[0], 0.0, cam1.intrinsics[2], - 0.0, cam1.intrinsics[1], cam1.intrinsics[3], - 0.0, 0.0, 1.0); + cv::Mat undistortedCam0; + cv::Mat undistortedCam1; - cv::fisheye::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, K0, cam0.distortion_coeffs, K0); - cv::fisheye::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, K1, cam1.distortion_coeffs, K1); - + //image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + //image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + // Build the image pyramids once since they're used at multiple places createImagePyramids(); @@ -306,8 +302,8 @@ void ImageProcessor::stereoCallback( } // stop playing bagfile if printing images - if(STREAMPAUSE) - nh.setParam("/play_bag_image", true); + //if(STREAMPAUSE) + // nh.setParam("/play_bag_image", true); return; } diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index e5f5b3f..9a31f42 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -450,7 +450,7 @@ void MsckfVio::imageCallback( double imu_processing_time = ( ros::Time::now()-start_time).toSec(); - //cout << "1" << endl; + cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); @@ -554,17 +554,15 @@ void MsckfVio::manageMovingWindow( // undistort Images - - const cv::Matx33d K(cam0.intrinsics[0], 0.0, cam0.intrinsics[2], - 0.0, cam0.intrinsics[1], cam0.intrinsics[3], - 0.0, 0.0, 1.0); - cv::Mat undistortedCam0; - cv::fisheye::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, K, cam0.distortion_coeffs, K); - //cv::imshow("anchor", undistortedCam0); - //cvWaitKey(0); - cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone(); - cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); + //cv::Mat undistortedCam0; + //cv::Mat undistortedCam1; + + //image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + //image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + + //cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone(); + //cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone(); //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) while(cam0.moving_window.size() > 100) @@ -1322,8 +1320,8 @@ bool MsckfVio::PhotometricMeasurementJacobian( //cout << "____feature-measurement_____\n" << endl; Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); - cv::Point2f p_in_c0 = feature.projectPositionToCameraUndistorted(cam_state, cam_state_id, cam0, point); - cv::Point2f p_in_anchor = feature.projectPositionToCameraUndistorted(anchor_state, anchor_state_id, cam0, point); + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); if( p_in_c0.x < 0 || p_in_c0.x >= cam0.resolution(0) || p_in_c0.y < 0 || p_in_c0.y >= cam0.resolution(1) ) { @@ -1509,7 +1507,7 @@ void MsckfVio::PhotometricFeatureJacobian( H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; - //cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; + Eigen::MatrixXd xres = H_x.colPivHouseholderQr().solve(r); if(PRINTIMAGES) { @@ -1556,6 +1554,13 @@ void MsckfVio::PhotometricFeatureJacobian( << "# columns: " << 1 << "\n" << r_i << endl; + + myfile << "# name: xres\n" + << "# type: matrix\n" + << "# rows: " << xres.rows() << "\n" + << "# columns: " << xres.cols() << "\n" + << xres << endl; + myfile.close(); cout << "---------- LOGGED -------- " << endl; cvWaitKey(0); @@ -1693,9 +1698,10 @@ void MsckfVio::featureJacobian( FullPivLU lu(H_fj.transpose()); MatrixXd A = lu.kernel(); - H_x = A.transpose() * H_xj; - r = A.transpose() * r_j; + H_x = A.transpose() * H_xj; + r = A.transpose() * r_j; + Eigen::MatrixXd xres = H_x.colPivHouseholderQr().solve(r); // stop playing bagfile if printing images if(PRINTIMAGES) { @@ -1715,13 +1721,19 @@ void MsckfVio::featureJacobian( << "# columns: " << H_fj.cols() << "\n" << H_fj << endl; - myfile << "# name: r\n" << "# type: matrix\n" << "# rows: " << r_j.rows() << "\n" << "# columns: " << 1 << "\n" << r_j << endl; + + myfile << "# name: xres\n" + << "# type: matrix\n" + << "# rows: " << xres.rows() << "\n" + << "# columns: " << 1 << "\n" + << xres << endl; + myfile.close(); cout << "---------- LOGGED ORG -------- " << endl; } @@ -2002,7 +2014,7 @@ void MsckfVio::removeLostFeatures() { } if(!feature.is_anchored) { - if(!feature.initializeAnchorUndistort(cam0, N)) + if(!feature.initializeAnchor(cam0, N)) { invalid_feature_ids.push_back(feature.id); continue; @@ -2049,20 +2061,20 @@ void MsckfVio::removeLostFeatures() { MatrixXd pH_xj; VectorXd pr_j; - PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); + //PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); } - + /* if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - + */ // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. //if (stack_cntr > 1500) break; @@ -2075,7 +2087,7 @@ void MsckfVio::removeLostFeatures() { // Perform the measurement update step. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); + //photometricMeasurementUpdate(pH_x, pr); // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) @@ -2168,7 +2180,7 @@ void MsckfVio::pruneLastCamStateBuffer() } if(!feature.is_anchored) { - if(!feature.initializeAnchorUndistort(cam0, N)) + if(!feature.initializeAnchor(cam0, N)) { feature.observations.erase(rm_cam_state_id); continue; @@ -2205,7 +2217,7 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + //PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { @@ -2214,11 +2226,13 @@ void MsckfVio::pruneLastCamStateBuffer() stack_cntr += H_xj.rows(); pruned_cntr++; } + /* if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } + */ for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2231,7 +2245,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Perform measurement update. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); + //photometricMeasurementUpdate(pH_x, pr); //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), @@ -2320,7 +2334,7 @@ void MsckfVio::pruneCamStateBuffer() { } if(!feature.is_anchored) { - if(!feature.initializeAnchorUndistort(cam0, N)) + if(!feature.initializeAnchor(cam0, N)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); @@ -2358,7 +2372,7 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + //PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { @@ -2366,13 +2380,13 @@ void MsckfVio::pruneCamStateBuffer() { r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); } - + /* if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - + */ for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2385,7 +2399,7 @@ void MsckfVio::pruneCamStateBuffer() { // Perform measurement update. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); + //photometricMeasurementUpdate(pH_x, pr); //reduction for (const auto& cam_id : rm_cam_state_ids) { From 5451c2d097696fb0502ab3a76fc6c4cde2a0fadb Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 16:04:29 +0200 Subject: [PATCH 61/86] uncommented stuff --- launch/msckf_vio_tum.launch | 4 ++-- src/msckf_vio.cpp | 38 +++++++++++++++++++------------------ 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 1c2c689..1096e37 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -22,10 +22,10 @@ - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 9a31f42..d6f8326 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -554,15 +554,19 @@ void MsckfVio::manageMovingWindow( // undistort Images + + const cv::Matx33d K(cam0.intrinsics[0], 0.0, cam0.intrinsics[2], + 0.0, cam0.intrinsics[1], cam0.intrinsics[3], + 0.0, 0.0, 1.0); - //cv::Mat undistortedCam0; - //cv::Mat undistortedCam1; + cv::Mat undistortedCam0; + cv::Mat undistortedCam1; - //image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); - //image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - //cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone(); - //cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone(); + cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone(); + cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone(); //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) while(cam0.moving_window.size() > 100) @@ -2061,20 +2065,20 @@ void MsckfVio::removeLostFeatures() { MatrixXd pH_xj; VectorXd pr_j; - //PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); + PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); } - /* + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - */ + // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. //if (stack_cntr > 1500) break; @@ -2087,7 +2091,7 @@ void MsckfVio::removeLostFeatures() { // Perform the measurement update step. measurementUpdate(H_x, r); - //photometricMeasurementUpdate(pH_x, pr); + photometricMeasurementUpdate(pH_x, pr); // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) @@ -2217,7 +2221,7 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - //PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { @@ -2226,13 +2230,11 @@ void MsckfVio::pruneLastCamStateBuffer() stack_cntr += H_xj.rows(); pruned_cntr++; } - /* if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - */ for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2245,7 +2247,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Perform measurement update. measurementUpdate(H_x, r); - //photometricMeasurementUpdate(pH_x, pr); + photometricMeasurementUpdate(pH_x, pr); //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), @@ -2372,7 +2374,7 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - //PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { @@ -2380,13 +2382,13 @@ void MsckfVio::pruneCamStateBuffer() { r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); } - /* + if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - */ + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2399,7 +2401,7 @@ void MsckfVio::pruneCamStateBuffer() { // Perform measurement update. measurementUpdate(H_x, r); - //photometricMeasurementUpdate(pH_x, pr); + photometricMeasurementUpdate(pH_x, pr); //reduction for (const auto& cam_id : rm_cam_state_ids) { From 010db87e4b1b9f69d38e19a1b1627a20cb2d0e79 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 16:28:45 +0200 Subject: [PATCH 62/86] tinytum works completely, image_handler equidistant distort/undistort work --- config/camchain-imucam-tum-scaled.yaml | 18 +++++++++--------- include/msckf_vio/image_handler.h | 10 ++++++++++ launch/image_processor_tinytum.launch | 4 ++-- launch/msckf_vio_tinytum.launch | 13 +++++-------- launch/msckf_vio_tum.launch | 6 +++--- 5 files changed, 29 insertions(+), 22 deletions(-) diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index 529828c..4facacc 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -5,11 +5,11 @@ cam0: -0.029453096117288798, -0.9989836729399656, 0.034119442089241274, -0.0697294754693238, 0.0, 0.0, 0.0, 1.0] camera_model: pinhole - distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, - -0.001662284667857643] + distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, + 0.00020293673591811182] distortion_model: equidistant - intrinsics: [95.2026071784, 95.2029854486, 127.573663262, 128.582615763] - resolution: [256, 256] + intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] + resolution: [512, 512] rostopic: /cam0/image_raw cam1: T_cam_imu: @@ -23,14 +23,14 @@ cam1: 0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572, 0.0, 0.0, 0.0, 1.0] camera_model: pinhole - distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, - -0.002347858896562788] + distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, + 0.0003299517423931039] distortion_model: equidistant - intrinsics: [94.8217471066, 94.8164593555, 126.391667581, 127.571024044] - resolution: [256, 256] + intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] + resolution: [512, 512] rostopic: /cam1/image_raw T_imu_body: [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, - 0.0000, 0.0000, 0.0000, 1.0000] \ No newline at end of file + 0.0000, 0.0000, 0.0000, 1.0000] diff --git a/include/msckf_vio/image_handler.h b/include/msckf_vio/image_handler.h index d664be6..3cf2ed2 100644 --- a/include/msckf_vio/image_handler.h +++ b/include/msckf_vio/image_handler.h @@ -16,6 +16,16 @@ namespace msckf_vio { */ namespace image_handler { +cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics); +cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics); + +void undistortImage( + cv::InputArray src, + cv::OutputArray dst, + const std::string& distortion_model, + const cv::Vec4d& intrinsics, + const cv::Vec4d& distortion_coeffs); + void undistortPoints( const std::vector& pts_in, const cv::Vec4d& intrinsics, diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 6fe2855..53dd99a 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -25,8 +25,8 @@ - - + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 94deb7c..06d4c7f 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -6,7 +6,7 @@ default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/> - + @@ -18,11 +18,11 @@ output="screen"> - + - + @@ -64,16 +64,13 @@ - - + + - - - diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 390ab03..0240de3 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,11 +18,11 @@ output="screen"> - + - + @@ -72,6 +72,6 @@ - + From 655416a49024681e280e1b4ff952d8a59b60c458 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 16:38:02 +0200 Subject: [PATCH 63/86] added image_handler undistorted image --- src/image_handler.cpp | 143 +++++++++++++++++++++++++++++++++++++--- src/image_processor.cpp | 6 ++ 2 files changed, 141 insertions(+), 8 deletions(-) diff --git a/src/image_handler.cpp b/src/image_handler.cpp index cb98426..8664922 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -14,6 +14,34 @@ namespace msckf_vio { namespace image_handler { +cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics) +{ + return cv::Point2f(p_in.x * intrinsics[0] + intrinsics[2], p_in.y * intrinsics[1] + intrinsics[3]); +} + +cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics) +{ + return cv::Point2f((p_in.x - intrinsics[2])/intrinsics[0], (p_in.y - intrinsics[3])/intrinsics[1]); +} + +void undistortImage( + cv::InputArray src, + cv::OutputArray dst, + const std::string& distortion_model, + const cv::Vec4d& intrinsics, + const cv::Vec4d& distortion_coeffs) +{ + + const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2], + 0.0, intrinsics[1], intrinsics[3], + 0.0, 0.0, 1.0); + + if (distortion_model == "pre-equidistant") + cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K); + else if (distortion_model == "equidistant") + src.copyTo(dst); +} + void undistortPoint( const cv::Point2f& pt_in, const cv::Vec4d& intrinsics, @@ -42,10 +70,36 @@ void undistortPoint( if (distortion_model == "radtan") { cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else if (distortion_model == "equidistant") { + } + // equidistant + else if (distortion_model == "equidistant") { cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else { + } + // fov + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + float omega = distortion_coeffs[0]; + float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float ru = tan(rd * omega)/(2 * tan(omega / 2)); + + cv::Point2f newPoint( + ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd), + ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd)); + + pts_out.push_back(newPoint); + } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + } + else { ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", distortion_model.c_str()); cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, @@ -79,10 +133,35 @@ void undistortPoints( if (distortion_model == "radtan") { cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else if (distortion_model == "equidistant") { + } + else if (distortion_model == "equidistant") { cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs, rectification_matrix, K_new); - } else { + } + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + float omega = distortion_coeffs[0]; + float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float ru = tan(rd * omega)/(2 * tan(omega / 2)); + + cv::Point2f newPoint( + ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd), + ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd)); + + pts_out.push_back(newPoint); + } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + + } + else { ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", distortion_model.c_str()); cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, @@ -110,7 +189,31 @@ std::vector distortPoints( distortion_coeffs, pts_out); } else if (distortion_model == "equidistant") { cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs); - } else { + } + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + // based on 'straight lines have to be straight' + float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float omega = distortion_coeffs[0]; + float rd = 1 / (omega)*atan(2*ru*tan(omega / 2)); + + cv::Point2f newPoint( + pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2], + pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]); + + pts_out.push_back(newPoint); + } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + temp_pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + } + else { ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", distortion_model.c_str()); std::vector homogenous_pts; @@ -143,7 +246,31 @@ cv::Point2f distortPoint( distortion_coeffs, pts_out); } else if (distortion_model == "equidistant") { cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs); - } else { + } + else if (distortion_model == "fov") { + for(int i = 0; i < pts_in.size(); i++) + { + // based on 'straight lines have to be straight' + float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y); + float omega = distortion_coeffs[0]; + float rd = 1 / (omega)*atan(2*ru*tan(omega / 2)); + + cv::Point2f newPoint( + pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2], + pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]); + + pts_out.push_back(newPoint); + } + } + else if (distortion_model == "pre-equidistant") + { + std::vector temp_pts_out; + for(int i = 0; i < pts_in.size(); i++) + pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics)); + + pts_out = temp_pts_out; + } + else { ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", distortion_model.c_str()); std::vector homogenous_pts; @@ -155,5 +282,5 @@ cv::Point2f distortPoint( return pts_out[0]; } - } // namespace image_handler -} // namespace msckf_vio \ No newline at end of file + } // namespace image_handler +} // namespace msckf_vio diff --git a/src/image_processor.cpp b/src/image_processor.cpp index d35bb16..0e690b7 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -219,6 +219,12 @@ void ImageProcessor::stereoCallback( cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img, sensor_msgs::image_encodings::MONO8); + + image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + + + // Build the image pyramids once since they're used at multiple places createImagePyramids(); From af1820a238a31f44a3af957afd41d55cc6e115cd Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 17:27:47 +0200 Subject: [PATCH 64/86] added check if pre-undistorted, works for still distorted --- include/msckf_vio/feature.hpp | 75 ++++++++++++++++++++++------------- src/msckf_vio.cpp | 10 +++++ 2 files changed, 57 insertions(+), 28 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 466d1e9..d558460 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -969,7 +969,7 @@ cv::Point2f Feature::projectPositionToCamera( Eigen::Isometry3d T_c0_w; cv::Point2f out_p; - + cv::Point2f my_p; // transfrom position to camera frame Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Eigen::Vector3d& t_c0_w = cam_state.position; @@ -980,11 +980,15 @@ cv::Point2f Feature::projectPositionToCamera( // if(cam_state_id == observations.begin()->first) //printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y); - cv::Point2f my_p = image_handler::distortPoint(out_p, + if (cam.distortion_model.substr(0,3) == "pre-") + my_p = cv::Point2f(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]); + else + my_p = image_handler::distortPoint(out_p, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); + // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); @@ -1008,8 +1012,6 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal bool Feature::initializeAnchor(const CameraCalibration& cam, int N) { - - //initialize patch Size int n = (int)(N-1)/2; @@ -1037,51 +1039,68 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2]; auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3]; - //testing - undist_anchor_center_pos = cv::Point2f(u,v); - //for NxN patch pixels around feature - int count = 0; + // check if image has been pre-undistorted + if(cam.distortion_model.substr(0,3) == "pre-") + { + std::cout << "is a pre" << std::endl; + //project onto pixel plane + undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]); - // get feature in undistorted pixel space - // this only reverts from 'pure' space into undistorted pixel space using camera matrix - cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs); - // create vector of patch in pixel plane - for(double u_run = -n; u_run <= n; u_run++) - for(double v_run = -n; v_run <= n; v_run++) - anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); + // create vector of patch in pixel plane + for(double u_run = -n; u_run <= n; u_run++) + for(double v_run = -n; v_run <= n; v_run++) + anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run)); + + //project back into u,v + for(int i = 0; i < N*N; i++) + anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1])); + } + + else + { + // get feature in undistorted pixel space + // this only reverts from 'pure' space into undistorted pixel space using camera matrix + cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); + // create vector of patch in pixel plane + for(double u_run = -n; u_run <= n; u_run++) + for(double v_run = -n; v_run <= n; v_run++) + anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); - //create undistorted pure points - image_handler::undistortPoints(anchorPatch_real, - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs, - anchorPatch_ideal); + //create undistorted pure points + image_handler::undistortPoints(anchorPatch_real, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs, + anchorPatch_ideal); + } // save anchor position for later visualisaztion anchor_center_pos = anchorPatch_real[(N*N-1)/2]; - + // save true pixel Patch position for(auto point : anchorPatch_real) - if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) + if(point.x - n < 0 || point.x + n >= cam.resolution(0)-1 || point.y - n < 0 || point.y + n >= cam.resolution(1)-1) return false; - + for(auto point : anchorPatch_real) anchorPatch.push_back(PixelIrradiance(point, anchorImage)); - + // project patch pixel to 3D space in camera coordinate system for(auto point : anchorPatch_ideal) anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam)); is_anchored = true; + return true; } + bool Feature::initializeRho(const CamStateServer& cam_states) { // Organize camera poses and feature observations properly. diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 323e791..e6ce390 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -546,10 +546,20 @@ void MsckfVio::manageMovingWindow( cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img, sensor_msgs::image_encodings::MONO8); + image_handler::undistortImage(cam0_img_ptr->image, cam0_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + + + // save image information into moving window cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone(); cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); + + + + + //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) while(cam0.moving_window.size() > 100) { From bfb2a551a7cfabaa34637b00005de814a3e94ed2 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 17:54:16 +0200 Subject: [PATCH 65/86] fixed timedelay for publishing structure when bigger time delays than queue allows for --- config/camchain-imucam-tum-scaled.yaml | 4 ++-- src/image_processor.cpp | 12 ++++++------ src/msckf_vio.cpp | 15 +++++++++++---- 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index 4facacc..f5ea7e8 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] resolution: [512, 512] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] resolution: [512, 512] rostopic: /cam1/image_raw diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 0e690b7..168eebc 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -219,18 +219,18 @@ void ImageProcessor::stereoCallback( cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img, sensor_msgs::image_encodings::MONO8); + ros::Time start_time = ros::Time::now(); image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - - - + ROS_INFO("Publishing: %f", + (ros::Time::now()-start_time).toSec()); // Build the image pyramids once since they're used at multiple places createImagePyramids(); // Detect features in the first frame. if (is_first_img) { - ros::Time start_time = ros::Time::now(); + start_time = ros::Time::now(); initializeFirstFrame(); //ROS_INFO("Detection time: %f", // (ros::Time::now()-start_time).toSec()); @@ -243,7 +243,7 @@ void ImageProcessor::stereoCallback( // (ros::Time::now()-start_time).toSec()); } else { // Track the feature in the previous image. - ros::Time start_time = ros::Time::now(); + start_time = ros::Time::now(); trackFeatures(); //ROS_INFO("Tracking time: %f", // (ros::Time::now()-start_time).toSec()); @@ -273,7 +273,7 @@ void ImageProcessor::stereoCallback( // (ros::Time::now()-start_time).toSec()); // Publish features in the current image. - ros::Time start_time = ros::Time::now(); + start_time = ros::Time::now(); publish(); //ROS_INFO("Publishing: %f", // (ros::Time::now()-start_time).toSec()); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index e6ce390..10b9cd7 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -405,12 +405,19 @@ void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - // Return if the gravity vector has not been set. - if (!is_gravity_set) return; - - // stop playing bagfile if printing images + cout << "hello" << endl; + // stop playing bagfile if printing images if(STREAMPAUSE) nh.setParam("/play_bag", false); + // Return if the gravity vector has not been set. + if (!is_gravity_set) + { + if(STREAMPAUSE) + nh.setParam("/play_bag", true); + return; + } + + // Start the system if the first image is received. // The frame where the first image is received will be From 9f528c1ea13e00d11c30352e5ce6683a7219838a Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 18:25:43 +0200 Subject: [PATCH 66/86] added support for euroc dataset --- launch/msckf_vio_euroc.launch | 11 +++++++++++ src/image_handler.cpp | 12 ++++++++---- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 896feff..5f6de1e 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -17,6 +17,17 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> + + + + + + + + + + + diff --git a/src/image_handler.cpp b/src/image_handler.cpp index 8664922..f174fb7 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -40,6 +40,10 @@ void undistortImage( cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K); else if (distortion_model == "equidistant") src.copyTo(dst); + else if (distortion_model == "pre-radtan") + cv::undistort(src, dst, K, distortion_coeffs, K); + else if (distortion_model == "radtan") + src.copyTo(dst); } void undistortPoint( @@ -91,7 +95,7 @@ void undistortPoint( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) @@ -152,7 +156,7 @@ void undistortPoints( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) @@ -205,7 +209,7 @@ std::vector distortPoints( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) @@ -262,7 +266,7 @@ cv::Point2f distortPoint( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) From 715ca6a6b46a273727c76f52a9ab71775cebadb7 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 19:22:08 +0200 Subject: [PATCH 67/86] added two filter, not working yet - compare with htest --- include/msckf_vio/msckf_vio.h | 14 +- launch/msckf_vio_euroc.launch | 5 +- launch/msckf_vio_tinytum.launch | 4 +- src/msckf_vio.cpp | 408 ++++++++++++++++++++++++++++++-- 4 files changed, 411 insertions(+), 20 deletions(-) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 9e9975d..49af3cd 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -207,6 +207,11 @@ class MsckfVio { Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + void twodotMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + void PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, @@ -214,6 +219,11 @@ class MsckfVio { Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + void twodotFeatureJacobian( + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + void PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, @@ -222,6 +232,8 @@ class MsckfVio { void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); void measurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); + void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); + bool gatingTest(const Eigen::MatrixXd& H, const Eigen::VectorXd&r, const int& dof); void removeLostFeatures(); @@ -234,7 +246,7 @@ class MsckfVio { void onlineReset(); // Photometry flag - bool PHOTOMETRIC; + int FILTER; // debug flag bool STREAMPAUSE; diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 5f6de1e..b818735 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -17,9 +17,8 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - - + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 06d4c7f..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -17,8 +17,8 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 10b9cd7..30ee2e2 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -67,7 +67,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh): bool MsckfVio::loadParameters() { //Photometry Flag - nh.param("PHOTOMETRIC", PHOTOMETRIC, false); + nh.param("FILTER", FILTER, 0); nh.param("PrintImages", PRINTIMAGES, false); nh.param("StreamPause", STREAMPAUSE, false); nh.param("GroundTruth", GROUNDTRUTH, false); @@ -405,8 +405,7 @@ void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - cout << "hello" << endl; - // stop playing bagfile if printing images + // stop playing bagfile if printing images if(STREAMPAUSE) nh.setParam("/play_bag", false); // Return if the gravity vector has not been set. @@ -455,7 +454,7 @@ void MsckfVio::imageCallback( double imu_processing_time = ( ros::Time::now()-start_time).toSec(); - //cout << "1" << endl; + cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); @@ -464,7 +463,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "2" << endl; + cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -473,7 +472,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "3" << endl; + cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); @@ -481,20 +480,20 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "4" << endl; + cout << "4" << endl; // Perform measurement update if necessary. start_time = ros::Time::now(); removeLostFeatures(); double remove_lost_features_time = ( ros::Time::now()-start_time).toSec(); - //cout << "5" << endl; + cout << "5" << endl; start_time = ros::Time::now(); pruneLastCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); - //cout << "6" << endl; + cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -1243,6 +1242,223 @@ void MsckfVio::addFeatureObservations( return; } +void MsckfVio::twodotMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) +{ + + // Prepare all the required data. + const CAMState& cam_state = state_server.cam_states[cam_state_id]; + const Feature& feature = map_server[feature_id]; + + // Cam0 pose. + Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); + const Vector3d& t_c0_w = cam_state.position; + + //photometric observation + std::vector photo_z; + + // individual Jacobians + Matrix dh_dCpij = Matrix::Zero(); + Matrix dh_dGpij = Matrix::Zero(); + Matrix dh_dXplj = Matrix::Zero(); + Matrix dGpj_drhoj = Matrix::Zero(); + Matrix dGpj_XpAj = Matrix::Zero(); + + Matrix dCpij_dGpij = Matrix::Zero(); + Matrix dCpij_dCGtheta = Matrix::Zero(); + Matrix dCpij_dGpC = Matrix::Zero(); + + // one line of the NxN Jacobians + Eigen::Matrix H_rho; + Eigen::Matrix H_plj; + Eigen::Matrix H_pAj; + + auto frame = cam0.moving_window.find(cam_state_id)->second.image; + + int count = 0; + + auto point = feature.anchorPatch_3d[0]; + + Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + // add jacobian + + //dh / d{}^Cp_{ij} + dh_dCpij(0, 0) = 1 / p_c0(2); + dh_dCpij(1, 1) = 1 / p_c0(2); + dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); + dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); + + dCpij_dGpij = quaternionToRotation(cam_state.orientation); + + //orientation takes camera frame to world frame + dh_dGpij = dh_dCpij * dCpij_dGpij; + + //dh / d X_{pl} + dCpij_dCGtheta = skewSymmetric(p_c0); + dCpij_dGpC = -quaternionToRotation(cam_state.orientation); + dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta; + dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC; + + //d{}^Gp_P{ij} / \rho_i + double rho = feature.anchor_rho; + // Isometry T_anchor_w takes a vector in anchor frame to world frame + dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); + + + + // alternative derivation towards feature + Matrix3d dCpc0_dpg = R_w_c0; + dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() + * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), + feature.anchorPatch_ideal[count].y/(rho), + 1/(rho))); + dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); + + // Intermediate Jakobians + H_rho = dh_dGpij * dGpj_drhoj; // 2 x 1 + H_plj = dh_dXplj; // 2 x 6 + H_pAj = dh_dGpij * dGpj_XpAj; // 2 x 6 + + // calculate residual + + //observation + const Vector4d& total_z = feature.observations.find(cam_state_id)->second; + const Vector2d z = Vector2d(total_z[0], total_z[1]); + + VectorXd r_i = VectorXd::Zero(2); + + //calculate residual + + r_i[0] = z[0] - p_c0(0)/p_c0(2); + r_i[1] = z[1] - p_c0(1)/p_c0(2); + + MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7); + + // set anchor Jakobi + // get position of anchor in cam states + + auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); + int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); + H_xl.block(0, 21+cam_state_cntr_anchor*7, 2, 6) = H_pAj; + + // set frame Jakobi + //get position of current frame in cam states + auto cam_state_iter = state_server.cam_states.find(cam_state_id); + int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); + + // set jakobi of state + H_xl.block(0, 21+cam_state_cntr*7, 2, 6) = H_plj; + + H_x = H_xl; + H_y = H_rho; + r = r_i; + + //TODO make this more fluent as well + if(PRINTIMAGES) + { + std::stringstream ss; + ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; + feature.MarkerGeneration(marker_pub, state_server.cam_states); + //feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); + } + + return; +} + +void MsckfVio::twodotFeatureJacobian( + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + MatrixXd& H_x, VectorXd& r) +{ + + const auto& feature = map_server[feature_id]; + + + // Check how many camera states in the provided camera + // id camera has actually seen this feature. + vector valid_cam_state_ids(0); + for (const auto& cam_id : cam_state_ids) { + if (feature.observations.find(cam_id) == + feature.observations.end()) continue; + + if (feature.observations.find(cam_id) == + feature.observations.begin()) continue; + valid_cam_state_ids.push_back(cam_id); + } + + int jacobian_row_size = 0; + jacobian_row_size = 2 * valid_cam_state_ids.size(); + + MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, + 21+state_server.cam_states.size()*7); + MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); + VectorXd r_i = VectorXd::Zero(jacobian_row_size); + int stack_cntr = 0; + + for (const auto& cam_id : valid_cam_state_ids) { + + MatrixXd H_xl; + MatrixXd H_yl; + Eigen::VectorXd r_l = VectorXd::Zero(2); + + twodotMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); + auto cam_state_iter = state_server.cam_states.find(cam_id); + int cam_state_cntr = std::distance( + state_server.cam_states.begin(), cam_state_iter); + + // Stack the Jacobians. + H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl; + H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl; + r_i.segment(stack_cntr, 2) = r_l; + stack_cntr += 2; + } + + // Project the residual and Jacobians onto the nullspace + // of H_yj. + + // get Nullspace + FullPivLU lu(H_yi.transpose()); + MatrixXd A_null_space = lu.kernel(); + + H_x = A_null_space.transpose() * H_xi; + r = A_null_space.transpose() * r_i; + + if(PRINTIMAGES) + { + + ofstream myfile; + myfile.open("/home/raphael/dev/octave/log2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: Hx\n" + << "# type: matrix\n" + << "# rows: " << H_xi.rows() << "\n" + << "# columns: " << H_xi.cols() << "\n" + << H_xi << endl; + + myfile << "# name: Hy\n" + << "# type: matrix\n" + << "# rows: " << H_yi.rows() << "\n" + << "# columns: " << H_yi.cols() << "\n" + << H_yi << endl; + + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r_i.rows() << "\n" + << "# columns: " << 1 << "\n" + << r_i << endl; + + myfile.close(); + std::cout << "resume playback" << std::endl; + nh.setParam("/play_bag", true); + } + return; +} + + + void MsckfVio::PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, @@ -1431,6 +1647,7 @@ void MsckfVio::PhotometricMeasurementJacobian( return; } + void MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, @@ -1737,7 +1954,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. - if(PHOTOMETRIC) return; + if(FILTER != 0) return; const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1793,6 +2010,108 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { return; } +void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { + + if (H.rows() == 0 || r.rows() == 0) + return; + // Decompose the final Jacobian matrix to reduce computational + // complexity as in Equation (28), (29). + MatrixXd H_thin; + VectorXd r_thin; + + // QR decomposition to make stuff faster + if (H.rows() > H.cols()) { + // Convert H to a sparse matrix. + SparseMatrix H_sparse = H.sparseView(); + + // Perform QR decompostion on H_sparse. + SPQR > spqr_helper; + spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); + spqr_helper.compute(H_sparse); + + MatrixXd H_temp; + VectorXd r_temp; + (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp); + (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp); + + H_thin = H_temp.topRows(21+state_server.cam_states.size()*7); + r_thin = r_temp.head(21+state_server.cam_states.size()*7); + + } else { + H_thin = H; + r_thin = r; + } + + + // Compute the Kalman gain. + const MatrixXd& P = state_server.state_cov; + MatrixXd S = H_thin*P*H_thin.transpose() + + Feature::observation_noise*MatrixXd::Identity( + H_thin.rows(), H_thin.rows()); + //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); + MatrixXd K_transpose = S.ldlt().solve(H_thin*P); + MatrixXd K = K_transpose.transpose(); + // Compute the error of the state. + + VectorXd delta_x = K * r; + cout << "two rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; + cout << "two update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // Update the IMU state. + if (FILTER != 2) return; + + const VectorXd& delta_x_imu = delta_x.head<21>(); + + if (//delta_x_imu.segment<3>(0).norm() > 0.15 || + //delta_x_imu.segment<3>(3).norm() > 0.15 || + delta_x_imu.segment<3>(6).norm() > 0.5 || + //delta_x_imu.segment<3>(9).norm() > 0.5 || + delta_x_imu.segment<3>(12).norm() > 1.0) { + printf("delta velocity: %f\n", delta_x_imu.segment<3>(6).norm()); + printf("delta position: %f\n", delta_x_imu.segment<3>(12).norm()); + ROS_WARN("Update change is too large."); + //return; + } + + const Vector4d dq_imu = + smallAngleQuaternion(delta_x_imu.head<3>()); + state_server.imu_state.orientation = quaternionMultiplication( + dq_imu, state_server.imu_state.orientation); + state_server.imu_state.gyro_bias += delta_x_imu.segment<3>(3); + state_server.imu_state.velocity += delta_x_imu.segment<3>(6); + state_server.imu_state.acc_bias += delta_x_imu.segment<3>(9); + state_server.imu_state.position += delta_x_imu.segment<3>(12); + + const Vector4d dq_extrinsic = + smallAngleQuaternion(delta_x_imu.segment<3>(15)); + state_server.imu_state.R_imu_cam0 = quaternionToRotation( + dq_extrinsic) * state_server.imu_state.R_imu_cam0; + state_server.imu_state.t_cam0_imu += delta_x_imu.segment<3>(18); + + // Update the camera states. + auto cam_state_iter = state_server.cam_states.begin(); + for (int i = 0; i < state_server.cam_states.size(); + ++i, ++cam_state_iter) { + const VectorXd& delta_x_cam = delta_x.segment(21+i*7, 6); + const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>()); + cam_state_iter->second.orientation = quaternionMultiplication( + dq_cam, cam_state_iter->second.orientation); + cam_state_iter->second.position += delta_x_cam.tail<3>(); + cam_state_iter->second.illumination.frame_bias += delta_x(21+i*7+6); + } + + // Update state covariance. + MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin; + //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() + + // K*K.transpose()*Feature::observation_noise; + state_server.state_cov = I_KH*state_server.state_cov; + + // Fix the covariance to be symmetric + MatrixXd state_cov_fixed = (state_server.state_cov + + state_server.state_cov.transpose()) / 2.0; + state_server.state_cov = state_cov_fixed; + return; +} + void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { @@ -1841,7 +2160,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. - if (not PHOTOMETRIC) return; + if (FILTER != 1) return; const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1925,6 +2244,7 @@ void MsckfVio::removeLostFeatures() { // BTW, find the size the final Jacobian matrix and residual vector. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int twojacobian_row_size = 0; vector invalid_feature_ids(0); vector processed_feature_ids(0); @@ -1965,7 +2285,9 @@ void MsckfVio::removeLostFeatures() { } pjacobian_row_size += N*N*feature.observations.size(); + twojacobian_row_size += 2*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; + processed_feature_ids.push_back(feature.id); } @@ -1974,6 +2296,9 @@ void MsckfVio::removeLostFeatures() { // processed_feature_ids.size() << endl; //cout << "jacobian row #: " << jacobian_row_size << endl; + + cout << "sizing" << endl; + // Remove the features that do not have enough measurements. for (const auto& feature_id : invalid_feature_ids) map_server.erase(feature_id); @@ -1991,6 +2316,12 @@ void MsckfVio::removeLostFeatures() { VectorXd pr = VectorXd::Zero(pjacobian_row_size); int pstack_cntr = 0; + + MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, + 21+7*state_server.cam_states.size()); + VectorXd twor = VectorXd::Zero(twojacobian_row_size); + int twostack_cntr = 0; + // Process the features which lose track. for (const auto& feature_id : processed_feature_ids) { auto& feature = map_server[feature_id]; @@ -2003,10 +2334,18 @@ void MsckfVio::removeLostFeatures() { VectorXd r_j; MatrixXd pH_xj; VectorXd pr_j; + MatrixXd twoH_xj; + VectorXd twor_j; + + + cout << "measuring" << endl; PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - + twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); + + cout << "gating" << endl; + if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; @@ -2017,20 +2356,29 @@ void MsckfVio::removeLostFeatures() { pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + twor.segment(twostack_cntr, twor_j.rows()) = twor_j; + twostack_cntr += twoH_xj.rows(); + } // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. //if (stack_cntr > 1500) break; } + cout << "resizing" << endl; H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); // Perform the measurement update step. measurementUpdate(H_x, r); photometricMeasurementUpdate(pH_x, pr); + twoMeasurementUpdate(twoH_x, twor); // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) @@ -2094,6 +2442,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Set the size of the Jacobian matrix. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int twojacobian_row_size = 0; //initialize all the features which are going to be removed @@ -2132,6 +2481,7 @@ void MsckfVio::pruneLastCamStateBuffer() pjacobian_row_size += N*N*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; + twojacobian_row_size += 2*feature.observations.size(); } @@ -2143,9 +2493,14 @@ void MsckfVio::pruneLastCamStateBuffer() VectorXd pr_j; MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size()); VectorXd pr = VectorXd::Zero(pjacobian_row_size); + MatrixXd twoH_xj; + VectorXd twor_j; + MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); + VectorXd twor = VectorXd::Zero(twojacobian_row_size); int stack_cntr = 0; int pruned_cntr = 0; int pstack_cntr = 0; + int twostack_cntr = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2162,6 +2517,7 @@ void MsckfVio::pruneLastCamStateBuffer() PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; @@ -2174,6 +2530,11 @@ void MsckfVio::pruneLastCamStateBuffer() pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + twor.segment(twostack_cntr, twor_j.rows()) = twor_j; + twostack_cntr += twoH_xj.rows(); + } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2184,12 +2545,16 @@ void MsckfVio::pruneLastCamStateBuffer() pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); + // Perform measurement update. + + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); // Perform measurement update. measurementUpdate(H_x, r); photometricMeasurementUpdate(pH_x, pr); - + twoMeasurementUpdate(twoH_x, twor); //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -2241,6 +2606,7 @@ void MsckfVio::pruneCamStateBuffer() { // Find the size of the Jacobian matrix. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int twojacobian_row_size = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2285,6 +2651,7 @@ void MsckfVio::pruneCamStateBuffer() { } } + twojacobian_row_size += 2*involved_cam_state_ids.size(); pjacobian_row_size += N*N*involved_cam_state_ids.size(); jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -2302,6 +2669,11 @@ void MsckfVio::pruneCamStateBuffer() { MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size()); VectorXd pr = VectorXd::Zero(pjacobian_row_size); int pstack_cntr = 0; + MatrixXd twoH_xj; + VectorXd twor_j; + MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); + VectorXd twor = VectorXd::Zero(twojacobian_row_size); + int twostack_cntr = 0; for (auto& item : map_server) { auto& feature = item.second; // Check how many camera states to be removed are associated @@ -2317,6 +2689,7 @@ void MsckfVio::pruneCamStateBuffer() { PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; @@ -2329,6 +2702,11 @@ void MsckfVio::pruneCamStateBuffer() { pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + twor.segment(twostack_cntr, twor_j.rows()) = twor_j; + twostack_cntr += twoH_xj.rows(); + } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); @@ -2339,11 +2717,13 @@ void MsckfVio::pruneCamStateBuffer() { r.conservativeResize(stack_cntr); pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); // Perform measurement update. measurementUpdate(H_x, r); photometricMeasurementUpdate(pH_x, pr); - + twoMeasurementUpdate(twoH_x, twor); //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), From 3ae7bdb13af5a1adf283105bf8b1804ed467ffe3 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 28 Jun 2019 10:14:32 +0200 Subject: [PATCH 68/86] upated two calculation, still not working --- launch/msckf_vio_tum.launch | 4 ++-- src/msckf_vio.cpp | 5 ++++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 0240de3..b3a66e5 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -17,8 +17,8 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 30ee2e2..d70591a 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -405,7 +405,7 @@ void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - // stop playing bagfile if printing images + // stop playing bagfile if printing images if(STREAMPAUSE) nh.setParam("/play_bag", false); // Return if the gravity vector has not been set. @@ -1454,6 +1454,7 @@ void MsckfVio::twodotFeatureJacobian( std::cout << "resume playback" << std::endl; nh.setParam("/play_bag", true); } + return; } @@ -2341,7 +2342,9 @@ void MsckfVio::removeLostFeatures() { cout << "measuring" << endl; PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); + cout << "norm" << endl; featureJacobian(feature.id, cam_state_ids, H_xj, r_j); + cout << "two" << endl; twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); cout << "gating" << endl; From 53e2a5d5245b278c56b899fc96b9fbdd14678cd1 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 28 Jun 2019 10:52:11 +0200 Subject: [PATCH 69/86] added ocvis output for multiple filter --- config/camchain-imucam-tum.yaml | 4 +- include/msckf_vio/feature.hpp | 8 +--- launch/msckf_vio_tum.launch | 4 +- src/msckf_vio.cpp | 81 +++++++++++++++++++++++++-------- 4 files changed, 68 insertions(+), 29 deletions(-) diff --git a/config/camchain-imucam-tum.yaml b/config/camchain-imucam-tum.yaml index 7edbd87..141035e 100644 --- a/config/camchain-imucam-tum.yaml +++ b/config/camchain-imucam-tum.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, -0.001662284667857643] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506] resolution: [1024, 1024] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, -0.002347858896562788] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407] resolution: [1024, 1024] rostopic: /cam1/image_raw diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index d558460..435f22f 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -220,9 +220,7 @@ bool VisualizeKernel( const StateIDType& cam_state_id, CameraCalibration& cam0, const Eigen::VectorXd& photo_r, - std::stringstream& ss, - cv::Point2f gradientVector, - cv::Point2f residualVector) const; + std::stringstream& ss) const; /* * @brief AnchorPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em @@ -720,9 +718,7 @@ bool Feature::VisualizePatch( const StateIDType& cam_state_id, CameraCalibration& cam0, const Eigen::VectorXd& photo_r, - std::stringstream& ss, - cv::Point2f gradientVector, - cv::Point2f residualVector) const + std::stringstream& ss) const { double rescale = 1; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index b3a66e5..e8294ef 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,11 +18,11 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index d70591a..ff4c24b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1528,13 +1528,6 @@ void MsckfVio::PhotometricMeasurementJacobian( int count = 0; double dx, dy; - // gradient visualization parameters - cv::Point2f gradientVector(0,0); - - // residual change visualization - cv::Point2f residualVector(0,0); - double res_sum = 0; - for (auto point : feature.anchorPatch_3d) { //cout << "____feature-measurement_____\n" << endl; @@ -1633,7 +1626,6 @@ void MsckfVio::PhotometricMeasurementJacobian( H_x = H_xl; H_y = H_yl; - //TODO make this more fluent as well r = r_photo; std::stringstream ss; @@ -1641,7 +1633,7 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); + feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } @@ -1747,13 +1739,18 @@ void MsckfVio::PhotometricFeatureJacobian( << "# columns: " << H_yi.cols() << "\n" << H_yi << endl; - myfile << "# name: r\n" << "# type: matrix\n" << "# rows: " << r_i.rows() << "\n" << "# columns: " << 1 << "\n" << r_i << endl; + myfile << "# name: r\n" + << "# type: A\n" + << "# rows: " << A_null_space.rows() << "\n" + << "# columns: " << A_null_space.cols() << "\n" + << A_null_space << endl; + myfile.close(); cout << "---------- LOGGED -------- " << endl; cvWaitKey(0); @@ -1902,7 +1899,34 @@ void MsckfVio::featureJacobian( myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; myfile.close(); - cout << "---------- LOGGED -------- " << endl; + + myfile.open("/home/raphael/dev/octave/org2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: Hx\n" + << "# type: matrix\n" + << "# rows: " << H_xj.rows() << "\n" + << "# columns: " << H_xj.cols() << "\n" + << H_xj << endl; + + myfile << "# name: Hy\n" + << "# type: matrix\n" + << "# rows: " << H_fj.rows() << "\n" + << "# columns: " << H_fj.cols() << "\n" + << H_fj << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r_j.rows() << "\n" + << "# columns: " << 1 << "\n" + << r_j << endl; + + myfile << "# name: r\n" + << "# type: A\n" + << "# rows: " << A.rows() << "\n" + << "# columns: " << A.cols() << "\n" + << A << endl; + + myfile.close(); } return; @@ -2019,8 +2043,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; - - // QR decomposition to make stuff faster +/* if (H.rows() > H.cols()) { // Convert H to a sparse matrix. SparseMatrix H_sparse = H.sparseView(); @@ -2038,10 +2061,17 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { H_thin = H_temp.topRows(21+state_server.cam_states.size()*7); r_thin = r_temp.head(21+state_server.cam_states.size()*7); - } else { + //HouseholderQR qr_helper(H); + //MatrixXd Q = qr_helper.householderQ(); + //MatrixXd Q1 = Q.leftCols(21+state_server.cam_states.size()*6); + + //H_thin = Q1.transpose() * H; + //r_thin = Q1.transpose() * r; + } else {*/ H_thin = H; r_thin = r; - } + //} + // Compute the Kalman gain. @@ -2518,10 +2548,15 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); + cout << "measuring" << endl; + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + cout << "norm" << endl; + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + cout << "two" << endl; + twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); + + cout << "gating" << endl; if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; @@ -2689,7 +2724,15 @@ void MsckfVio::pruneCamStateBuffer() { } if (involved_cam_state_ids.size() == 0) continue; - + cout << "measuring" << endl; + + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + cout << "norm" << endl; + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + cout << "two" << endl; + twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); + + cout << "gating" << endl; PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); From 7b7e9662174370b83dc7c327f7e4b6f44f23ce3f Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 28 Jun 2019 12:13:02 +0200 Subject: [PATCH 70/86] added ground truth visualization --- include/msckf_vio/msckf_vio.h | 1 + launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 42 ++++++++++++++++++++++++++--------- 3 files changed, 33 insertions(+), 12 deletions(-) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 49af3cd..f8e7d62 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -328,6 +328,7 @@ class MsckfVio { // Subscribers and publishers ros::Subscriber imu_sub; ros::Subscriber truth_sub; + ros::Publisher truth_odom_pub; ros::Publisher odom_pub; ros::Publisher marker_pub; ros::Publisher feature_pub; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index e8294ef..b7e9365 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -22,7 +22,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index ff4c24b..4dacfce 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -261,6 +261,7 @@ bool MsckfVio::createRosIO() { nh.setParam("/play_bag", true); odom_pub = nh.advertise("odom", 10); + truth_odom_pub = nh.advertise("true_odom", 10); feature_pub = nh.advertise( "feature_point_cloud", 10); marker_pub = nh.advertise("/visualization_marker_array", 10); @@ -351,11 +352,6 @@ void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){ void MsckfVio::truthCallback(const geometry_msgs::TransformStampedPtr& msg){ static bool first_truth_odom_msg = true; - // errorstate - /*if(not ErrorState) - return; - */ - // If this is the first mocap odometry messsage, set // the initial frame. if (first_truth_odom_msg) { @@ -1429,7 +1425,7 @@ void MsckfVio::twodotFeatureJacobian( { ofstream myfile; - myfile.open("/home/raphael/dev/octave/log2octave"); + myfile.open("/home/raphael/dev/octave/two2octave"); myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" << "# name: Hx\n" << "# type: matrix\n" @@ -1450,6 +1446,13 @@ void MsckfVio::twodotFeatureJacobian( << "# columns: " << 1 << "\n" << r_i << endl; + + myfile << "# name: A\n" + << "# type: matrix\n" + << "# rows: " << A_null_space.rows() << "\n" + << "# columns: " << 1 << "\n" + << A_null_space << endl; + myfile.close(); std::cout << "resume playback" << std::endl; nh.setParam("/play_bag", true); @@ -1633,7 +1636,7 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); + //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } @@ -1745,8 +1748,8 @@ void MsckfVio::PhotometricFeatureJacobian( << "# columns: " << 1 << "\n" << r_i << endl; - myfile << "# name: r\n" - << "# type: A\n" + myfile << "# name: A\n" + << "# type: matrix\n" << "# rows: " << A_null_space.rows() << "\n" << "# columns: " << A_null_space.cols() << "\n" << A_null_space << endl; @@ -1920,8 +1923,8 @@ void MsckfVio::featureJacobian( << "# columns: " << 1 << "\n" << r_j << endl; - myfile << "# name: r\n" - << "# type: A\n" + myfile << "# name: A\n" + << "# type: matrix\n" << "# rows: " << A.rows() << "\n" << "# columns: " << A.cols() << "\n" << A << endl; @@ -2915,6 +2918,23 @@ void MsckfVio::publish(const ros::Time& time) { T_i_c_tf, time, child_frame_id, "camera")); } + // Publish true odometry + // Ground truth odometry. + nav_msgs::Odometry truth_odom_msg; + truth_odom_msg.header.stamp = time; + truth_odom_msg.header.frame_id = fixed_frame_id; + truth_odom_msg.child_frame_id = child_frame_id+"_true"; + + Eigen::Isometry3d true_T_i_w = Eigen::Isometry3d::Identity(); + true_T_i_w.linear() = quaternionToRotation( + true_state_server.imu_state.orientation).transpose(); + true_T_i_w.translation() = true_state_server.imu_state.position; + tf::poseEigenToMsg(true_T_i_w, truth_odom_msg.pose.pose); + Eigen::Isometry3d true_T_b_w = IMUState::T_imu_body * true_T_i_w * + IMUState::T_imu_body.inverse(); + + tf::poseEigenToMsg(true_T_b_w, truth_odom_msg.pose.pose); + truth_odom_pub.publish(truth_odom_msg); // Publish the odometry nav_msgs::Odometry odom_msg; odom_msg.header.stamp = time; From d013a1b080cdbd4a0443b8d432f2ae2751f04a08 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 28 Jun 2019 17:47:47 +0200 Subject: [PATCH 71/86] added multiple couts for testing, not working --- include/msckf_vio/feature.hpp | 4 +- include/msckf_vio/msckf_vio.h | 9 +- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 250 ++++++++++++++++++++++++++-------- 4 files changed, 203 insertions(+), 62 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 435f22f..b818dc6 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -1024,8 +1024,8 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3); cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3); - xderImage/=8; - yderImage/=8; + xderImage/=8.; + yderImage/=8.; cv::convertScaleAbs(xderImage, abs_xderImage); cv::convertScaleAbs(yderImage, abs_yderImage); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index f8e7d62..995c5a3 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -212,7 +212,7 @@ class MsckfVio { const FeatureIDType& feature_id, Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); - void PhotometricMeasurementJacobian( + bool PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, Eigen::MatrixXd& H_x, @@ -224,7 +224,7 @@ class MsckfVio { const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); - void PhotometricFeatureJacobian( + bool PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); @@ -263,6 +263,11 @@ class MsckfVio { // Chi squared test table. static std::map chi_squared_test_table; + + // change in position + Eigen::Vector3d delta_position; + Eigen::Vector3d delta_orientation; + // State vector StateServer state_server; StateServer photometric_state_server; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index b7e9365..b3a66e5 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,7 +18,7 @@ output="screen"> - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 4dacfce..f9178c2 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -412,8 +412,6 @@ void MsckfVio::imageCallback( return; } - - // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -433,10 +431,6 @@ void MsckfVio::imageCallback( batchImuProcessing(feature_msg->header.stamp.toSec()); - // save true state in seperate state vector - - batchTruthProcessing(feature_msg->header.stamp.toSec()); - if(GROUNDTRUTH) { state_server.imu_state.position = true_state_server.imu_state.position; @@ -489,6 +483,10 @@ void MsckfVio::imageCallback( double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); + // save true state in seperate state vector and calculate relative error in change + batchTruthProcessing(feature_msg->header.stamp.toSec()); + + cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); @@ -808,6 +806,8 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { // Counter how many IMU msgs in the buffer are used. int used_truth_msg_cntr = 0; + const IMUState old_true_state = true_state_server.imu_state; + for (const auto& truth_msg : truth_msg_buffer) { double truth_time = truth_msg.header.stamp.toSec(); if (truth_time < true_state_server.imu_state.time) { @@ -839,6 +839,22 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { truth_msg_buffer.erase(truth_msg_buffer.begin(), truth_msg_buffer.begin()+used_truth_msg_cntr); + // calculate error in position + + // calculate error in change + + // calculate change + Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - old_true_state.position; + Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), old_true_state.orientation); + Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2); + Eigen::Vector3d error_delta_position = delta_true_position - delta_position; + Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation; + + double dx = (error_delta_position[0]/delta_true_position[0]); + double dy = (error_delta_position[1]/delta_true_position[1]); + double dz = (error_delta_position[2]/delta_true_position[2]); + cout << "relative pos error: " << sqrt(dx*dx + dy*dy + dz*dz) * 100 << "%" << endl; + } void MsckfVio::processTruthtoIMU(const double& time, @@ -1463,7 +1479,7 @@ void MsckfVio::twodotFeatureJacobian( -void MsckfVio::PhotometricMeasurementJacobian( +bool MsckfVio::PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) @@ -1531,6 +1547,7 @@ void MsckfVio::PhotometricMeasurementJacobian( int count = 0; double dx, dy; + cout << "patching" << endl; for (auto point : feature.anchorPatch_3d) { //cout << "____feature-measurement_____\n" << endl; @@ -1538,6 +1555,11 @@ void MsckfVio::PhotometricMeasurementJacobian( cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); + if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) + { + cout << "skipped" << endl; + return false; + } //add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); @@ -1597,8 +1619,9 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } + cout << "done" << endl; MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); + MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1); // set anchor Jakobi // get position of anchor in cam states @@ -1616,15 +1639,18 @@ void MsckfVio::PhotometricMeasurementJacobian( H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl; // set ones for irradiance bias - H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); + // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); // set irradiance error Block - H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N); + H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N); // TODO make this calculation more fluent - for(int i = 0; i< N*N; i++) + + /*for(int i = 0; i< N*N; i++) H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; - H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho; + */ + + H_yl.block(0, N*N, N*N, 1) = -H_rho; H_x = H_xl; H_y = H_yl; @@ -1636,15 +1662,16 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); + feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } - return; + cout << "returning" << endl; + return true; } -void MsckfVio::PhotometricFeatureJacobian( +bool MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) @@ -1669,7 +1696,7 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, 21+state_server.cam_states.size()*7); - MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1); + MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+1); VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1679,7 +1706,8 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd H_yl; Eigen::VectorXd r_l = VectorXd::Zero(N*N); - PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); + if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) + continue; auto cam_state_iter = state_server.cam_states.find(cam_id); int cam_state_cntr = std::distance( @@ -1691,7 +1719,11 @@ void MsckfVio::PhotometricFeatureJacobian( r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; } - + if(stack_cntr == 0) + { + cout << "skip feature" << endl; + return false; + } // Project the residual and Jacobians onto the nullspace // of H_yj. @@ -1754,12 +1786,20 @@ void MsckfVio::PhotometricFeatureJacobian( << "# columns: " << A_null_space.cols() << "\n" << A_null_space << endl; + myfile << "# name: P\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.rows() << "\n" + << state_server.state_cov << endl; + + myfile.close(); cout << "---------- LOGGED -------- " << endl; - cvWaitKey(0); } - return; + cout << "donefeature" << endl; + + return true; } void MsckfVio::measurementJacobian( @@ -1929,6 +1969,13 @@ void MsckfVio::featureJacobian( << "# columns: " << A.cols() << "\n" << A << endl; + myfile << "# name: P\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.rows() << "\n" + << state_server.state_cov << endl; + + myfile.close(); } @@ -1970,19 +2017,50 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; + MatrixXd S = H_thin*P*H_thin.transpose() + Feature::observation_noise*MatrixXd::Identity( H_thin.rows(), H_thin.rows()); + + + //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); MatrixXd K_transpose = S.ldlt().solve(H_thin*P); MatrixXd K = K_transpose.transpose(); // Compute the error of the state. VectorXd delta_x = K * r; + + cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; - // Update the IMU state. if(FILTER != 0) return; + + if(PRINTIMAGES) + { + //octave + ofstream myfile; + + myfile.open("/home/raphael/dev/octave/measurement2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: K\n" + << "# type: matrix\n" + << "# rows: " << K.rows() << "\n" + << "# columns: " << K.cols() << "\n" + << K << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << r.cols() << "\n" + << r << endl; + + myfile.close(); + } + delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); + delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); + + // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -2046,7 +2124,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; -/* + if (H.rows() > H.cols()) { // Convert H to a sparse matrix. SparseMatrix H_sparse = H.sparseView(); @@ -2070,10 +2148,10 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { //H_thin = Q1.transpose() * H; //r_thin = Q1.transpose() * r; - } else {*/ + } else { H_thin = H; r_thin = r; - //} + } @@ -2093,6 +2171,9 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // Update the IMU state. if (FILTER != 2) return; + delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); + delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); + const VectorXd& delta_x_imu = delta_x.head<21>(); if (//delta_x_imu.segment<3>(0).norm() > 0.15 || @@ -2196,6 +2277,33 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // Update the IMU state. if (FILTER != 1) return; + cout << "here" << endl; + + if(PRINTIMAGES) + { + //octave + ofstream myfile; + + myfile.open("/home/raphael/dev/octave/measurement2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: K\n" + << "# type: matrix\n" + << "# rows: " << K.rows() << "\n" + << "# columns: " << K.cols() << "\n" + << K << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << r.cols() << "\n" + << r << endl; + + myfile.close(); + } + + delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); + delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); + const VectorXd& delta_x_imu = delta_x.head<21>(); if (//delta_x_imu.segment<3>(0).norm() > 0.15 || @@ -2374,8 +2482,18 @@ void MsckfVio::removeLostFeatures() { cout << "measuring" << endl; - PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); - cout << "norm" << endl; + if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true); + { + cout << "p gating" << endl; + + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; + pr.segment(pstack_cntr, pr_j.rows()) = pr_j; + pstack_cntr += pH_xj.rows(); + } + } + + cout << "donephoto" << endl; featureJacobian(feature.id, cam_state_ids, H_xj, r_j); cout << "two" << endl; twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2387,11 +2505,6 @@ void MsckfVio::removeLostFeatures() { r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); } - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { - pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; - pr.segment(pstack_cntr, pr_j.rows()) = pr_j; - pstack_cntr += pH_xj.rows(); - } if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; @@ -2403,17 +2516,23 @@ void MsckfVio::removeLostFeatures() { //if (stack_cntr > 1500) break; } + if(pstack_cntr) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + + photometricMeasurementUpdate(pH_x, pr); + } + cout << "resizing" << endl; H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); // Perform the measurement update step. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); twoMeasurementUpdate(twoH_x, twor); // Remove all processed features from the map. @@ -2553,7 +2672,18 @@ void MsckfVio::pruneLastCamStateBuffer() cout << "measuring" << endl; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true); + { + cout << "p gating" << endl; + + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; + pr.segment(pstack_cntr, pr_j.rows()) = pr_j; + pstack_cntr += pH_xj.rows(); + } + } + + cout << "norm" << endl; featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); cout << "two" << endl; @@ -2566,11 +2696,7 @@ void MsckfVio::pruneLastCamStateBuffer() stack_cntr += H_xj.rows(); pruned_cntr++; } - if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { - pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; - pr.segment(pstack_cntr, pr_j.rows()) = pr_j; - pstack_cntr += pH_xj.rows(); - } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; @@ -2580,12 +2706,17 @@ void MsckfVio::pruneLastCamStateBuffer() feature.observations.erase(cam_id); } + if (pstack_cntr) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + + photometricMeasurementUpdate(pH_x, pr); + } + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - - pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); // Perform measurement update. twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); @@ -2594,7 +2725,6 @@ void MsckfVio::pruneLastCamStateBuffer() measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); twoMeasurementUpdate(twoH_x, twor); //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), @@ -2729,28 +2859,28 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; cout << "measuring" << endl; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) + { + cout << "p gating" << endl; + if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { + pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; + pr.segment(pstack_cntr, pr_j.rows()) = pr_j; + pstack_cntr += pH_xj.rows(); + } + } cout << "norm" << endl; featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); cout << "two" << endl; twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); cout << "gating" << endl; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); } - - if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { - pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; - pr.segment(pstack_cntr, pr_j.rows()) = pr_j; - pstack_cntr += pH_xj.rows(); - } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; @@ -2762,16 +2892,22 @@ void MsckfVio::pruneCamStateBuffer() { } + if(pstack_cntr > 0) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + + photometricMeasurementUpdate(pH_x, pr); + } + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); // Perform measurement update. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); twoMeasurementUpdate(twoH_x, twor); //reduction for (const auto& cam_id : rm_cam_state_ids) { From 58fe9916477aa5ebd283b0e7eaad53f8f4843cc7 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 28 Jun 2019 18:21:36 +0200 Subject: [PATCH 72/86] removed comments, reg. msckf not working currently --- config/camchain-imucam-tum.yaml | 4 +-- launch/msckf_vio_tum.launch | 4 +-- src/msckf_vio.cpp | 44 +++++---------------------------- 3 files changed, 10 insertions(+), 42 deletions(-) diff --git a/config/camchain-imucam-tum.yaml b/config/camchain-imucam-tum.yaml index 141035e..7edbd87 100644 --- a/config/camchain-imucam-tum.yaml +++ b/config/camchain-imucam-tum.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, -0.001662284667857643] - distortion_model: pre-equidistant + distortion_model: equidistant intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506] resolution: [1024, 1024] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, -0.002347858896562788] - distortion_model: pre-equidistant + distortion_model: equidistant intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407] resolution: [1024, 1024] rostopic: /cam1/image_raw diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index b3a66e5..a8432b2 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index f9178c2..22b5f4b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1547,7 +1547,6 @@ bool MsckfVio::PhotometricMeasurementJacobian( int count = 0; double dx, dy; - cout << "patching" << endl; for (auto point : feature.anchorPatch_3d) { //cout << "____feature-measurement_____\n" << endl; @@ -1666,7 +1665,6 @@ bool MsckfVio::PhotometricMeasurementJacobian( //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } - cout << "returning" << endl; return true; } @@ -1797,8 +1795,6 @@ bool MsckfVio::PhotometricFeatureJacobian( cout << "---------- LOGGED -------- " << endl; } - cout << "donefeature" << endl; - return true; } @@ -2166,8 +2162,6 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // Compute the error of the state. VectorXd delta_x = K * r; - cout << "two rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - cout << "two update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. if (FILTER != 2) return; @@ -2272,13 +2266,9 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // Compute the error of the state. VectorXd delta_x = K * r; - cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. if (FILTER != 1) return; - cout << "here" << endl; - if(PRINTIMAGES) { //octave @@ -2438,9 +2428,6 @@ void MsckfVio::removeLostFeatures() { // processed_feature_ids.size() << endl; //cout << "jacobian row #: " << jacobian_row_size << endl; - - cout << "sizing" << endl; - // Remove the features that do not have enough measurements. for (const auto& feature_id : invalid_feature_ids) map_server.erase(feature_id); @@ -2479,27 +2466,19 @@ void MsckfVio::removeLostFeatures() { MatrixXd twoH_xj; VectorXd twor_j; - - cout << "measuring" << endl; - + /* if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true); { - cout << "p gating" << endl; - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - } + }*/ - cout << "donephoto" << endl; featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - cout << "two" << endl; twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); - cout << "gating" << endl; - if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; @@ -2524,7 +2503,6 @@ void MsckfVio::removeLostFeatures() { photometricMeasurementUpdate(pH_x, pr); } - cout << "resizing" << endl; H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2670,26 +2648,21 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - cout << "measuring" << endl; - + /* if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true); { - cout << "p gating" << endl; if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - } + }*/ - cout << "norm" << endl; featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - cout << "two" << endl; twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); - cout << "gating" << endl; if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; @@ -2857,23 +2830,18 @@ void MsckfVio::pruneCamStateBuffer() { } if (involved_cam_state_ids.size() == 0) continue; - cout << "measuring" << endl; + /* if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { - cout << "p gating" << endl; if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - } - cout << "norm" << endl; + }*/ featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - cout << "two" << endl; twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); - - cout << "gating" << endl; if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; From 737c23f32ae92350d365b7a955bc807e34c1c3f2 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Tue, 2 Jul 2019 08:32:56 +0200 Subject: [PATCH 73/86] restructured calcualtion of patches in code --- include/msckf_vio/msckf_vio.h | 61 +++- launch/msckf_vio_tinytum.launch | 4 +- src/image_processor.cpp | 4 +- src/msckf_vio.cpp | 534 +++++++++++++++++++------------- 4 files changed, 365 insertions(+), 238 deletions(-) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 995c5a3..250dfac 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -202,32 +202,56 @@ class MsckfVio { Eigen::Vector4d& r); // This function computes the Jacobian of all measurements viewed // in the given camera states of this feature. - void featureJacobian(const FeatureIDType& feature_id, + void featureJacobian( + const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); void twodotMeasurementJacobian( - const StateIDType& cam_state_id, - const FeatureIDType& feature_id, - Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + + bool ConstructJacobians( + Eigen::MatrixXd& H_rho, + Eigen::MatrixXd& H_pl, + Eigen::MatrixXd& H_pA, + const Feature& feature, + const StateIDType& cam_state_id, + Eigen::MatrixXd& H_xl, + Eigen::MatrixXd& H_yl); + + bool PhotometricPatchPointResidual( + const StateIDType& cam_state_id, + const Feature& feature, + Eigen::VectorXd& r); + + bool PhotometricPatchPointJacobian( + const CAMState& cam_state, + const Feature& feature, + Eigen::Vector3d point, + int count, + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj); bool PhotometricMeasurementJacobian( - const StateIDType& cam_state_id, - const FeatureIDType& feature_id, - Eigen::MatrixXd& H_x, - Eigen::MatrixXd& H_y, - Eigen::VectorXd& r); + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::MatrixXd& H_x, + Eigen::MatrixXd& H_y, + Eigen::VectorXd& r); void twodotFeatureJacobian( - const FeatureIDType& feature_id, - const std::vector& cam_state_ids, - Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); bool PhotometricFeatureJacobian( - const FeatureIDType& feature_id, - const std::vector& cam_state_ids, - Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); void measurementUpdate(const Eigen::MatrixXd& H, @@ -263,6 +287,13 @@ class MsckfVio { // Chi squared test table. static std::map chi_squared_test_table; + double eval_time; + + IMUState timed_old_imu_state; + IMUState timed_old_true_state; + + IMUState old_imu_state; + IMUState old_true_state; // change in position Eigen::Vector3d delta_position; diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 28ae944..89112e1 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 168eebc..6867743 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -223,8 +223,8 @@ void ImageProcessor::stereoCallback( image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - ROS_INFO("Publishing: %f", - (ros::Time::now()-start_time).toSec()); + //ROS_INFO("Publishing: %f", + // (ros::Time::now()-start_time).toSec()); // Build the image pyramids once since they're used at multiple places createImagePyramids(); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 22b5f4b..51a56b0 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -260,6 +260,7 @@ bool MsckfVio::createRosIO() { // activating bag playing parameter, for debugging nh.setParam("/play_bag", true); + eval_time = 0; odom_pub = nh.advertise("odom", 10); truth_odom_pub = nh.advertise("true_odom", 10); feature_pub = nh.advertise( @@ -412,6 +413,10 @@ void MsckfVio::imageCallback( return; } + old_imu_state = state_server.imu_state; + old_true_state = true_state_server.imu_state; + + // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -419,7 +424,6 @@ void MsckfVio::imageCallback( is_first_img = false; state_server.imu_state.time = feature_msg->header.stamp.toSec(); } - static double max_processing_time = 0.0; static int critical_time_cntr = 0; double processing_start_time = ros::Time::now().toSec(); @@ -427,24 +431,14 @@ void MsckfVio::imageCallback( // Propogate the IMU state. // that are received before the image feature_msg. ros::Time start_time = ros::Time::now(); - - - batchImuProcessing(feature_msg->header.stamp.toSec()); - - if(GROUNDTRUTH) - { - state_server.imu_state.position = true_state_server.imu_state.position; - state_server.imu_state.orientation = true_state_server.imu_state.orientation; - state_server.imu_state.position_null = true_state_server.imu_state.position_null; - state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null; - state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0; - state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu; - } + nh.param("FILTER", FILTER, 0); + + batchImuProcessing(feature_msg->header.stamp.toSec()); + double imu_processing_time = ( ros::Time::now()-start_time).toSec(); - cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); @@ -453,7 +447,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - cout << "2" << endl; + // cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -462,7 +456,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - cout << "3" << endl; + // cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); @@ -470,14 +464,14 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - cout << "4" << endl; + // cout << "4" << endl; // Perform measurement update if necessary. start_time = ros::Time::now(); removeLostFeatures(); double remove_lost_features_time = ( ros::Time::now()-start_time).toSec(); - cout << "5" << endl; + // cout << "5" << endl; start_time = ros::Time::now(); pruneLastCamStateBuffer(); double prune_cam_states_time = ( @@ -487,7 +481,7 @@ void MsckfVio::imageCallback( batchTruthProcessing(feature_msg->header.stamp.toSec()); - cout << "6" << endl; + // cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -504,18 +498,18 @@ void MsckfVio::imageCallback( ++critical_time_cntr; ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", processing_time, critical_time_cntr); - printf("IMU processing time: %f/%f\n", - imu_processing_time, imu_processing_time/processing_time); - printf("State augmentation time: %f/%f\n", - state_augmentation_time, state_augmentation_time/processing_time); - printf("Add observations time: %f/%f\n", - add_observations_time, add_observations_time/processing_time); - printf("Remove lost features time: %f/%f\n", - remove_lost_features_time, remove_lost_features_time/processing_time); - printf("Remove camera states time: %f/%f\n", - prune_cam_states_time, prune_cam_states_time/processing_time); - printf("Publish time: %f/%f\n", - publish_time, publish_time/processing_time); + //printf("IMU processing time: %f/%f\n", + // imu_processing_time, imu_processing_time/processing_time); + //printf("State augmentation time: %f/%f\n", + // state_augmentation_time, state_augmentation_time/processing_time); + //printf("Add observations time: %f/%f\n", + // add_observations_time, add_observations_time/processing_time); + //printf("Remove lost features time: %f/%f\n", + // remove_lost_features_time, remove_lost_features_time/processing_time); + //printf("Remove camera states time: %f/%f\n", + // prune_cam_states_time, prune_cam_states_time/processing_time); + //printf("Publish time: %f/%f\n", + // publish_time, publish_time/processing_time); } if(STREAMPAUSE) @@ -806,8 +800,6 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { // Counter how many IMU msgs in the buffer are used. int used_truth_msg_cntr = 0; - const IMUState old_true_state = true_state_server.imu_state; - for (const auto& truth_msg : truth_msg_buffer) { double truth_time = truth_msg.header.stamp.toSec(); if (truth_time < true_state_server.imu_state.time) { @@ -839,22 +831,76 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { truth_msg_buffer.erase(truth_msg_buffer.begin(), truth_msg_buffer.begin()+used_truth_msg_cntr); + /* + // calculate change + delta_position = state_server.imu_state.position - old_imu_state.position; + Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), old_imu_state.orientation); + delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2); // calculate error in position // calculate error in change - // calculate change Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - old_true_state.position; Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), old_true_state.orientation); Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2); Eigen::Vector3d error_delta_position = delta_true_position - delta_position; Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation; - - double dx = (error_delta_position[0]/delta_true_position[0]); - double dy = (error_delta_position[1]/delta_true_position[1]); - double dz = (error_delta_position[2]/delta_true_position[2]); - cout << "relative pos error: " << sqrt(dx*dx + dy*dy + dz*dz) * 100 << "%" << endl; + Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position; + Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation); + Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2); + + double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/ + sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2])); + + double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + + cout << "relative pos error: " << relerr * 100 << "%" << endl; + cout << "relative ori error: " << relOerr * 100 << "%" << endl; + //cout << "absolute pos error: " << + */ + if (eval_time + 1 < ros::Time::now().toSec()) + { + + // calculate change + delta_position = state_server.imu_state.position - timed_old_imu_state.position; + Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), timed_old_imu_state.orientation); + delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2); + // calculate error in position + + // calculate error in change + + Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - timed_old_true_state.position; + Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), timed_old_true_state.orientation); + Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2); + Eigen::Vector3d error_delta_position = delta_true_position - delta_position; + Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation; + + Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position; + Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation); + Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2); + + double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/ + sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2])); + + double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + // cout << "relative pos error: " << relerr * 100 << "%" << endl; + // cout << "relative ori error: " << relOerr * 100 << "%" << endl; + + timed_old_imu_state = state_server.imu_state; + timed_old_true_state = true_state_server.imu_state; + eval_time = ros::Time::now().toSec(); + } } void MsckfVio::processTruthtoIMU(const double& time, @@ -1477,17 +1523,60 @@ void MsckfVio::twodotFeatureJacobian( return; } - - -bool MsckfVio::PhotometricMeasurementJacobian( - const StateIDType& cam_state_id, - const FeatureIDType& feature_id, - MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) +bool MsckfVio::PhotometricPatchPointResidual( + const StateIDType& cam_state_id, + const Feature& feature, + VectorXd& r) { - // Prepare all the required data. + VectorXd r_photo = VectorXd::Zero(N*N); + int count = 0; + auto frame = cam0.moving_window.find(cam_state_id)->second.image; + const CAMState& cam_state = state_server.cam_states[cam_state_id]; - const Feature& feature = map_server[feature_id]; + + //estimate photometric measurement + std::vector estimate_irradiance; + std::vector estimate_photo_z; + std::vector photo_z; + + IlluminationParameter estimated_illumination; + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); + for (auto& estimate_irradiance_j : estimate_irradiance) + estimate_photo_z.push_back (estimate_irradiance_j * + estimated_illumination.frame_gain * estimated_illumination.feature_gain + + estimated_illumination.frame_bias + estimated_illumination.feature_bias); + + + for(auto point : feature.anchorPatch_3d) + { + + + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + // test if projection is inside frame + if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) + return false; + //add observation + photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); + + //calculate photom. residual + r_photo(count) = photo_z[count] - estimate_photo_z[count]; + count++; + } + r = r_photo; + return true; +} + +// generates the jacobian of one patch point regarding rho, anchor and current frame +bool MsckfVio::PhotometricPatchPointJacobian( + const CAMState& cam_state, + const Feature& feature, + Eigen::Vector3d point, + int count, + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj) +{ const StateIDType anchor_state_id = feature.observations.begin()->first; const CAMState anchor_state = state_server.cam_states[anchor_state_id]; @@ -1501,10 +1590,7 @@ bool MsckfVio::PhotometricMeasurementJacobian( Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0; Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); - - //photometric observation - std::vector photo_z; - VectorXd r_photo = VectorXd::Zero(N*N); + // individual Jacobians Matrix dI_dhj = Matrix::Zero(); Matrix dh_dCpij = Matrix::Zero(); @@ -1517,6 +1603,69 @@ bool MsckfVio::PhotometricMeasurementJacobian( Matrix dCpij_dCGtheta = Matrix::Zero(); Matrix dCpij_dGpC = Matrix::Zero(); + double dx, dy; + + Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); + + + // calculate derivation for anchor frame, use position for derivation calculation + // frame derivative calculated convoluting with kernel [-1, 0, 1] + dx = feature.cvKernel(p_in_anchor, "Sobel_x"); + dy = feature.cvKernel(p_in_anchor, "Sobel_y"); + + dI_dhj(0, 0) = dx * cam0.intrinsics[0]; + dI_dhj(0, 1) = dy * cam0.intrinsics[1]; + + //dh / d{}^Cp_{ij} + dh_dCpij(0, 0) = 1 / p_c0(2); + dh_dCpij(1, 1) = 1 / p_c0(2); + dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); + dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); + + dCpij_dGpij = quaternionToRotation(cam_state.orientation); + + //orientation takes camera frame to world frame, we wa + dh_dGpij = dh_dCpij * dCpij_dGpij; + + //dh / d X_{pl} + dCpij_dCGtheta = skewSymmetric(p_c0); + dCpij_dGpC = -quaternionToRotation(cam_state.orientation); + dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta; + dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC; + + // d{}^Gp_P{ij} / \rho_i + double rho = feature.anchor_rho; + // Isometry T_anchor_w takes a vector in anchor frame to world frame + dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); + + dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() + * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), + feature.anchorPatch_ideal[count].y/(rho), + 1/(rho))); + dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); + + // Intermediate Jakobians + H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1 + H_plj = dI_dhj * dh_dXplj; // 1 x 6 + H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 + + return true; +} + +bool MsckfVio::PhotometricMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) +{ + + // Prepare all the required data. + const CAMState& cam_state = state_server.cam_states[cam_state_id]; + const Feature& feature = map_server[feature_id]; + + //photometric observation + VectorXd r_photo = VectorXd::Zero(N*N); + // one line of the NxN Jacobians Eigen::Matrix H_rhoj; Eigen::Matrix H_plj; @@ -1528,89 +1677,18 @@ bool MsckfVio::PhotometricMeasurementJacobian( Eigen::MatrixXd H_pA(N*N, 6); auto frame = cam0.moving_window.find(cam_state_id)->second.image; - auto anchor_frame = cam0.moving_window.find(anchor_state_id)->second.image; - //observation - const Vector4d& z = feature.observations.find(cam_state_id)->second; - //estimate photometric measurement - std::vector estimate_irradiance; - std::vector estimate_photo_z; - IlluminationParameter estimated_illumination; - feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); - - // calculated here, because we need true 'estimate_irradiance' later for jacobi - for (auto& estimate_irradiance_j : estimate_irradiance) - estimate_photo_z.push_back (estimate_irradiance_j * - estimated_illumination.frame_gain * estimated_illumination.feature_gain + - estimated_illumination.frame_bias + estimated_illumination.feature_bias); + // calcualte residual of patch + PhotometricPatchPointResidual(cam_state_id, feature, r_photo); + // calculate jacobian for patch int count = 0; - double dx, dy; - for (auto point : feature.anchorPatch_3d) { - //cout << "____feature-measurement_____\n" << endl; - Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); - cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); - cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); - - if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) - { - cout << "skipped" << endl; - return false; - } - //add observation - photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - - //calculate photom. residual - r_photo(count) = photo_z[count] - estimate_photo_z[count]; - //cout << "residual: " << photo_r.back() << endl; - - // calculate derivation for anchor frame, use position for derivation calculation - // frame derivative calculated convoluting with kernel [-1, 0, 1] - - dx = feature.cvKernel(p_in_anchor, "Sobel_x"); - dy = feature.cvKernel(p_in_anchor, "Sobel_y"); - - // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); - // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); - - dI_dhj(0, 0) = dx * cam0.intrinsics[0]; - dI_dhj(0, 1) = dy * cam0.intrinsics[1]; - - //dh / d{}^Cp_{ij} - dh_dCpij(0, 0) = 1 / p_c0(2); - dh_dCpij(1, 1) = 1 / p_c0(2); - dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); - dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); - - dCpij_dGpij = quaternionToRotation(cam_state.orientation); - - //orientation takes camera frame to world frame, we wa - dh_dGpij = dh_dCpij * dCpij_dGpij; - - //dh / d X_{pl} - dCpij_dCGtheta = skewSymmetric(p_c0); - dCpij_dGpC = -quaternionToRotation(cam_state.orientation); - dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta; - dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC; - - // d{}^Gp_P{ij} / \rho_i - double rho = feature.anchor_rho; - // Isometry T_anchor_w takes a vector in anchor frame to world frame - dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); - - dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() - * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), - feature.anchorPatch_ideal[count].y/(rho), - 1/(rho))); - dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); - - // Intermediate Jakobians - H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1 - H_plj = dI_dhj * dh_dXplj; // 1 x 6 - H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 - + // get jacobi of single point in patch + PhotometricPatchPointJacobian(cam_state, feature, point, count, H_rhoj, H_plj, H_pAj); + + // stack point into entire jacobi H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; H_pA.block<1, 6>(count, 0) = H_pAj; @@ -1618,48 +1696,31 @@ bool MsckfVio::PhotometricMeasurementJacobian( count++; } - cout << "done" << endl; + // construct the jacobian structure needed for nullspacing MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1); - // set anchor Jakobi - // get position of anchor in cam states + ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); - auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); - int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); - H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; - - // set frame Jakobi - //get position of current frame in cam states - auto cam_state_iter = state_server.cam_states.find(cam_state_id); - int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); - - // set jakobi of state - H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl; - - // set ones for irradiance bias - // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); - - // set irradiance error Block - H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N); - - // TODO make this calculation more fluent - - /*for(int i = 0; i< N*N; i++) - H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; - */ - - H_yl.block(0, N*N, N*N, 1) = -H_rho; + // set to return values H_x = H_xl; H_y = H_yl; - r = r_photo; - std::stringstream ss; - ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) - { + { + // pregenerating information for visualization + std::stringstream ss; + auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); + int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); + //get position of current frame in cam states + auto cam_state_iter = state_server.cam_states.find(cam_state_id); + int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); + + ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; + + // visualizing functions feature.MarkerGeneration(marker_pub, state_server.cam_states); feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); @@ -1668,6 +1729,43 @@ bool MsckfVio::PhotometricMeasurementJacobian( return true; } +bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, + Eigen::MatrixXd& H_pl, + Eigen::MatrixXd& H_pA, + const Feature& feature, + const StateIDType& cam_state_id, + Eigen::MatrixXd& H_xl, + Eigen::MatrixXd& H_yl) +{ + // get position of anchor in cam states + auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); + int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); + // set anchor Jakobi + H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; + + //get position of current frame in cam states + auto cam_state_iter = state_server.cam_states.find(cam_state_id); + // set frame Jakobi + int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); + + // set jakobi of state + H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl; + + // set ones for irradiance bias + // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); + + // set irradiance error Block + H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N); + + + /*for(int i = 0; i< N*N; i++) + H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; + */ + + H_yl.block(0, N*N, N*N, 1) = -H_rho; + +} + bool MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, @@ -1717,11 +1815,9 @@ bool MsckfVio::PhotometricFeatureJacobian( r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; } - if(stack_cntr == 0) - { - cout << "skip feature" << endl; + if(stack_cntr < 2*N*N) return false; - } + // Project the residual and Jacobians onto the nullspace // of H_yj. @@ -2027,8 +2123,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { VectorXd delta_x = K * r; - cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; + cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; if(FILTER != 0) return; @@ -2053,9 +2149,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { myfile.close(); } - delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); - delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); - // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -2239,10 +2332,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // Perform QR decompostion on H_sparse. SPQR > spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); - spqr_helper.compute(H_sparse); + cout << "comp" << endl; + spqr_helper.compute(H_sparse); + cout << "done" << endl; MatrixXd H_temp; VectorXd r_temp; + (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp); (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp); @@ -2261,6 +2357,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r Feature::observation_noise*MatrixXd::Identity( H_thin.rows(), H_thin.rows()); //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); + MatrixXd K_transpose = S.ldlt().solve(H_thin*P); MatrixXd K = K_transpose.transpose(); // Compute the error of the state. @@ -2271,28 +2368,27 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r if(PRINTIMAGES) { - //octave - ofstream myfile; - - myfile.open("/home/raphael/dev/octave/measurement2octave"); - myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" - << "# name: K\n" - << "# type: matrix\n" - << "# rows: " << K.rows() << "\n" - << "# columns: " << K.cols() << "\n" - << K << endl; + //octave + ofstream myfile; + + myfile.open("/home/raphael/dev/octave/measurement2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: K\n" + << "# type: matrix\n" + << "# rows: " << K.rows() << "\n" + << "# columns: " << K.cols() << "\n" + << K << endl; - myfile << "# name: r\n" - << "# type: matrix\n" - << "# rows: " << r.rows() << "\n" - << "# columns: " << r.cols() << "\n" - << r << endl; + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << r.cols() << "\n" + << r << endl; - myfile.close(); + myfile.close(); } - delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); - delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); + cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -2349,8 +2445,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - MatrixXd P1 = H * state_server.state_cov * H.transpose(); + MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * MatrixXd::Identity(H.rows(), H.rows()); @@ -2363,10 +2459,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) if (chi_squared_test_table[dof] == 0) return false; if (gamma < chi_squared_test_table[dof]) { - //cout << "passed" << endl; + // cout << "passed" << endl; return true; } else { - //cout << "failed" << endl; + // cout << "failed" << endl; return false; } } @@ -2465,16 +2561,15 @@ void MsckfVio::removeLostFeatures() { VectorXd pr_j; MatrixXd twoH_xj; VectorXd twor_j; - - /* - if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true); + + if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - }*/ + } featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2499,10 +2594,11 @@ void MsckfVio::removeLostFeatures() { { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - + photometricMeasurementUpdate(pH_x, pr); } + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2648,17 +2744,15 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - /* - if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true); - { + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) + { if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - }*/ - + } featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -2679,26 +2773,26 @@ void MsckfVio::pruneLastCamStateBuffer() feature.observations.erase(cam_id); } - if (pstack_cntr) + + if(pstack_cntr) { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - photometricMeasurementUpdate(pH_x, pr); + photometricMeasurementUpdate(pH_x, pr); } H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - // Perform measurement update. - + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); - // Perform measurement update. - - + + // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -2831,7 +2925,7 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - /* + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { @@ -2839,7 +2933,8 @@ void MsckfVio::pruneCamStateBuffer() { pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - }*/ + } + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -2860,23 +2955,24 @@ void MsckfVio::pruneCamStateBuffer() { } - if(pstack_cntr > 0) + + if(pstack_cntr) { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); - + pr.conservativeResize(pstack_cntr); photometricMeasurementUpdate(pH_x, pr); } H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); - - // Perform measurement update. + + // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); + //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), From 6bcc72f8265b8faa04f8b3d2c496ae6d71abada6 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Tue, 2 Jul 2019 16:58:03 +0200 Subject: [PATCH 74/86] changed to simple irradiance calcualation with derivation of image per frame - not working --- include/msckf_vio/feature.hpp | 45 +++++++++++ include/msckf_vio/msckf_vio.h | 4 +- launch/msckf_vio_tinytum.launch | 4 +- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 132 +++++++++++++++++++++----------- 5 files changed, 137 insertions(+), 50 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index b818dc6..c45f6a0 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -184,6 +184,11 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, const CameraCalibration& cam, Eigen::Vector3d& in_p) const; +double CompleteCvKernel( + const cv::Point2f pose, + const cv::Mat& frame, + std::string type) const; + double cvKernel( const cv::Point2f pose, std::string type) const; @@ -488,6 +493,45 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const else return false; } +double Feature::CompleteCvKernel( + const cv::Point2f pose, + const cv::Mat& frame, + std::string type) const +{ + + double delta = 0; + + cv::Mat xder; + cv::Mat yder; + cv::Mat deeper_frame; + frame.convertTo(deeper_frame,CV_16S); + //TODO remove this? + + + cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); + + xder/=8.; + yder/=8.; + + /* + cv::Mat norm_abs_xderImage; + cv::Mat abs_xderImage2; + cv::convertScaleAbs(xder, abs_xderImage2); + + cv::normalize(abs_xderImage2, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); + + cv::imshow("xder", norm_abs_xderImage); + cvWaitKey(0); + */ + + if(type == "Sobel_x") + delta = ((double)xder.at(pose.y, pose.x))/255.; + else if (type == "Sobel_y") + delta = ((double)yder.at(pose.y, pose.x))/255.; + return delta; +} + double Feature::cvKernel( const cv::Point2f pose, std::string type) const @@ -823,6 +867,7 @@ bool Feature::VisualizePatch( // residual grid projection, positive - red, negative - blue colored namer.str(std::string()); namer << "residual"; + std::cout << "-- photo_r -- \n" << photo_r << " -- " << std::endl; cv::putText(irradianceFrame, namer.str() , cvPoint(30+scale*N, scale*N/2-5), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 250dfac..90a5750 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -229,12 +229,14 @@ class MsckfVio { bool PhotometricPatchPointJacobian( const CAMState& cam_state, + const StateIDType& cam_state_id, const Feature& feature, Eigen::Vector3d point, int count, Eigen::Matrix& H_rhoj, Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj); + Eigen::Matrix& H_pAj, + Eigen::Matrix& dI_dhj); bool PhotometricMeasurementJacobian( const StateIDType& cam_state_id, diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 89112e1..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index a8432b2..b7e9365 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -25,7 +25,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 51a56b0..2ba011b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -496,8 +496,8 @@ void MsckfVio::imageCallback( processing_end_time - processing_start_time; if (processing_time > 1.0/frame_rate) { ++critical_time_cntr; - ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", - processing_time, critical_time_cntr); + //ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", + // processing_time, critical_time_cntr); //printf("IMU processing time: %f/%f\n", // imu_processing_time, imu_processing_time/processing_time); //printf("State augmentation time: %f/%f\n", @@ -1418,8 +1418,7 @@ void MsckfVio::twodotMeasurementJacobian( { std::stringstream ss; ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; - feature.MarkerGeneration(marker_pub, state_server.cam_states); - //feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); + feature.MarkerGeneration(marker_pub, state_server.cam_states); } return; @@ -1540,27 +1539,40 @@ bool MsckfVio::PhotometricPatchPointResidual( std::vector estimate_photo_z; std::vector photo_z; + // estimate irradiance based on anchor frame IlluminationParameter estimated_illumination; feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); for (auto& estimate_irradiance_j : estimate_irradiance) - estimate_photo_z.push_back (estimate_irradiance_j * - estimated_illumination.frame_gain * estimated_illumination.feature_gain + - estimated_illumination.frame_bias + estimated_illumination.feature_bias); + estimate_photo_z.push_back (estimate_irradiance_j);// * + //estimated_illumination.frame_gain * estimated_illumination.feature_gain + + //estimated_illumination.frame_bias + estimated_illumination.feature_bias); + + + // irradiance measurement around feature point + std::vector true_irradiance; + cv::Point2f p_f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); + p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); + cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; + for(int i = 0; i frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) return false; - //add observation + // add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - //calculate photom. residual - r_photo(count) = photo_z[count] - estimate_photo_z[count]; + // calculate photom. residual acc. to paper + // r_photo(count) = photo_z[count] - estimate_photo_z[count]; + + // calculate alternate photom. residual + r_photo(count) = true_irradiance[count] - photo_z[count]; count++; } r = r_photo; @@ -1570,12 +1582,14 @@ bool MsckfVio::PhotometricPatchPointResidual( // generates the jacobian of one patch point regarding rho, anchor and current frame bool MsckfVio::PhotometricPatchPointJacobian( const CAMState& cam_state, + const StateIDType& cam_state_id, const Feature& feature, Eigen::Vector3d point, int count, Eigen::Matrix& H_rhoj, Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj) + Eigen::Matrix& H_pAj, + Matrix& dI_dhj) { const StateIDType anchor_state_id = feature.observations.begin()->first; @@ -1592,7 +1606,7 @@ bool MsckfVio::PhotometricPatchPointJacobian( // individual Jacobians - Matrix dI_dhj = Matrix::Zero(); + /*Matrix */dI_dhj = Matrix::Zero(); Matrix dh_dCpij = Matrix::Zero(); Matrix dh_dGpij = Matrix::Zero(); Matrix dh_dXplj = Matrix::Zero(); @@ -1606,13 +1620,17 @@ bool MsckfVio::PhotometricPatchPointJacobian( double dx, dy; Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); + auto frame = cam0.moving_window.find(cam_state_id)->second.image; + // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.cvKernel(p_in_anchor, "Sobel_x"); - dy = feature.cvKernel(p_in_anchor, "Sobel_y"); + dx = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_x"); + dy = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_y"); + //cout << "dx: " << dx << " : " << feature.cvKernel(p_in_c0, "Sobel_x") << " : " << feature.Kernel(p_in_c0, frame, "Sobel_x") << endl; dI_dhj(0, 0) = dx * cam0.intrinsics[0]; dI_dhj(0, 1) = dy * cam0.intrinsics[1]; @@ -1650,7 +1668,11 @@ bool MsckfVio::PhotometricPatchPointJacobian( H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 - return true; + // check if point nullspaceable + if (H_rhoj(0, 0) != 0) + return true; + + return false; } bool MsckfVio::PhotometricMeasurementJacobian( @@ -1671,6 +1693,8 @@ bool MsckfVio::PhotometricMeasurementJacobian( Eigen::Matrix H_plj; Eigen::Matrix H_pAj; + Eigen::MatrixXd dI_dh(N*N, 2); + // combined Jacobians Eigen::MatrixXd H_rho(N*N, 1); Eigen::MatrixXd H_pl(N*N, 6); @@ -1683,26 +1707,33 @@ bool MsckfVio::PhotometricMeasurementJacobian( // calculate jacobian for patch int count = 0; + bool valid = false; + Matrix dI_dhj;// = Matrix::Zero(); for (auto point : feature.anchorPatch_3d) { // get jacobi of single point in patch - PhotometricPatchPointJacobian(cam_state, feature, point, count, H_rhoj, H_plj, H_pAj); - + if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj)) + valid = true; + // stack point into entire jacobi H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; H_pA.block<1, 6>(count, 0) = H_pAj; + dI_dh.block<1, 2>(count, 0) = dI_dhj; + count++; } + //Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; + //cout << "h photo: \n" << h_photo << endl; + // construct the jacobian structure needed for nullspacing MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1); - + MatrixXd H_yl = MatrixXd::Zero(N*N, 1); + ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); - // set to return values H_x = H_xl; H_y = H_yl; @@ -1726,9 +1757,13 @@ bool MsckfVio::PhotometricMeasurementJacobian( //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } - return true; + if(valid) + return true; + else + return false; } +// uses the calcualted jacobians to construct the final Hx Hy jacobians used for nullspacing bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, Eigen::MatrixXd& H_pl, Eigen::MatrixXd& H_pA, @@ -1741,7 +1776,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); // set anchor Jakobi - H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; + H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = H_pA; //get position of current frame in cam states auto cam_state_iter = state_server.cam_states.find(cam_state_id); @@ -1749,21 +1784,20 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); // set jakobi of state - H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl; + H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = H_pl; // set ones for irradiance bias // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); // set irradiance error Block - H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N); + //H_yl.block(0, 0, N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N); /*for(int i = 0; i< N*N; i++) H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; */ - H_yl.block(0, N*N, N*N, 1) = -H_rho; - + H_yl = H_rho; } @@ -1772,7 +1806,6 @@ bool MsckfVio::PhotometricFeatureJacobian( const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { - const auto& feature = map_server[feature_id]; // Check how many camera states in the provided camera @@ -1792,7 +1825,7 @@ bool MsckfVio::PhotometricFeatureJacobian( MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, 21+state_server.cam_states.size()*7); - MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+1); + MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); // CHANGED N*N+1 to 1 VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1803,7 +1836,7 @@ bool MsckfVio::PhotometricFeatureJacobian( Eigen::VectorXd r_l = VectorXd::Zero(N*N); if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) - continue; + continue; auto cam_state_iter = state_server.cam_states.find(cam_id); int cam_state_cntr = std::distance( @@ -1815,19 +1848,25 @@ bool MsckfVio::PhotometricFeatureJacobian( r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; } - if(stack_cntr < 2*N*N) - return false; + // if not enough to propper nullspace (in paper implementation) + if(stack_cntr < N*N) + return false; // Project the residual and Jacobians onto the nullspace // of H_yj. // get Nullspace + + bool valid = false; + for(int i = 0; i < H_yi.rows(); i++) + if(H_yi(i,0) != 0) + valid = true; + FullPivLU lu(H_yi.transpose()); MatrixXd A_null_space = lu.kernel(); H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; - //cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; if(PRINTIMAGES) @@ -1956,6 +1995,7 @@ void MsckfVio::measurementJacobian( r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); + // cout << "h reg: \n" << r(0) << "\n" << r(1) << endl; return; } @@ -2333,9 +2373,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r SPQR > spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); - cout << "comp" << endl; spqr_helper.compute(H_sparse); - cout << "done" << endl; + MatrixXd H_temp; VectorXd r_temp; @@ -2364,6 +2403,9 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r VectorXd delta_x = K * r; // Update the IMU state. + cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + + if (FILTER != 1) return; if(PRINTIMAGES) @@ -2387,9 +2429,6 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r myfile.close(); } - - cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; - const VectorXd& delta_x_imu = delta_x.head<21>(); if (//delta_x_imu.segment<3>(0).norm() > 0.15 || @@ -2445,7 +2484,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - + return true; MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * @@ -2453,8 +2492,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - //cout << "gate: " << dof << " " << gamma << " " << - //chi_squared_test_table[dof] << endl; + cout << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; if (chi_squared_test_table[dof] == 0) return false; @@ -2562,7 +2601,7 @@ void MsckfVio::removeLostFeatures() { MatrixXd twoH_xj; VectorXd twor_j; - if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true) + if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) { if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; @@ -2747,7 +2786,7 @@ void MsckfVio::pruneLastCamStateBuffer() if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); @@ -2769,6 +2808,7 @@ void MsckfVio::pruneLastCamStateBuffer() twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2928,7 +2968,7 @@ void MsckfVio::pruneCamStateBuffer() { if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { + if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); @@ -2937,7 +2977,7 @@ void MsckfVio::pruneCamStateBuffer() { featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); - + if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; @@ -2949,7 +2989,7 @@ void MsckfVio::pruneCamStateBuffer() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } From 6ee756941c062d779763903a8a90070bd3b8b602 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Wed, 3 Jul 2019 16:11:23 +0200 Subject: [PATCH 75/86] added stereo camera residual and jacobi to twomsckf - works --- include/msckf_vio/cam_state.h | 2 + include/msckf_vio/feature.hpp | 36 ++------ launch/msckf_vio_tinytum.launch | 4 +- src/msckf_vio.cpp | 150 +++++++++++++++++++------------- 4 files changed, 99 insertions(+), 93 deletions(-) diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index 723f252..880c70c 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -18,6 +18,8 @@ namespace msckf_vio { struct Frame{ cv::Mat image; + cv::Mat dximage; + cv::Mat dyimage; double exposureTime_ms; }; diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index c45f6a0..e30df3c 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -184,9 +184,10 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, const CameraCalibration& cam, Eigen::Vector3d& in_p) const; -double CompleteCvKernel( + double CompleteCvKernel( const cv::Point2f pose, - const cv::Mat& frame, + const StateIDType& cam_state_id, + CameraCalibration& cam, std::string type) const; double cvKernel( @@ -495,40 +496,17 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const double Feature::CompleteCvKernel( const cv::Point2f pose, - const cv::Mat& frame, + const StateIDType& cam_state_id, + CameraCalibration& cam, std::string type) const { double delta = 0; - cv::Mat xder; - cv::Mat yder; - cv::Mat deeper_frame; - frame.convertTo(deeper_frame,CV_16S); - //TODO remove this? - - - cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); - cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); - - xder/=8.; - yder/=8.; - - /* - cv::Mat norm_abs_xderImage; - cv::Mat abs_xderImage2; - cv::convertScaleAbs(xder, abs_xderImage2); - - cv::normalize(abs_xderImage2, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); - - cv::imshow("xder", norm_abs_xderImage); - cvWaitKey(0); - */ - if(type == "Sobel_x") - delta = ((double)xder.at(pose.y, pose.x))/255.; + delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at(pose.y, pose.x))/255.; else if (type == "Sobel_y") - delta = ((double)yder.at(pose.y, pose.x))/255.; + delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at(pose.y, pose.x))/255.; return delta; } diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 28ae944..d4fbc31 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 2ba011b..af56991 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -544,13 +544,22 @@ void MsckfVio::manageMovingWindow( image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - // save image information into moving window cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone(); cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); + cv::Mat xder; + cv::Mat yder; + cv::Mat deeper_frame; + cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); - + cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); + xder/=8.; + yder/=8.; + + cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); + cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); @@ -1310,96 +1319,98 @@ void MsckfVio::twodotMeasurementJacobian( const CAMState& cam_state = state_server.cam_states[cam_state_id]; const Feature& feature = map_server[feature_id]; - // Cam0 pose. + // Cam0 pose Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Vector3d& t_c0_w = cam_state.position; - //photometric observation - std::vector photo_z; + // Cam1 pose + Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); + Matrix3d R_w_c1 = R_c0_c1 * R_w_c0; + Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); // individual Jacobians - Matrix dh_dCpij = Matrix::Zero(); - Matrix dh_dGpij = Matrix::Zero(); - Matrix dh_dXplj = Matrix::Zero(); + Matrix dh_dC0pij = Matrix::Zero(); + Matrix dh_dC1pij = Matrix::Zero(); + Matrix dh_dGpij = Matrix::Zero(); + Matrix dh_dXplj = Matrix::Zero(); Matrix dGpj_drhoj = Matrix::Zero(); Matrix dGpj_XpAj = Matrix::Zero(); - Matrix dCpij_dGpij = Matrix::Zero(); - Matrix dCpij_dCGtheta = Matrix::Zero(); - Matrix dCpij_dGpC = Matrix::Zero(); + Matrix dC0pij_dGpij = Matrix::Zero(); + Matrix dC1pij_dGpij = Matrix::Zero(); + Matrix dC0pij_dXplj = Matrix::Zero(); + Matrix dC1pij_dXplj = Matrix::Zero(); // one line of the NxN Jacobians - Eigen::Matrix H_rho; - Eigen::Matrix H_plj; - Eigen::Matrix H_pAj; + Eigen::Matrix H_rho; + Eigen::Matrix H_plj; + Eigen::Matrix H_pAj; auto frame = cam0.moving_window.find(cam_state_id)->second.image; - int count = 0; - auto point = feature.anchorPatch_3d[0]; Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + Eigen::Vector3d p_c1 = R_w_c1 * (point-t_c1_w); // add jacobian //dh / d{}^Cp_{ij} - dh_dCpij(0, 0) = 1 / p_c0(2); - dh_dCpij(1, 1) = 1 / p_c0(2); - dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); - dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); + dh_dC0pij(0, 0) = 1. / p_c0(2); + dh_dC0pij(1, 1) = 1. / p_c0(2); + dh_dC0pij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); + dh_dC0pij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); - dCpij_dGpij = quaternionToRotation(cam_state.orientation); + //dh / d{}^Cp_{ij} + dh_dC1pij(2, 0) = 1. / p_c1(2); + dh_dC1pij(3, 1) = 1. / p_c1(2); + dh_dC1pij(2, 2) = -(p_c1(0))/(p_c1(2)*p_c1(2)); + dh_dC1pij(3, 2) = -(p_c1(1))/(p_c1(2)*p_c1(2)); - //orientation takes camera frame to world frame - dh_dGpij = dh_dCpij * dCpij_dGpij; + dC0pij_dGpij = R_w_c0; + dC1pij_dGpij = R_c0_c1 * R_w_c0; - //dh / d X_{pl} - dCpij_dCGtheta = skewSymmetric(p_c0); - dCpij_dGpC = -quaternionToRotation(cam_state.orientation); - dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta; - dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC; + dC0pij_dXplj.leftCols(3) = skewSymmetric(p_c0); + dC0pij_dXplj.rightCols(3) = -R_w_c0; - //d{}^Gp_P{ij} / \rho_i + + // d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; // Isometry T_anchor_w takes a vector in anchor frame to world frame - dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); + dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(N*N-1)/2].y/(rho*rho), 1/(rho*rho)); - - - // alternative derivation towards feature - Matrix3d dCpc0_dpg = R_w_c0; dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() - * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), - feature.anchorPatch_ideal[count].y/(rho), + * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho), + feature.anchorPatch_ideal[(N*N-1)/2].y/(rho), 1/(rho))); dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); // Intermediate Jakobians - H_rho = dh_dGpij * dGpj_drhoj; // 2 x 1 - H_plj = dh_dXplj; // 2 x 6 - H_pAj = dh_dGpij * dGpj_XpAj; // 2 x 6 + H_rho = dh_dC0pij * dC0pij_dGpij * dGpj_drhoj + dh_dC1pij * dC1pij_dGpij * dGpj_drhoj; // 4 x 1 + H_plj = dh_dC0pij * dC0pij_dXplj + dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6 + H_pAj = dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 // calculate residual //observation const Vector4d& total_z = feature.observations.find(cam_state_id)->second; - const Vector2d z = Vector2d(total_z[0], total_z[1]); - VectorXd r_i = VectorXd::Zero(2); + VectorXd r_i = VectorXd::Zero(4); //calculate residual - r_i[0] = z[0] - p_c0(0)/p_c0(2); - r_i[1] = z[1] - p_c0(1)/p_c0(2); - - MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7); + r_i[0] = total_z[0] - p_c0(0)/p_c0(2); + r_i[1] = total_z[1] - p_c0(1)/p_c0(2); + r_i[2] = total_z[2] - p_c1(0)/p_c1(2); + r_i[3] = total_z[3] - p_c1(1)/p_c1(2); + + MatrixXd H_xl = MatrixXd::Zero(4, 21+state_server.cam_states.size()*7); // set anchor Jakobi // get position of anchor in cam states auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); - H_xl.block(0, 21+cam_state_cntr_anchor*7, 2, 6) = H_pAj; + H_xl.block(0, 21+cam_state_cntr_anchor*7, 4, 6) = H_pAj; // set frame Jakobi //get position of current frame in cam states @@ -1407,7 +1418,7 @@ void MsckfVio::twodotMeasurementJacobian( int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); // set jakobi of state - H_xl.block(0, 21+cam_state_cntr*7, 2, 6) = H_plj; + H_xl.block(0, 21+cam_state_cntr*7, 4, 6) = H_plj; H_x = H_xl; H_y = H_rho; @@ -1446,7 +1457,7 @@ void MsckfVio::twodotFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = 2 * valid_cam_state_ids.size(); + jacobian_row_size = 4 * valid_cam_state_ids.size(); MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, 21+state_server.cam_states.size()*7); @@ -1458,7 +1469,7 @@ void MsckfVio::twodotFeatureJacobian( MatrixXd H_xl; MatrixXd H_yl; - Eigen::VectorXd r_l = VectorXd::Zero(2); + Eigen::VectorXd r_l = VectorXd::Zero(4); twodotMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); auto cam_state_iter = state_server.cam_states.find(cam_id); @@ -1468,13 +1479,14 @@ void MsckfVio::twodotFeatureJacobian( // Stack the Jacobians. H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl; H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl; - r_i.segment(stack_cntr, 2) = r_l; - stack_cntr += 2; + r_i.segment(stack_cntr, 4) = r_l; + stack_cntr += 4; } // Project the residual and Jacobians onto the nullspace // of H_yj. + // get Nullspace FullPivLU lu(H_yi.transpose()); MatrixXd A_null_space = lu.kernel(); @@ -1518,10 +1530,10 @@ void MsckfVio::twodotFeatureJacobian( std::cout << "resume playback" << std::endl; nh.setParam("/play_bag", true); } - return; } + bool MsckfVio::PhotometricPatchPointResidual( const StateIDType& cam_state_id, const Feature& feature, @@ -1628,8 +1640,8 @@ bool MsckfVio::PhotometricPatchPointJacobian( // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_x"); - dy = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_y"); + dx = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x"); + dy = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y"); //cout << "dx: " << dx << " : " << feature.cvKernel(p_in_c0, "Sobel_x") << " : " << feature.Kernel(p_in_c0, frame, "Sobel_x") << endl; dI_dhj(0, 0) = dx * cam0.intrinsics[0]; @@ -1806,6 +1818,8 @@ bool MsckfVio::PhotometricFeatureJacobian( const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { + + return false; const auto& feature = map_server[feature_id]; // Check how many camera states in the provided camera @@ -2164,7 +2178,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { // cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; if(FILTER != 0) return; @@ -2247,8 +2261,12 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { + if (H.rows() == 0 || r.rows() == 0) + { + cout << "zero" << endl; return; + } // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -2298,6 +2316,8 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // Update the IMU state. if (FILTER != 2) return; + cout << "two: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); @@ -2403,7 +2423,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r VectorXd delta_x = K * r; // Update the IMU state. - cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; if (FILTER != 1) return; @@ -2484,7 +2504,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - return true; + //return true; MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * @@ -2492,8 +2512,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - cout << "gate: " << dof << " " << gamma << " " << - chi_squared_test_table[dof] << endl; + //cout << "gate: " << dof << " " << gamma << " " << + //chi_squared_test_table[dof] << endl; if (chi_squared_test_table[dof] == 0) return false; @@ -2552,7 +2572,7 @@ void MsckfVio::removeLostFeatures() { } pjacobian_row_size += N*N*feature.observations.size(); - twojacobian_row_size += 2*feature.observations.size(); + twojacobian_row_size += 4*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; processed_feature_ids.push_back(feature.id); @@ -2604,6 +2624,7 @@ void MsckfVio::removeLostFeatures() { if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) { if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + //cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); @@ -2619,6 +2640,7 @@ void MsckfVio::removeLostFeatures() { stack_cntr += H_xj.rows(); } if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { + cout << "passed" << endl; twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); @@ -2749,7 +2771,7 @@ void MsckfVio::pruneLastCamStateBuffer() pjacobian_row_size += N*N*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; - twojacobian_row_size += 2*feature.observations.size(); + twojacobian_row_size += 4*feature.observations.size(); } @@ -2787,6 +2809,7 @@ void MsckfVio::pruneLastCamStateBuffer() if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + //cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); @@ -2804,6 +2827,7 @@ void MsckfVio::pruneLastCamStateBuffer() } if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + cout << "passed" << endl; twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); @@ -2929,7 +2953,7 @@ void MsckfVio::pruneCamStateBuffer() { } } - twojacobian_row_size += 2*involved_cam_state_ids.size(); + twojacobian_row_size += 4*involved_cam_state_ids.size(); pjacobian_row_size += N*N*involved_cam_state_ids.size(); jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -2969,6 +2993,7 @@ void MsckfVio::pruneCamStateBuffer() { if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { + //cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); @@ -2985,6 +3010,7 @@ void MsckfVio::pruneCamStateBuffer() { } if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + cout << "passed" << endl; twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); From 3873c978dd29ac155e3fc623c6fd9d8b560ec97f Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Wed, 3 Jul 2019 17:48:54 +0200 Subject: [PATCH 76/86] added structure for stereo photometry - diverging --- include/msckf_vio/cam_state.h | 1 + include/msckf_vio/feature.hpp | 40 ++++-- include/msckf_vio/msckf_vio.h | 8 +- launch/msckf_vio_tinytum.launch | 4 +- src/msckf_vio.cpp | 222 ++++++++++++++++++-------------- 5 files changed, 161 insertions(+), 114 deletions(-) diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index 880c70c..9eda6f5 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -41,6 +41,7 @@ struct CameraCalibration{ cv::Vec4d distortion_coeffs; movingWindow moving_window; cv::Mat featureVisu; + int id; }; diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index e30df3c..a2a3d63 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -507,6 +507,7 @@ double Feature::CompleteCvKernel( delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at(pose.y, pose.x))/255.; else if (type == "Sobel_y") delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at(pose.y, pose.x))/255.; + return delta; } @@ -990,28 +991,39 @@ cv::Point2f Feature::projectPositionToCamera( cv::Point2f out_p; cv::Point2f my_p; // transfrom position to camera frame + + // cam0 position Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Eigen::Vector3d& t_c0_w = cam_state.position; - Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); - out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); - // if(cam_state_id == observations.begin()->first) - //printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y); - + // project point according to model + if(cam.id == 0) + { + Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); + out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); + + } + // if camera is one, calcualte the cam1 position from cam0 position first + else if(cam.id == 1) + { + // cam1 position + Eigen::Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); + Eigen::Matrix3d R_w_c1 = R_c0_c1 * R_w_c0; + Eigen::Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); + + Eigen::Vector3d p_c1 = R_w_c1 * (in_p-t_c1_w); + out_p = cv::Point2f(p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); + } + + // undistort point according to camera model if (cam.distortion_model.substr(0,3) == "pre-") my_p = cv::Point2f(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]); else my_p = image_handler::distortPoint(out_p, - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs); - - - // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); - // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); - // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); - + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); return my_p; } diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 90a5750..06bad0f 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -233,10 +233,10 @@ class MsckfVio { const Feature& feature, Eigen::Vector3d point, int count, - Eigen::Matrix& H_rhoj, - Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj, - Eigen::Matrix& dI_dhj); + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj, + Eigen::Matrix& dI_dhj); bool PhotometricMeasurementJacobian( const StateIDType& cam_state_id, diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index d4fbc31..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index af56991..5e3dd8c 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -184,6 +184,8 @@ bool MsckfVio::loadParameters() { cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; + cam0.id = 0; + cam1.id = 1; state_server.state_cov = MatrixXd::Zero(21, 21); @@ -551,8 +553,8 @@ void MsckfVio::manageMovingWindow( cv::Mat xder; cv::Mat yder; cv::Mat deeper_frame; - cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); + cam0_img_ptr->image.convertTo(deeper_frame,CV_16S); cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); xder/=8.; @@ -561,6 +563,15 @@ void MsckfVio::manageMovingWindow( cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); + cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); + cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); + xder/=8.; + yder/=8.; + + cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); + cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); + //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) @@ -1540,51 +1551,63 @@ bool MsckfVio::PhotometricPatchPointResidual( VectorXd& r) { - VectorXd r_photo = VectorXd::Zero(N*N); + VectorXd r_photo = VectorXd::Zero(2*N*N); int count = 0; - auto frame = cam0.moving_window.find(cam_state_id)->second.image; - const CAMState& cam_state = state_server.cam_states[cam_state_id]; //estimate photometric measurement std::vector estimate_irradiance; std::vector estimate_photo_z; - std::vector photo_z; + std::vector photo_z_c0, photo_z_c1; // estimate irradiance based on anchor frame + /* IlluminationParameter estimated_illumination; feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); for (auto& estimate_irradiance_j : estimate_irradiance) estimate_photo_z.push_back (estimate_irradiance_j);// * //estimated_illumination.frame_gain * estimated_illumination.feature_gain + //estimated_illumination.frame_bias + estimated_illumination.feature_bias); + */ + // irradiance measurement around feature point in c0 and c1 + std::vector true_irradiance_c0, true_irradiance_c1; + cv::Point2f p_f_c0(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); + cv::Point2f p_f_c1(feature.observations.find(cam_state_id)->second(2), feature.observations.find(cam_state_id)->second(3)); - // irradiance measurement around feature point - std::vector true_irradiance; - cv::Point2f p_f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); - p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); - cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; - for(int i = 0; isecond.image; + cv::Mat current_image_c1 = cam1.moving_window.find(cam_state_id)->second.image; + for(int i = 0; i frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) + if(p_in_c0.x < 0 or p_in_c0.x > current_image_c0.cols-1 or p_in_c0.y < 0 or p_in_c0.y > current_image_c0.rows-1) + return false; + if(p_in_c1.x < 0 or p_in_c1.x > current_image_c1.cols-1 or p_in_c1.y < 0 or p_in_c1.y > current_image_c1.rows-1) return false; - // add observation - photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); + // add observation + photo_z_c0.push_back(feature.PixelIrradiance(p_in_c0, current_image_c0)); + photo_z_c1.push_back(feature.PixelIrradiance(p_in_c1, current_image_c1)); // calculate photom. residual acc. to paper // r_photo(count) = photo_z[count] - estimate_photo_z[count]; - // calculate alternate photom. residual - r_photo(count) = true_irradiance[count] - photo_z[count]; + // calculate photom. residual alternating between frames + r_photo(count*2) = true_irradiance_c0[count] - photo_z_c0[count]; + r_photo(count*2+1) = true_irradiance_c1[count] - photo_z_c1[count]; count++; } r = r_photo; @@ -1598,10 +1621,10 @@ bool MsckfVio::PhotometricPatchPointJacobian( const Feature& feature, Eigen::Vector3d point, int count, - Eigen::Matrix& H_rhoj, - Eigen::Matrix& H_plj, - Eigen::Matrix& H_pAj, - Matrix& dI_dhj) + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj, + Matrix& dI_dhj) { const StateIDType anchor_state_id = feature.observations.begin()->first; @@ -1618,67 +1641,82 @@ bool MsckfVio::PhotometricPatchPointJacobian( // individual Jacobians - /*Matrix */dI_dhj = Matrix::Zero(); - Matrix dh_dCpij = Matrix::Zero(); - Matrix dh_dGpij = Matrix::Zero(); - Matrix dh_dXplj = Matrix::Zero(); + /*Matrix */dI_dhj = Matrix::Zero(); + + Matrix dh_dC0pij = Matrix::Zero(); + Matrix dh_dC1pij = Matrix::Zero(); + Matrix dh_dGpij = Matrix::Zero(); + Matrix dh_dXplj = Matrix::Zero(); Matrix dGpj_drhoj = Matrix::Zero(); Matrix dGpj_XpAj = Matrix::Zero(); - Matrix dCpij_dGpij = Matrix::Zero(); - Matrix dCpij_dCGtheta = Matrix::Zero(); - Matrix dCpij_dGpC = Matrix::Zero(); + Matrix dC0pij_dGpij = Matrix::Zero(); + Matrix dC1pij_dGpij = Matrix::Zero(); + Matrix dC0pij_dXplj = Matrix::Zero(); + Matrix dC1pij_dXplj = Matrix::Zero(); - double dx, dy; + // irradiance jacobian + double dx_c0, dy_c0, dx_c1, dy_c1; Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + Eigen::Vector3d p_c1 = R_w_c1 * (point-t_c1_w); + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_c1 = feature.projectPositionToCamera(cam_state, cam_state_id, cam1, point); + cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); - - auto frame = cam0.moving_window.find(cam_state_id)->second.image; - // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x"); - dy = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y"); - //cout << "dx: " << dx << " : " << feature.cvKernel(p_in_c0, "Sobel_x") << " : " << feature.Kernel(p_in_c0, frame, "Sobel_x") << endl; + dx_c0 = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x"); + dy_c0 = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y"); - dI_dhj(0, 0) = dx * cam0.intrinsics[0]; - dI_dhj(0, 1) = dy * cam0.intrinsics[1]; + dx_c1 = feature.CompleteCvKernel(p_in_c1, cam_state_id, cam1, "Sobel_x"); + dy_c1 = feature.CompleteCvKernel(p_in_c1, cam_state_id, cam1, "Sobel_y"); + + dI_dhj(0, 0) = dx_c0 * cam0.intrinsics[0]; + dI_dhj(0, 1) = dy_c0 * cam0.intrinsics[1]; + dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0]; + dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1]; + + cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; + + // add jacobian + + //dh / d{}^Cp_{ij} + dh_dC0pij(0, 0) = 1. / p_c0(2); + dh_dC0pij(1, 1) = 1. / p_c0(2); + dh_dC0pij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); + dh_dC0pij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); //dh / d{}^Cp_{ij} - dh_dCpij(0, 0) = 1 / p_c0(2); - dh_dCpij(1, 1) = 1 / p_c0(2); - dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); - dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); + dh_dC1pij(2, 0) = 1. / p_c1(2); + dh_dC1pij(3, 1) = 1. / p_c1(2); + dh_dC1pij(2, 2) = -(p_c1(0))/(p_c1(2)*p_c1(2)); + dh_dC1pij(3, 2) = -(p_c1(1))/(p_c1(2)*p_c1(2)); - dCpij_dGpij = quaternionToRotation(cam_state.orientation); + dC0pij_dGpij = R_w_c0; + dC1pij_dGpij = R_c0_c1 * R_w_c0; - //orientation takes camera frame to world frame, we wa - dh_dGpij = dh_dCpij * dCpij_dGpij; + dC0pij_dXplj.leftCols(3) = skewSymmetric(p_c0); + dC0pij_dXplj.rightCols(3) = -R_w_c0; - //dh / d X_{pl} - dCpij_dCGtheta = skewSymmetric(p_c0); - dCpij_dGpC = -quaternionToRotation(cam_state.orientation); - dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta; - dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC; // d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; // Isometry T_anchor_w takes a vector in anchor frame to world frame - dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); + dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(N*N-1)/2].y/(rho*rho), 1/(rho*rho)); dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() - * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), - feature.anchorPatch_ideal[count].y/(rho), + * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho), + feature.anchorPatch_ideal[(N*N-1)/2].y/(rho), 1/(rho))); dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); // Intermediate Jakobians - H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1 - H_plj = dI_dhj * dh_dXplj; // 1 x 6 - H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 + H_rhoj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_drhoj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_drhoj; // 4 x 1 + H_plj = dI_dhj * dh_dC0pij * dC0pij_dXplj + dI_dhj * dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6 + H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 // check if point nullspaceable if (H_rhoj(0, 0) != 0) @@ -1701,26 +1739,25 @@ bool MsckfVio::PhotometricMeasurementJacobian( VectorXd r_photo = VectorXd::Zero(N*N); // one line of the NxN Jacobians - Eigen::Matrix H_rhoj; - Eigen::Matrix H_plj; - Eigen::Matrix H_pAj; + Eigen::Matrix H_rhoj; + Eigen::Matrix H_plj; + Eigen::Matrix H_pAj; - Eigen::MatrixXd dI_dh(N*N, 2); + Eigen::MatrixXd dI_dh(2* N*N, 4); // combined Jacobians - Eigen::MatrixXd H_rho(N*N, 1); - Eigen::MatrixXd H_pl(N*N, 6); - Eigen::MatrixXd H_pA(N*N, 6); - - auto frame = cam0.moving_window.find(cam_state_id)->second.image; + Eigen::MatrixXd H_rho(2 * N*N, 1); + Eigen::MatrixXd H_pl(2 * N*N, 6); + Eigen::MatrixXd H_pA(2 * N*N, 6); // calcualte residual of patch PhotometricPatchPointResidual(cam_state_id, feature, r_photo); + cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; bool valid = false; - Matrix dI_dhj;// = Matrix::Zero(); + Matrix dI_dhj;// = Matrix::Zero(); for (auto point : feature.anchorPatch_3d) { // get jacobi of single point in patch @@ -1728,11 +1765,11 @@ bool MsckfVio::PhotometricMeasurementJacobian( valid = true; // stack point into entire jacobi - H_rho.block<1, 1>(count, 0) = H_rhoj; - H_pl.block<1, 6>(count, 0) = H_plj; - H_pA.block<1, 6>(count, 0) = H_pAj; + H_rho.block<2, 1>(count*2, 0) = H_rhoj; + H_pl.block<2, 6>(count*2, 0) = H_plj; + H_pA.block<2, 6>(count*2, 0) = H_pAj; - dI_dh.block<1, 2>(count, 0) = dI_dhj; + dI_dh.block<2, 4>(count*2, 0) = dI_dhj; count++; } @@ -1741,8 +1778,8 @@ bool MsckfVio::PhotometricMeasurementJacobian( //cout << "h photo: \n" << h_photo << endl; // construct the jacobian structure needed for nullspacing - MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(N*N, 1); + MatrixXd H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7); + MatrixXd H_yl = MatrixXd::Zero(2*N*N, 1); ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); @@ -1788,7 +1825,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); // set anchor Jakobi - H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = H_pA; + H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*N*N, 6) = H_pA; //get position of current frame in cam states auto cam_state_iter = state_server.cam_states.find(cam_state_id); @@ -1796,7 +1833,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); // set jakobi of state - H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = H_pl; + H_xl.block(0, 21+cam_state_cntr*7, 2*N*N, 6) = H_pl; // set ones for irradiance bias // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); @@ -1819,7 +1856,6 @@ bool MsckfVio::PhotometricFeatureJacobian( MatrixXd& H_x, VectorXd& r) { - return false; const auto& feature = map_server[feature_id]; // Check how many camera states in the provided camera @@ -1835,7 +1871,7 @@ bool MsckfVio::PhotometricFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = N * N * valid_cam_state_ids.size(); + jacobian_row_size = 2 * N * N * valid_cam_state_ids.size(); MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, 21+state_server.cam_states.size()*7); @@ -1847,7 +1883,7 @@ bool MsckfVio::PhotometricFeatureJacobian( MatrixXd H_xl; MatrixXd H_yl; - Eigen::VectorXd r_l = VectorXd::Zero(N*N); + Eigen::VectorXd r_l = VectorXd::Zero(2*N*N); if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) continue; @@ -1855,15 +1891,16 @@ bool MsckfVio::PhotometricFeatureJacobian( auto cam_state_iter = state_server.cam_states.find(cam_id); int cam_state_cntr = std::distance( state_server.cam_states.begin(), cam_state_iter); - + // Stack the Jacobians. H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl; H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl; - r_i.segment(stack_cntr, N*N) = r_l; - stack_cntr += N*N; + r_i.segment(stack_cntr, 2*N*N) = r_l; + stack_cntr += 2*N*N; } + // if not enough to propper nullspace (in paper implementation) - if(stack_cntr < N*N) + if(stack_cntr < 2*N*N) return false; // Project the residual and Jacobians onto the nullspace @@ -2263,10 +2300,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { if (H.rows() == 0 || r.rows() == 0) - { - cout << "zero" << endl; return; - } // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -2316,7 +2350,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // Update the IMU state. if (FILTER != 2) return; - cout << "two: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + //cout << "two: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); @@ -2377,8 +2411,11 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { - if (H.rows() == 0 || r.rows() == 0) + if (H.rows() == 0 || r.rows() == 0) + { + cout << "zero" << endl; return; + } // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -2423,7 +2460,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r VectorXd delta_x = K * r; // Update the IMU state. - // cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; if (FILTER != 1) return; @@ -2504,7 +2541,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - //return true; + return true; MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * @@ -2571,7 +2608,7 @@ void MsckfVio::removeLostFeatures() { } } - pjacobian_row_size += N*N*feature.observations.size(); + pjacobian_row_size += 2*N*N*feature.observations.size(); twojacobian_row_size += 4*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; @@ -2640,7 +2677,6 @@ void MsckfVio::removeLostFeatures() { stack_cntr += H_xj.rows(); } if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { - cout << "passed" << endl; twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); @@ -2769,7 +2805,7 @@ void MsckfVio::pruneLastCamStateBuffer() } } - pjacobian_row_size += N*N*feature.observations.size(); + pjacobian_row_size += 2*N*N*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; twojacobian_row_size += 4*feature.observations.size(); @@ -2827,7 +2863,6 @@ void MsckfVio::pruneLastCamStateBuffer() } if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { - cout << "passed" << endl; twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); @@ -2954,7 +2989,7 @@ void MsckfVio::pruneCamStateBuffer() { } twojacobian_row_size += 4*involved_cam_state_ids.size(); - pjacobian_row_size += N*N*involved_cam_state_ids.size(); + pjacobian_row_size += 2*N*N*involved_cam_state_ids.size(); jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -3010,7 +3045,6 @@ void MsckfVio::pruneCamStateBuffer() { } if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { - cout << "passed" << endl; twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); From 1a07ba3d3c85b5ce84506fb671119c5614128115 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 5 Jul 2019 09:43:35 +0200 Subject: [PATCH 77/86] added larger sobel filter in calculation - converges sometimes for a few seconds --- launch/msckf_vio_tinytum.launch | 4 +- launch/msckf_vio_tum.launch | 4 +- src/msckf_vio.cpp | 99 +++++++++++++++++++++++---------- 3 files changed, 73 insertions(+), 34 deletions(-) diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 28ae944..d5de69a 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -25,7 +25,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index b7e9365..7b1b86a 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 5e3dd8c..016ef8b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -50,7 +50,7 @@ Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity(); // Static member variables in Feature class. FeatureIDType Feature::next_id = 0; -double Feature::observation_noise = 0.01; +double Feature::observation_noise = 0.05; Feature::OptimizationConfig Feature::optimization_config; map MsckfVio::chi_squared_test_table; @@ -312,7 +312,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 1000; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.05); + boost::math::quantile(chi_squared_dist, 0.1); } if (!createRosIO()) return false; @@ -555,19 +555,21 @@ void MsckfVio::manageMovingWindow( cv::Mat deeper_frame; cam0_img_ptr->image.convertTo(deeper_frame,CV_16S); - cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); - cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); - xder/=8.; - yder/=8.; + cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); + xder/=72.; + yder/=72.; cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); - cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); - cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); - xder/=8.; - yder/=8.; + cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); + xder/=72.; + yder/=72.; + + cvWaitKey(0); cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); @@ -1679,7 +1681,7 @@ bool MsckfVio::PhotometricPatchPointJacobian( dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0]; dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1]; - cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; + //cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; // add jacobian @@ -1718,6 +1720,10 @@ bool MsckfVio::PhotometricPatchPointJacobian( H_plj = dI_dhj * dh_dC0pij * dC0pij_dXplj + dI_dhj * dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6 H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 + // check if any direction not large enough for eval + if(dI_dhj(0, 0) < 0.01 or dI_dhj(0, 1) < 0.01 or dI_dhj(1, 2) < 0.01 or dI_dhj(1, 3) < 0.01) + return false; + // check if point nullspaceable if (H_rhoj(0, 0) != 0) return true; @@ -1751,9 +1757,11 @@ bool MsckfVio::PhotometricMeasurementJacobian( Eigen::MatrixXd H_pA(2 * N*N, 6); // calcualte residual of patch - PhotometricPatchPointResidual(cam_state_id, feature, r_photo); + if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) + return false; - cout << "r\n" << r_photo << endl; + + //cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; bool valid = false; @@ -2446,7 +2454,6 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r r_thin = r; } - // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; MatrixXd S = H_thin*P*H_thin.transpose() + @@ -2536,12 +2543,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r MatrixXd state_cov_fixed = (state_server.state_cov + state_server.state_cov.transpose()) / 2.0; state_server.state_cov = state_cov_fixed; + return; } bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - return true; + MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * @@ -2549,12 +2557,40 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - //cout << "gate: " << dof << " " << gamma << " " << - //chi_squared_test_table[dof] << endl; + // cout << "r" << r << endl; + // cout << "procov" << P1+P2 << endl; + cout << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; + + if(gamma > 1000000) + { + cout << " logging " << endl; + ofstream myfile; + myfile.open("/home/raphael/dev/octave/log2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: H\n" + << "# type: matrix\n" + << "# rows: " << H.rows() << "\n" + << "# columns: " << H.cols() << "\n" + << H << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << 1 << "\n" + << r << endl; + + myfile << "# name: C\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.cols() << "\n" + << state_server.state_cov << endl; + myfile.close(); + } if (chi_squared_test_table[dof] == 0) return false; - if (gamma < chi_squared_test_table[dof]) { + if (gamma < chi_squared_test_table[dof]*10) { // cout << "passed" << endl; return true; } else { @@ -2661,13 +2697,14 @@ void MsckfVio::removeLostFeatures() { if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) { if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { - //cout << "passed" << endl; + cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } } + /* featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2681,6 +2718,7 @@ void MsckfVio::removeLostFeatures() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } + */ // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. @@ -2695,7 +2733,7 @@ void MsckfVio::removeLostFeatures() { photometricMeasurementUpdate(pH_x, pr); } - + /* H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2705,7 +2743,7 @@ void MsckfVio::removeLostFeatures() { // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - + */ // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) map_server.erase(feature_id); @@ -2845,13 +2883,13 @@ void MsckfVio::pruneLastCamStateBuffer() if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { - //cout << "passed" << endl; + cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } } - + /* featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -2867,7 +2905,7 @@ void MsckfVio::pruneLastCamStateBuffer() twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - + */ for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2881,7 +2919,7 @@ void MsckfVio::pruneLastCamStateBuffer() photometricMeasurementUpdate(pH_x, pr); } - + /* H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2891,7 +2929,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - + */ //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -3024,17 +3062,17 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { - //cout << "passed" << endl; + cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } } + /* featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -3049,7 +3087,7 @@ void MsckfVio::pruneCamStateBuffer() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - + */ for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -3063,6 +3101,7 @@ void MsckfVio::pruneCamStateBuffer() { photometricMeasurementUpdate(pH_x, pr); } + /* H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -3072,7 +3111,7 @@ void MsckfVio::pruneCamStateBuffer() { // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - + */ //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), From a7c296ca3d14b7b639032421fcb6b9fc18244cfe Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 5 Jul 2019 13:51:58 +0200 Subject: [PATCH 78/86] removed dx filter, corrected jacobi calculation with bigger sobel (and correct division), removed scale for mahalanobis --- include/msckf_vio/feature.hpp | 3 +- include/msckf_vio/msckf_vio.h | 2 +- launch/msckf_vio_euroc.launch | 4 +- src/msckf_vio.cpp | 70 +++++++++++++++++++++-------------- 4 files changed, 48 insertions(+), 31 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index a2a3d63..17e0ce1 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -736,6 +736,7 @@ bool Feature::VisualizeKernel( cvWaitKey(0); } + bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, @@ -934,7 +935,7 @@ bool Feature::VisualizePatch( //cv::imwrite(loc.str(), cam0.featureVisu); cv::imshow("patch", cam0.featureVisu); - cvWaitKey(0); + cvWaitKey(1); } float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 06bad0f..3ffc586 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -261,7 +261,7 @@ class MsckfVio { void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); bool gatingTest(const Eigen::MatrixXd& H, - const Eigen::VectorXd&r, const int& dof); + const Eigen::VectorXd&r, const int& dof, int filter=0); void removeLostFeatures(); void findRedundantCamStates( std::vector& rm_cam_state_ids); diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index b818735..b57704e 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 016ef8b..ef8f76e 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -312,7 +312,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 1000; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.1); + boost::math::quantile(chi_squared_dist, 0.4); } if (!createRosIO()) return false; @@ -554,23 +554,34 @@ void MsckfVio::manageMovingWindow( cv::Mat yder; cv::Mat deeper_frame; + // generate derivative matrix for cam0 cam0_img_ptr->image.convertTo(deeper_frame,CV_16S); cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); - xder/=72.; - yder/=72.; + xder/=96.; + yder/=96.; + // save into moving window cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); + // generate derivative matrix for cam 1 cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); - xder/=72.; - yder/=72.; + xder/=96.; + yder/=96.; + + /* + cv::Mat norm_abs_xderImage; + cv::convertScaleAbs(xder, norm_abs_xderImage); + cv::normalize(norm_abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); + + cv::imshow("xder", norm_abs_xderImage); cvWaitKey(0); - + */ + // save into moving window cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); @@ -1721,9 +1732,9 @@ bool MsckfVio::PhotometricPatchPointJacobian( H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 // check if any direction not large enough for eval - if(dI_dhj(0, 0) < 0.01 or dI_dhj(0, 1) < 0.01 or dI_dhj(1, 2) < 0.01 or dI_dhj(1, 3) < 0.01) + /*if((dI_dhj(0, 0) < 0.1 or dI_dhj(0, 1) < 0.1) and (dI_dhj(1, 2) < 0.1 or dI_dhj(1, 3) < 0.1)) return false; - + */ // check if point nullspaceable if (H_rhoj(0, 0) != 0) return true; @@ -1766,11 +1777,15 @@ bool MsckfVio::PhotometricMeasurementJacobian( int count = 0; bool valid = false; Matrix dI_dhj;// = Matrix::Zero(); + int valid_count = 0; for (auto point : feature.anchorPatch_3d) { // get jacobi of single point in patch if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj)) + { + valid_count++; valid = true; + } // stack point into entire jacobi H_rho.block<2, 1>(count*2, 0) = H_rhoj; @@ -1781,7 +1796,7 @@ bool MsckfVio::PhotometricMeasurementJacobian( count++; } - + cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; //Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; //cout << "h photo: \n" << h_photo << endl; @@ -2548,7 +2563,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r } -bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { +bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, int filter) { MatrixXd P1 = H * state_server.state_cov * H.transpose(); @@ -2559,6 +2574,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) // cout << "r" << r << endl; // cout << "procov" << P1+P2 << endl; + if(filter == 1) cout << "gate: " << dof << " " << gamma << " " << chi_squared_test_table[dof] << endl; @@ -2588,9 +2604,9 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) myfile.close(); } - if (chi_squared_test_table[dof] == 0) + if (chi_squared_test_table[dof] == 0) return false; - if (gamma < chi_squared_test_table[dof]*10) { + if (gamma < chi_squared_test_table[dof]) { // cout << "passed" << endl; return true; } else { @@ -2696,7 +2712,7 @@ void MsckfVio::removeLostFeatures() { if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; @@ -2704,7 +2720,7 @@ void MsckfVio::removeLostFeatures() { } } - /* + featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2718,7 +2734,7 @@ void MsckfVio::removeLostFeatures() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - */ + // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. @@ -2733,7 +2749,7 @@ void MsckfVio::removeLostFeatures() { photometricMeasurementUpdate(pH_x, pr); } - /* + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2743,7 +2759,7 @@ void MsckfVio::removeLostFeatures() { // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - */ + // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) map_server.erase(feature_id); @@ -2882,14 +2898,14 @@ void MsckfVio::pruneLastCamStateBuffer() if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } } - /* + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -2905,7 +2921,7 @@ void MsckfVio::pruneLastCamStateBuffer() twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - */ + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2919,7 +2935,7 @@ void MsckfVio::pruneLastCamStateBuffer() photometricMeasurementUpdate(pH_x, pr); } - /* + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2929,7 +2945,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - */ + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -3064,7 +3080,7 @@ void MsckfVio::pruneCamStateBuffer() { if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// involved_cam_state_ids.size())) { cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; @@ -3072,7 +3088,7 @@ void MsckfVio::pruneCamStateBuffer() { } } - /* + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -3087,7 +3103,7 @@ void MsckfVio::pruneCamStateBuffer() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - */ + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -3101,7 +3117,7 @@ void MsckfVio::pruneCamStateBuffer() { photometricMeasurementUpdate(pH_x, pr); } - /* + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -3111,7 +3127,7 @@ void MsckfVio::pruneCamStateBuffer() { // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - */ + //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), From 1380ec426f049ef9d4de5c7bdbffb578e14ae7d1 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Tue, 9 Jul 2019 11:24:25 +0200 Subject: [PATCH 79/86] fixed minor error when not enough samples, edited feature ammount and patch size to make irradiance msckf more stable --- config/camchain-imucam-tum-scaled.yaml | 4 +- include/msckf_vio/feature.hpp | 10 +- launch/image_processor_tinytum.launch | 4 +- launch/msckf_vio_euroc.launch | 2 +- launch/msckf_vio_tinytum.launch | 4 +- src/msckf_vio.cpp | 132 +++++++++++++------------ 6 files changed, 79 insertions(+), 77 deletions(-) diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index f5ea7e8..4facacc 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] - distortion_model: pre-equidistant + distortion_model: equidistant intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] resolution: [512, 512] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] - distortion_model: pre-equidistant + distortion_model: equidistant intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] resolution: [512, 512] rostopic: /cam1/image_raw diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 17e0ce1..6c55861 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -208,7 +208,7 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, bool estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, std::vector& anchorPatch_estimate, IlluminationParameter& estimatedIllumination) const; @@ -549,7 +549,7 @@ return delta; bool Feature::estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, std::vector& anchorPatch_estimate, IlluminationParameter& estimated_illumination) const { @@ -558,11 +558,11 @@ bool Feature::estimate_FrameIrradiance( // muliply by a and add b of this frame auto anchor = observations.begin(); - if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end()) + if(cam.moving_window.find(anchor->first) == cam.moving_window.end()) return false; - double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms; - double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms; + double anchorExposureTime_ms = cam.moving_window.find(anchor->first)->second.exposureTime_ms; + double frameExposureTime_ms = cam.moving_window.find(cam_state_id)->second.exposureTime_ms; double a_A = anchorExposureTime_ms; diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 53dd99a..0e5985c 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -14,8 +14,8 @@ - - + + diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index b57704e..6237037 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -18,7 +18,7 @@ output="screen"> - + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index d5de69a..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -25,7 +25,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index ef8f76e..172fda2 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -277,9 +277,9 @@ bool MsckfVio::createRosIO() { truth_sub = nh.subscribe("ground_truth", 100, &MsckfVio::truthCallback, this); - cam0_img_sub.subscribe(nh, "cam0_image", 10); - cam1_img_sub.subscribe(nh, "cam1_image", 10); - feature_sub.subscribe(nh, "features", 10); + cam0_img_sub.subscribe(nh, "cam0_image", 100); + cam1_img_sub.subscribe(nh, "cam1_image", 100); + feature_sub.subscribe(nh, "features", 100); image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub); image_sub.registerCallback(&MsckfVio::imageCallback, this); @@ -312,7 +312,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 1000; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.4); + boost::math::quantile(chi_squared_dist, 0.05); } if (!createRosIO()) return false; @@ -703,7 +703,7 @@ bool MsckfVio::resetCallback( is_first_img = true; // Restart the subscribers. - imu_sub = nh.subscribe("imu", 100, + imu_sub = nh.subscribe("imu", 1000, &MsckfVio::imuCallback, this); truth_sub = nh.subscribe("ground_truth", 100, @@ -1563,25 +1563,28 @@ bool MsckfVio::PhotometricPatchPointResidual( const Feature& feature, VectorXd& r) { - VectorXd r_photo = VectorXd::Zero(2*N*N); int count = 0; const CAMState& cam_state = state_server.cam_states[cam_state_id]; //estimate photometric measurement std::vector estimate_irradiance; - std::vector estimate_photo_z; + IlluminationParameter estimated_illumination; + std::vector estimate_photo_z_c0, estimate_photo_z_c1; std::vector photo_z_c0, photo_z_c1; // estimate irradiance based on anchor frame - /* - IlluminationParameter estimated_illumination; + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); for (auto& estimate_irradiance_j : estimate_irradiance) - estimate_photo_z.push_back (estimate_irradiance_j);// * + estimate_photo_z_c0.push_back (estimate_irradiance_j);// * + //estimated_illumination.frame_gain * estimated_illumination.feature_gain + + //estimated_illumination.frame_bias + estimated_illumination.feature_bias); + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam1, estimate_irradiance, estimated_illumination); + for (auto& estimate_irradiance_j : estimate_irradiance) + estimate_photo_z_c1.push_back (estimate_irradiance_j);// * //estimated_illumination.frame_gain * estimated_illumination.feature_gain + //estimated_illumination.frame_bias + estimated_illumination.feature_bias); - */ // irradiance measurement around feature point in c0 and c1 std::vector true_irradiance_c0, true_irradiance_c1; @@ -1615,9 +1618,11 @@ bool MsckfVio::PhotometricPatchPointResidual( // add observation photo_z_c0.push_back(feature.PixelIrradiance(p_in_c0, current_image_c0)); photo_z_c1.push_back(feature.PixelIrradiance(p_in_c1, current_image_c1)); + // calculate photom. residual acc. to paper - // r_photo(count) = photo_z[count] - estimate_photo_z[count]; - + //r_photo(count*2) = photo_z_c0[count] - estimate_photo_z_c0[count]; + //r_photo(count*2+1) = photo_z_c1[count] - estimate_photo_z_c1[count]; + // calculate photom. residual alternating between frames r_photo(count*2) = true_irradiance_c0[count] - photo_z_c0[count]; r_photo(count*2+1) = true_irradiance_c1[count] - photo_z_c1[count]; @@ -1747,13 +1752,12 @@ bool MsckfVio::PhotometricMeasurementJacobian( const FeatureIDType& feature_id, MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) { - // Prepare all the required data. const CAMState& cam_state = state_server.cam_states[cam_state_id]; const Feature& feature = map_server[feature_id]; //photometric observation - VectorXd r_photo = VectorXd::Zero(N*N); + VectorXd r_photo; // one line of the NxN Jacobians Eigen::Matrix H_rhoj; @@ -1771,7 +1775,6 @@ bool MsckfVio::PhotometricMeasurementJacobian( if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) return false; - //cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; @@ -1796,13 +1799,13 @@ bool MsckfVio::PhotometricMeasurementJacobian( count++; } - cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; - //Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; - //cout << "h photo: \n" << h_photo << endl; + // cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; + // Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; + // cout << "h photo: \n" << h_photo << endl; // construct the jacobian structure needed for nullspacing - MatrixXd H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(2*N*N, 1); + MatrixXd H_xl; + MatrixXd H_yl; ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); @@ -1844,6 +1847,9 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, Eigen::MatrixXd& H_xl, Eigen::MatrixXd& H_yl) { + H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7); + H_yl = MatrixXd::Zero(2*N*N, 1); + // get position of anchor in cam states auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); @@ -1867,9 +1873,9 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, /*for(int i = 0; i< N*N; i++) H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; - */ + */ - H_yl = H_rho; + H_yl.block(0, 0, H_rho.rows(), H_rho.cols()) = H_rho; } @@ -1894,23 +1900,35 @@ bool MsckfVio::PhotometricFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = 2 * N * N * valid_cam_state_ids.size(); + + // stacked feature jacobians + MatrixXd H_xi; + MatrixXd H_yi; + VectorXd r_i; - MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, - 21+state_server.cam_states.size()*7); - MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); // CHANGED N*N+1 to 1 - VectorXd r_i = VectorXd::Zero(jacobian_row_size); + // temporary measurement jacobians + MatrixXd H_xl; + MatrixXd H_yl; + Eigen::VectorXd r_l; int stack_cntr = 0; + bool first = true; + // go through every valid state the feature was observed in for (const auto& cam_id : valid_cam_state_ids) { - MatrixXd H_xl; - MatrixXd H_yl; - Eigen::VectorXd r_l = VectorXd::Zero(2*N*N); - + // skip observation if measurement is not valid if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) continue; + // set size of stacking jacobians, once the returned jacobians are known + if(first) + { + first = not first; + jacobian_row_size = r_l.size() * valid_cam_state_ids.size(); + H_xi = MatrixXd::Zero(jacobian_row_size, H_xl.cols()); + H_yi = MatrixXd::Zero(jacobian_row_size, H_yl.cols()); // CHANGED N*N+1 to 1 + r_i = VectorXd::Zero(jacobian_row_size); + } auto cam_state_iter = state_server.cam_states.find(cam_id); int cam_state_cntr = std::distance( state_server.cam_states.begin(), cam_state_iter); @@ -1918,12 +1936,12 @@ bool MsckfVio::PhotometricFeatureJacobian( // Stack the Jacobians. H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl; H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl; - r_i.segment(stack_cntr, 2*N*N) = r_l; - stack_cntr += 2*N*N; + r_i.segment(stack_cntr, r_l.size()) = r_l; + stack_cntr += r_l.size(); } // if not enough to propper nullspace (in paper implementation) - if(stack_cntr < 2*N*N) + if(stack_cntr < r_l.size()) return false; // Project the residual and Jacobians onto the nullspace @@ -1936,6 +1954,7 @@ bool MsckfVio::PhotometricFeatureJacobian( if(H_yi(i,0) != 0) valid = true; + FullPivLU lu(H_yi.transpose()); MatrixXd A_null_space = lu.kernel(); @@ -2004,7 +2023,9 @@ bool MsckfVio::PhotometricFeatureJacobian( cout << "---------- LOGGED -------- " << endl; } - return true; + if(valid) + return true; + return false; } void MsckfVio::measurementJacobian( @@ -2435,10 +2456,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { if (H.rows() == 0 || r.rows() == 0) - { - cout << "zero" << endl; - return; - } + return; // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -2572,12 +2590,6 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - // cout << "r" << r << endl; - // cout << "procov" << P1+P2 << endl; - if(filter == 1) - cout << "gate: " << dof << " " << gamma << " " << - chi_squared_test_table[dof] << endl; - if(gamma > 1000000) { cout << " logging " << endl; @@ -2608,6 +2620,9 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, return false; if (gamma < chi_squared_test_table[dof]) { // cout << "passed" << endl; + if(filter == 1) + cout << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; return true; } else { // cout << "failed" << endl; @@ -2713,13 +2728,11 @@ void MsckfVio::removeLostFeatures() { if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { - cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } } - featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2735,7 +2748,6 @@ void MsckfVio::removeLostFeatures() { twostack_cntr += twoH_xj.rows(); } - // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. //if (stack_cntr > 1500) break; @@ -2745,11 +2757,8 @@ void MsckfVio::removeLostFeatures() { { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - photometricMeasurementUpdate(pH_x, pr); } - - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2898,8 +2907,8 @@ void MsckfVio::pruneLastCamStateBuffer() if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { - cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); @@ -2921,27 +2930,24 @@ void MsckfVio::pruneLastCamStateBuffer() twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } - if(pstack_cntr) { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); - + // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); @@ -3081,17 +3087,14 @@ void MsckfVio::pruneCamStateBuffer() { if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// involved_cam_state_ids.size())) { - cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } } - - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); - + if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; @@ -3104,11 +3107,11 @@ void MsckfVio::pruneCamStateBuffer() { twostack_cntr += twoH_xj.rows(); } + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); + } - - if(pstack_cntr) { @@ -3117,7 +3120,6 @@ void MsckfVio::pruneCamStateBuffer() { photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); From 02156bd89afd444071fb1b2a4ba01dd8db80c8c7 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 12 Jul 2019 14:01:11 +0200 Subject: [PATCH 80/86] changed visualization --- config/camchain-imucam-tum-scaled.yaml | 4 +-- include/msckf_vio/feature.hpp | 34 +++++++++++++------------- launch/image_processor_tinytum.launch | 10 ++++---- src/msckf_vio.cpp | 2 +- 4 files changed, 25 insertions(+), 25 deletions(-) diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index 4facacc..f5ea7e8 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] resolution: [512, 512] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] resolution: [512, 512] rostopic: /cam1/image_raw diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 6c55861..8d5c9c1 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -224,7 +224,7 @@ bool VisualizeKernel( bool VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, const Eigen::VectorXd& photo_r, std::stringstream& ss) const; /* @@ -740,7 +740,7 @@ bool Feature::VisualizeKernel( bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, const Eigen::VectorXd& photo_r, std::stringstream& ss) const { @@ -749,7 +749,7 @@ bool Feature::VisualizePatch( //visu - anchor auto anchor = observations.begin(); - cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image; + cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image; cv::Mat dottedFrame(anchorImage.size(), CV_8UC3); cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB); @@ -761,10 +761,10 @@ bool Feature::VisualizePatch( cv::Point ys(point.x, point.y); cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255)); } - cam0.featureVisu = dottedFrame.clone(); + cam.featureVisu = dottedFrame.clone(); // visu - feature - cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; + cv::Mat current_image = cam.moving_window.find(cam_state_id)->second.image; cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB); // set position in frame @@ -772,7 +772,7 @@ bool Feature::VisualizePatch( std::vector projectionPatch; for(auto point : anchorPatch_3d) { - cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point); projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image)); // visu - feature cv::Point xs(p_in_c0.x, p_in_c0.y); @@ -780,7 +780,7 @@ bool Feature::VisualizePatch( cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); } - cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); + cv::hconcat(cam.featureVisu, dottedFrame, cam.featureVisu); // patches visualization @@ -829,7 +829,7 @@ bool Feature::VisualizePatch( cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1)); // move to real pixels - p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); + p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); for(int i = 0; i0) + if(photo_r(2*(i*N+j))>0) cv::rectangle(irradianceFrame, cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), - cv::Scalar(255 - photo_r(i*N+j)*255, 255 - photo_r(i*N+j)*255, 255), + cv::Scalar(255 - photo_r(2*(i*N+j)+1)*255, 255 - photo_r(2*(i*N+j)+1)*255, 255), CV_FILLED); else cv::rectangle(irradianceFrame, cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), - cv::Scalar(255, 255 + photo_r(i*N+j)*255, 255 + photo_r(i*N+j)*255), + cv::Scalar(255, 255 + photo_r(2*(i*N+j)+1)*255, 255 + photo_r(2*(i*N+j)+1)*255), CV_FILLED); // gradient arrow @@ -884,7 +884,7 @@ bool Feature::VisualizePatch( 3); */ - cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); + cv::hconcat(cam.featureVisu, irradianceFrame, cam.featureVisu); /* // visualize position of used observations and resulting feature position @@ -916,15 +916,15 @@ bool Feature::VisualizePatch( // draw, x y position and arrow with direction - write z next to it - cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale); + cv::resize(cam.featureVisu, cam.featureVisu, cv::Size(), rescale, rescale); - cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu); + cv::hconcat(cam.featureVisu, positionFrame, cam.featureVisu); */ // write feature position std::stringstream pos_s; pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1); - cv::putText(cam0.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), + cv::putText(cam.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA); // create line? @@ -932,9 +932,9 @@ bool Feature::VisualizePatch( std::stringstream loc; // loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; - //cv::imwrite(loc.str(), cam0.featureVisu); + //cv::imwrite(loc.str(), cam.featureVisu); - cv::imshow("patch", cam0.featureVisu); + cv::imshow("patch", cam.featureVisu); cvWaitKey(1); } diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 0e5985c..f862755 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -14,8 +14,8 @@ - - + + @@ -24,9 +24,9 @@ - - - + + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 172fda2..00b2c99 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1828,7 +1828,7 @@ bool MsckfVio::PhotometricMeasurementJacobian( // visualizing functions feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); + feature.VisualizePatch(cam_state, cam_state_id, cam1, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } From 876fa7617cb8fc1365673602c1bbc328a6b3f5e7 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 12 Jul 2019 14:39:10 +0200 Subject: [PATCH 81/86] changed patch size --- launch/msckf_vio_tinytum.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 5e5e3b3..28ae944 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -25,7 +25,7 @@ - + From 5a80f97b68e54bed52f2971e8ad430a4a4ec201f Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 12 Jul 2019 16:36:36 +0200 Subject: [PATCH 82/86] fixed visualization issue --- config/camchain-imucam-euroc.yaml | 4 ++-- include/msckf_vio/feature.hpp | 3 +-- launch/image_processor_tinytum.launch | 6 +++--- launch/msckf_vio_euroc.launch | 4 ++-- src/image_processor.cpp | 12 ++++++++++-- src/msckf_vio.cpp | 16 ++++++++++------ 6 files changed, 28 insertions(+), 17 deletions(-) diff --git a/config/camchain-imucam-euroc.yaml b/config/camchain-imucam-euroc.yaml index de4c321..ad2572c 100644 --- a/config/camchain-imucam-euroc.yaml +++ b/config/camchain-imucam-euroc.yaml @@ -9,7 +9,7 @@ cam0: 0, 0, 0, 1.000000000000000] camera_model: pinhole distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] - distortion_model: radtan + distortion_model: pre-radtan intrinsics: [458.654, 457.296, 367.215, 248.375] resolution: [752, 480] timeshift_cam_imu: 0.0 @@ -26,7 +26,7 @@ cam1: 0, 0, 0, 1.000000000000000] camera_model: pinhole distortion_coeffs: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05] - distortion_model: radtan + distortion_model: pre-radtan intrinsics: [457.587, 456.134, 379.999, 255.238] resolution: [752, 480] timeshift_cam_imu: 0.0 diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 5cc997f..44f2a9a 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -823,11 +823,10 @@ bool Feature::VisualizePatch( cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); - cv::Point2f p_f(observations.find(cam_state_id)->second(0)*cam.intrinsics[0] + cam.intrinsics[2],observations.find(cam_state_id)->second(1)*cam.intrinsics[1] + cam.intrinsics[3]); + cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1)); // move to real pixels p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); - for(int i = 0; i - - - + + + diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 864f15e..8f59c97 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -19,11 +19,11 @@ - + - + diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 65acd43..9242b15 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -226,8 +226,16 @@ void ImageProcessor::stereoCallback( ros::Time start_time = ros::Time::now(); - image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); - image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + + cv::Mat new_cam0; + cv::Mat new_cam1; + + image_handler::undistortImage(cam0_curr_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + image_handler::undistortImage(cam1_curr_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + + new_cam0.copyTo(cam0_curr_img_ptr->image); + new_cam1.copyTo(cam1_curr_img_ptr->image); + //ROS_INFO("Publishing: %f", // (ros::Time::now()-start_time).toSec()); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index f6fa93f..c9cb9e9 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -544,13 +544,14 @@ void MsckfVio::manageMovingWindow( cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img, sensor_msgs::image_encodings::MONO8); - image_handler::undistortImage(cam0_img_ptr->image, cam0_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); - image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - + cv::Mat new_cam0; + cv::Mat new_cam1; + image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); // save image information into moving window - cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone(); - cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); + cam0.moving_window[state_server.imu_state.id].image = new_cam0.clone(); + cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone(); cv::Mat xder; cv::Mat yder; @@ -1704,7 +1705,8 @@ bool MsckfVio::PhotometricPatchPointJacobian( dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0]; dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1]; - //cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; + cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; + cout << dI_dhj(1, 2) << ", " << dI_dhj(1, 3) << endl; // add jacobian @@ -1782,6 +1784,8 @@ bool MsckfVio::PhotometricMeasurementJacobian( if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) return false; + cout << "-\n" << r_photo << "-" << endl; + //cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; From 0ef71b9220e77e5e5dfedab3c3c83a3aa39707fe Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 12 Jul 2019 18:02:29 +0200 Subject: [PATCH 83/86] fixed anchor initialization and added image resizing for incomming into msckf --- include/msckf_vio/feature.hpp | 16 ++++++++++------ launch/msckf_vio_euroc.launch | 4 ++-- src/msckf_vio.cpp | 36 +++++++++++++++++++++++++---------- 3 files changed, 38 insertions(+), 18 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 44f2a9a..0014809 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -823,7 +823,11 @@ bool Feature::VisualizePatch( cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); - cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1)); + cv::Point2f p_f; + if(cam.id == 0) + p_f = cv::Point2f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1)); + else if(cam.id == 1) + p_f = cv::Point2f(observations.find(cam_state_id)->second(2),observations.find(cam_state_id)->second(3)); // move to real pixels p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); @@ -1042,8 +1046,9 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) auto anchor = observations.begin(); if(cam.moving_window.find(anchor->first) == cam.moving_window.end()) + { return false; - + } cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image; cv::Mat anchorImage_deeper; anchorImage.convertTo(anchorImage_deeper,CV_16S); @@ -1066,17 +1071,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) // check if image has been pre-undistorted - if(cam.distortion_model.substr(0,3) == "pre-") + if(cam.distortion_model.substr(0,3) == "pre") { - std::cout << "is a pre" << std::endl; //project onto pixel plane undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]); - // create vector of patch in pixel plane for(double u_run = -n; u_run <= n; u_run++) for(double v_run = -n; v_run <= n; v_run++) anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run)); + //project back into u,v for(int i = 0; i < N*N; i++) anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1])); @@ -1112,7 +1116,7 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) for(auto point : anchorPatch_real) if(point.x - n < 0 || point.x + n >= cam.resolution(0)-1 || point.y - n < 0 || point.y + n >= cam.resolution(1)-1) return false; - + for(auto point : anchorPatch_real) anchorPatch.push_back(PixelIrradiance(point, anchorImage)); diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 8f59c97..9e8ef7a 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -19,11 +19,11 @@ - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index c9cb9e9..eb33e53 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -425,6 +425,10 @@ void MsckfVio::imageCallback( // the origin. if (is_first_img) { is_first_img = false; + cam0.intrinsics *=0.5; + cam1.intrinsics *=0.5; + cam0.resolution *=0.5; + cam1.resolution *=0.5; state_server.imu_state.time = feature_msg->header.stamp.toSec(); } static double max_processing_time = 0.0; @@ -546,12 +550,26 @@ void MsckfVio::manageMovingWindow( cv::Mat new_cam0; cv::Mat new_cam1; + cam0.intrinsics *=2; + cam1.intrinsics *=2; + cam0.resolution *=2; + cam1.resolution *=2; image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + cam0.intrinsics *=0.5; + cam1.intrinsics *=0.5; + cam0.resolution *=0.5; + cam1.resolution *=0.5; + //resize + + cv::Mat new_cam02; + cv::Mat new_cam12; + cv::resize(new_cam0, new_cam02, cv::Size(), 0.5, 0.5); + cv::resize(new_cam1, new_cam12, cv::Size(), 0.5, 0.5); // save image information into moving window - cam0.moving_window[state_server.imu_state.id].image = new_cam0.clone(); - cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone(); + cam0.moving_window[state_server.imu_state.id].image = new_cam02.clone(); + cam1.moving_window[state_server.imu_state.id].image = new_cam12.clone(); cv::Mat xder; cv::Mat yder; @@ -576,14 +594,15 @@ void MsckfVio::manageMovingWindow( yder/=96.; - /* +/* cv::Mat norm_abs_xderImage; cv::convertScaleAbs(xder, norm_abs_xderImage); cv::normalize(norm_abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); cv::imshow("xder", norm_abs_xderImage); cvWaitKey(0); - */ +*/ + // save into moving window cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); @@ -1705,9 +1724,6 @@ bool MsckfVio::PhotometricPatchPointJacobian( dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0]; dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1]; - cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; - cout << dI_dhj(1, 2) << ", " << dI_dhj(1, 3) << endl; - // add jacobian //dh / d{}^Cp_{ij} @@ -1784,8 +1800,6 @@ bool MsckfVio::PhotometricMeasurementJacobian( if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) return false; - cout << "-\n" << r_photo << "-" << endl; - //cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; @@ -1929,7 +1943,9 @@ bool MsckfVio::PhotometricFeatureJacobian( // skip observation if measurement is not valid if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) - continue; + { + continue; + } // set size of stacking jacobians, once the returned jacobians are known if(first) From e94d4a06b50b1069863351b44b5323375fda619d Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Tue, 16 Jul 2019 09:53:30 +0200 Subject: [PATCH 84/86] added image rescaling factor --- include/msckf_vio/msckf_vio.h | 3 ++- launch/image_processor_tinytum.launch | 6 +++--- launch/msckf_vio_euroc.launch | 7 +++--- launch/msckf_vio_tinytum.launch | 4 +++- src/msckf_vio.cpp | 31 +++++++++++++++------------ 5 files changed, 29 insertions(+), 22 deletions(-) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index c7fdcea..adbf6cc 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -286,7 +286,8 @@ class MsckfVio { // Patch size for Photometry int N; - + // Image rescale + int SCALE; // Chi squared test table. static std::map chi_squared_test_table; diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 26ceacd..ec463d9 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -27,9 +27,9 @@ - - - + + + diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 9e8ef7a..0df8102 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -19,14 +19,15 @@ - + - + - + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 28ae944..196c72e 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -25,7 +25,9 @@ - + + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index eb33e53..bfbe609 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -137,6 +137,8 @@ bool MsckfVio::loadParameters() { // Get the photometric patch size nh.param("patch_size_n", N, 3); + // Get rescaling factor for image + nh.param("image_scale", SCALE, 1); // get camera information (used for back projection) nh.param("cam0/distortion_model", @@ -425,10 +427,10 @@ void MsckfVio::imageCallback( // the origin. if (is_first_img) { is_first_img = false; - cam0.intrinsics *=0.5; - cam1.intrinsics *=0.5; - cam0.resolution *=0.5; - cam1.resolution *=0.5; + //cam0.intrinsics *=SCALE; + //cam1.intrinsics *=SCALE; + //cam0.resolution *=SCALE; + //cam1.resolution *=SCALE; state_server.imu_state.time = feature_msg->header.stamp.toSec(); } static double max_processing_time = 0.0; @@ -550,22 +552,23 @@ void MsckfVio::manageMovingWindow( cv::Mat new_cam0; cv::Mat new_cam1; - cam0.intrinsics *=2; - cam1.intrinsics *=2; - cam0.resolution *=2; - cam1.resolution *=2; + //cam0.intrinsics *=float(1./SCALE); + //cam1.intrinsics *=float(1./SCALE); + //cam0.resolution *=float(1./SCALE); + //cam1.resolution *=float(1./SCALE); image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - cam0.intrinsics *=0.5; - cam1.intrinsics *=0.5; - cam0.resolution *=0.5; - cam1.resolution *=0.5; + //cam0.intrinsics *=SCALE; + //cam1.intrinsics *=SCALE; + //cam0.resolution *=SCALE; + //cam1.resolution *=SCALE; //resize + int SCALE = 1;yy cv::Mat new_cam02; cv::Mat new_cam12; - cv::resize(new_cam0, new_cam02, cv::Size(), 0.5, 0.5); - cv::resize(new_cam1, new_cam12, cv::Size(), 0.5, 0.5); + cv::resize(new_cam0, new_cam02, cv::Size(), SCALE, SCALE); + cv::resize(new_cam1, new_cam12, cv::Size(), SCALE, SCALE); // save image information into moving window cam0.moving_window[state_server.imu_state.id].image = new_cam02.clone(); From ed2ba618287e2356e2fd1cb057587f2aa66ff8d9 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Wed, 17 Jul 2019 10:34:28 +0200 Subject: [PATCH 85/86] fixed anchor frame calcualtion --- include/msckf_vio/feature.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 5cc997f..f222a8e 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -1067,9 +1067,8 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) // check if image has been pre-undistorted - if(cam.distortion_model.substr(0,3) == "pre-") + if(cam.distortion_model.substr(0,3) == "pre") { - std::cout << "is a pre" << std::endl; //project onto pixel plane undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]); From a8090ca58a5a941f0dca222da30d5ff40849dc86 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 19 Jul 2019 17:20:10 +0200 Subject: [PATCH 86/86] added full switch --- include/msckf_vio/msckf_vio.h | 5 +- launch/image_processor_tinytum.launch | 8 +- launch/msckf_vio_tinytum.launch | 5 +- src/msckf_vio.cpp | 236 +++++++++++++++----------- 4 files changed, 142 insertions(+), 112 deletions(-) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index c7fdcea..5be54eb 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -202,7 +202,7 @@ class MsckfVio { Eigen::Vector4d& r); // This function computes the Jacobian of all measurements viewed // in the given camera states of this feature. - void featureJacobian( + bool featureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); @@ -246,7 +246,7 @@ class MsckfVio { Eigen::VectorXd& r); - void twodotFeatureJacobian( + bool twodotFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); @@ -283,6 +283,7 @@ class MsckfVio { bool nan_flag; bool play; double last_time_bound; + double time_offset; // Patch size for Photometry int N; diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index ec463d9..efe6e7c 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -18,7 +18,7 @@ - + @@ -27,9 +27,9 @@ - - - + + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 5e5e3b3..58e2ca2 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -17,8 +17,7 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - + @@ -33,7 +32,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index f6fa93f..d9e66f3 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -50,7 +50,7 @@ Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity(); // Static member variables in Feature class. FeatureIDType Feature::next_id = 0; -double Feature::observation_noise = 0.05; +double Feature::observation_noise = 0.01; Feature::OptimizationConfig Feature::optimization_config; map MsckfVio::chi_squared_test_table; @@ -425,6 +425,7 @@ void MsckfVio::imageCallback( // the origin. if (is_first_img) { is_first_img = false; + time_offset = feature_msg->header.stamp.toSec(); state_server.imu_state.time = feature_msg->header.stamp.toSec(); } static double max_processing_time = 0.0; @@ -786,7 +787,7 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) { void MsckfVio::calcErrorState() { - // imu error + // imu "error" err_state_server.imu_state.id = state_server.imu_state.id; err_state_server.imu_state.time = state_server.imu_state.time; @@ -832,9 +833,9 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { // Counter how many IMU msgs in the buffer are used. int used_truth_msg_cntr = 0; - + double truth_time; for (const auto& truth_msg : truth_msg_buffer) { - double truth_time = truth_msg.header.stamp.toSec(); + truth_time = truth_msg.header.stamp.toSec(); if (truth_time < true_state_server.imu_state.time) { ++used_truth_msg_cntr; continue; @@ -854,6 +855,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { // Execute process model. processTruthtoIMU(truth_time, m_rotation, m_translation); ++used_truth_msg_cntr; + } last_time_bound = time_bound; @@ -864,6 +866,31 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { truth_msg_buffer.erase(truth_msg_buffer.begin(), truth_msg_buffer.begin()+used_truth_msg_cntr); + std::ofstream logfile; + + int FileHandler; + char FileBuffer[1024]; + float load = 0; + + FileHandler = open("/proc/loadavg", O_RDONLY); + if( FileHandler >= 0) { + auto file = read(FileHandler, FileBuffer, sizeof(FileBuffer) - 1); + sscanf(FileBuffer, "%f", &load); + close(FileHandler); + } + + auto gt = true_state_server.imu_state.position; + auto gr = true_state_server.imu_state.orientation; + logfile.open("/home/raphael/dev/MSCKF_ws/bag/log.txt", std::ios_base::app); + //ros time, cpu load , ground truth, estimation, ros time + logfile << true_state_server.imu_state.time - time_offset << "; " << load << "; "; + logfile << gt[0] << "; " << gt[1] << "; " << gt[2] << "; " << gr[0] << "; " << gr[1] << "; " << gr[2] << "; " << gr[3] << ";"; + + // estimation + auto et = state_server.imu_state.position; + auto er = state_server.imu_state.orientation; + logfile << et[0] << "; " << et[1] << "; " << et[2] << "; " << er[0] << "; " << er[1] << "; " << er[2] << "; " << er[3] << "; \n" << endl;; + logfile.close(); /* // calculate change delta_position = state_server.imu_state.position - old_imu_state.position; @@ -1459,12 +1486,14 @@ void MsckfVio::twodotMeasurementJacobian( return; } -void MsckfVio::twodotFeatureJacobian( +bool MsckfVio::twodotFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { - + if(FILTER != 2) + return false; + const auto& feature = map_server[feature_id]; @@ -1561,7 +1590,7 @@ void MsckfVio::twodotFeatureJacobian( std::cout << "resume playback" << std::endl; nh.setParam("/play_bag", true); } - return; + return true; } @@ -1891,6 +1920,8 @@ bool MsckfVio::PhotometricFeatureJacobian( const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { + if(FILTER != 1) + return false; const auto& feature = map_server[feature_id]; @@ -2101,12 +2132,15 @@ void MsckfVio::measurementJacobian( return; } -void MsckfVio::featureJacobian( +bool MsckfVio::featureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { - // stop playing bagfile if printing images + + + if(FILTER != 0) + return false; const auto& feature = map_server[feature_id]; @@ -2239,7 +2273,7 @@ void MsckfVio::featureJacobian( myfile.close(); } - return; + return true; } @@ -2625,37 +2659,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - if(gamma > 1000000) - { - cout << " logging " << endl; - ofstream myfile; - myfile.open("/home/raphael/dev/octave/log2octave"); - myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" - << "# name: H\n" - << "# type: matrix\n" - << "# rows: " << H.rows() << "\n" - << "# columns: " << H.cols() << "\n" - << H << endl; - - myfile << "# name: r\n" - << "# type: matrix\n" - << "# rows: " << r.rows() << "\n" - << "# columns: " << 1 << "\n" - << r << endl; - - myfile << "# name: C\n" - << "# type: matrix\n" - << "# rows: " << state_server.state_cov.rows() << "\n" - << "# columns: " << state_server.state_cov.cols() << "\n" - << state_server.state_cov << endl; - myfile.close(); - } - if (chi_squared_test_table[dof] == 0) return false; if (gamma < chi_squared_test_table[dof]) { // cout << "passed" << endl; - if(filter == 1) cout << "gate: " << dof << " " << gamma << " " << chi_squared_test_table[dof] << endl; return true; @@ -2769,20 +2776,25 @@ void MsckfVio::removeLostFeatures() { } } - featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); - - if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { - H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; - r.segment(stack_cntr, r_j.rows()) = r_j; - stack_cntr += H_xj.rows(); - } - - if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { - twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; - twor.segment(twostack_cntr, twor_j.rows()) = twor_j; - twostack_cntr += twoH_xj.rows(); + if(featureJacobian(feature.id, cam_state_ids, H_xj, r_j)) + { + if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { + H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; + r.segment(stack_cntr, r_j.rows()) = r_j; + stack_cntr += H_xj.rows(); + } } + + if(twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j)) + { + if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + twor.segment(twostack_cntr, twor_j.rows()) = twor_j; + twostack_cntr += twoH_xj.rows(); + } + } + + // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. @@ -2795,16 +2807,22 @@ void MsckfVio::removeLostFeatures() { pr.conservativeResize(pstack_cntr); photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); - r.conservativeResize(stack_cntr); + + if(stack_cntr) + { + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + measurementUpdate(H_x, r); - twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); - twor.conservativeResize(twostack_cntr); - - // Perform the measurement update step. - measurementUpdate(H_x, r); - twoMeasurementUpdate(twoH_x, twor); - + } + if(twostack_cntr) + { + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); + twoMeasurementUpdate(twoH_x, twor); + } + + // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) map_server.erase(feature_id); @@ -2951,20 +2969,23 @@ void MsckfVio::pruneLastCamStateBuffer() } } - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); - + if(featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j)) + { if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { - H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; - r.segment(stack_cntr, r_j.rows()) = r_j; - stack_cntr += H_xj.rows(); - pruned_cntr++; + H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; + r.segment(stack_cntr, r_j.rows()) = r_j; + stack_cntr += H_xj.rows(); + pruned_cntr++; } - - if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { - twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; - twor.segment(twostack_cntr, twor_j.rows()) = twor_j; - twostack_cntr += twoH_xj.rows(); + } + + if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j)) + { + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + twor.segment(twostack_cntr, twor_j.rows()) = twor_j; + twostack_cntr += twoH_xj.rows(); + } } for (const auto& cam_id : involved_cam_state_ids) @@ -2978,16 +2999,20 @@ void MsckfVio::pruneLastCamStateBuffer() photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); - r.conservativeResize(stack_cntr); - - twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); - twor.conservativeResize(twostack_cntr); - - // Perform the measurement update step. - measurementUpdate(H_x, r); - twoMeasurementUpdate(twoH_x, twor); + if(stack_cntr) + { + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + measurementUpdate(H_x, r); + } + if(twostack_cntr) + { + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); + twoMeasurementUpdate(twoH_x, twor); + } + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -3128,21 +3153,22 @@ void MsckfVio::pruneCamStateBuffer() { pstack_cntr += pH_xj.rows(); } } - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); - - if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { - H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; - r.segment(stack_cntr, r_j.rows()) = r_j; - stack_cntr += H_xj.rows(); + if(featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j)) + { + if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { + H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; + r.segment(stack_cntr, r_j.rows()) = r_j; + stack_cntr += H_xj.rows(); + } } - - if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { - twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; - twor.segment(twostack_cntr, twor_j.rows()) = twor_j; - twostack_cntr += twoH_xj.rows(); + if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j)) + { + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + twor.segment(twostack_cntr, twor_j.rows()) = twor_j; + twostack_cntr += twoH_xj.rows(); + } } - for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); @@ -3156,16 +3182,20 @@ void MsckfVio::pruneCamStateBuffer() { photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); - r.conservativeResize(stack_cntr); - - twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); - twor.conservativeResize(twostack_cntr); - - // Perform the measurement update step. - measurementUpdate(H_x, r); - twoMeasurementUpdate(twoH_x, twor); - + if(stack_cntr) + { + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + measurementUpdate(H_x, r); + } + + if(stack_cntr) + { + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); + twoMeasurementUpdate(twoH_x, twor); + } + //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(),