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
Download Patch File Download Diff File Expand all files Collapse all files

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

137
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,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;

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;