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

added moving window structure, not yet done. added timestame sync for images and features detected

Browse Source
This commit is contained in:
Raphael Maenle 2019-04-10 18:36:11 +02:00
parent e6620a4ed4
commit 79cce26dad
8 changed files with 174 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/msckf_vio/feature.hpp
View File

@ -144,6 +144,8 @@ struct Feature {
Eigen::aligned_allocator<
std::pair<const StateIDType, Eigen::Vector4d> > > observations;
// NxN Patch of Anchor Image
std::vector<double> patch;
// 3d postion of the feature in the world frame.
Eigen::Vector3d position;

11
include/msckf_vio/image_processor.h
View File

@ -14,10 +14,6 @@
#include <opencv2/opencv.hpp>
#include <opencv2/video.hpp>
#include <opencv2/cudaoptflow.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudaarithm.hpp>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
@ -312,7 +308,7 @@ private:
const std::vector<unsigned char>& markers,
std::vector<T>& 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<GridFeatures> prev_features_ptr;
boost::shared_ptr<GridFeatures> 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;

37
include/msckf_vio/msckf_vio.h
View File

@ -14,10 +14,14 @@
#include <string>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <boost/shared_ptr.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/video.hpp>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Image.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <std_srvs/Trigger.h>
@ -27,6 +31,11 @@
#include "feature.hpp"
#include <msckf_vio/CameraMeasurement.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
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<sensor_msgs::Imu> imu_msg_buffer;
// Moving Window buffer
std::map<StateIDType, cv::Mat, std::less<StateIDType>,
Eigen::aligned_allocator<
std::pair<StateIDType, cv::Mat> > > cam0_moving_window;
std::map<StateIDType, cv::Mat, std::less<StateIDType>,
Eigen::aligned_allocator<
std::pair<StateIDType, cv::Mat> > > 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<sensor_msgs::Image> cam0_img_sub;
message_filters::Subscriber<sensor_msgs::Image> cam1_img_sub;
message_filters::Subscriber<CameraMeasurement> feature_sub;
message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, CameraMeasurement> image_sub;
// Frame id
std::string fixed_frame_id;
std::string child_frame_id;

7
include/msckf_vio/utils.h
View File

@ -11,9 +11,6 @@
#include <ros/ros.h>
#include <string>
#include <opencv2/core/core.hpp>
#include <opencv2/cudaoptflow.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudaarithm.hpp>
#include <Eigen/Geometry>
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<uchar>& vec);
void download(const cv::cuda::GpuMat& d_mat, std::vector<cv::Point2f>& vec);
Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh,
const std::string &field);

2
msg/CameraMeasurement.msg
View File

@ -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

38
src/image_processor.cpp
View File

@ -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]);

135
src/msckf_vio.cpp
View File

@ -53,6 +53,7 @@ map<int, double> 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,8 +469,9 @@ bool MsckfVio::resetCallback(
// Restart the subscribers.
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?
res.success = 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;

14
src/utils.cpp
View File

@ -11,20 +11,6 @@
namespace msckf_vio {
namespace utils {
void download(const cv::cuda::GpuMat& d_mat, std::vector<cv::Point2f>& 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<uchar>& 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;