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

full revert of master to original msckf. parallelization is now in cuda branch, photometic expansion in photometry branch

Browse Source
This commit is contained in:
Raphael Maenle 2019-04-17 16:52:21 +02:00
parent 9ded72a366
commit 4842c175a5
3 changed files with 7 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/msckf_vio/feature.hpp
View File

@ -144,8 +144,6 @@ 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;

37
include/msckf_vio/msckf_vio.h
View File

@ -14,14 +14,10 @@
#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>
@ -31,11 +27,6 @@
#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
@ -112,12 +103,6 @@ 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.
@ -141,11 +126,6 @@ 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(
@ -199,15 +179,6 @@ 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;
@ -235,18 +206,12 @@ 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;

135
src/msckf_vio.cpp
View File

@ -53,7 +53,6 @@ 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;
}
@ -187,16 +186,8 @@ 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);
feature_sub.subscribe(nh, "features", 10);
image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub);
image_sub.registerCallback(&MsckfVio::imageCallback, this);
feature_sub = nh.subscribe("features", 40,
&MsckfVio::featureCallback, this);
mocap_odom_sub = nh.subscribe("mocap_odom", 10,
&MsckfVio::mocapOdomCallback, this);
@ -254,119 +245,6 @@ 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.
@ -412,6 +290,7 @@ 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.
@ -469,9 +348,8 @@ 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;
@ -876,7 +754,6 @@ void MsckfVio::stateAugmentation(const double& time) {
return;
}
void MsckfVio::addFeatureObservations(
const CameraMeasurementConstPtr& msg) {
@ -1421,8 +1298,6 @@ 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;