1511 lines
52 KiB
C++
1511 lines
52 KiB
C++
/*
|
|
* COPYRIGHT AND PERMISSION NOTICE
|
|
* Penn Software MSCKF_VIO
|
|
* Copyright (C) 2017 The Trustees of the University of Pennsylvania
|
|
* All rights reserved.
|
|
*/
|
|
|
|
#include <iostream>
|
|
#include <algorithm>
|
|
#include <set>
|
|
#include <Eigen/Dense>
|
|
|
|
#include <sensor_msgs/image_encodings.h>
|
|
#include <random_numbers/random_numbers.h>
|
|
|
|
#include <msckf_vio/CameraMeasurement.h>
|
|
#include <msckf_vio/TrackingInfo.h>
|
|
#include <msckf_vio/image_processor.h>
|
|
|
|
using namespace std;
|
|
using namespace cv;
|
|
using namespace Eigen;
|
|
|
|
namespace msckf_vio {
|
|
ImageProcessor::ImageProcessor(ros::NodeHandle& n) :
|
|
nh(n),
|
|
is_first_img(true),
|
|
//img_transport(n),
|
|
stereo_sub(10),
|
|
prev_features_ptr(new GridFeatures()),
|
|
curr_features_ptr(new GridFeatures()) {
|
|
return;
|
|
}
|
|
|
|
ImageProcessor::~ImageProcessor() {
|
|
destroyAllWindows();
|
|
//ROS_INFO("Feature lifetime statistics:");
|
|
//featureLifetimeStatistics();
|
|
return;
|
|
}
|
|
|
|
bool ImageProcessor::loadParameters() {
|
|
// Camera calibration parameters
|
|
nh.param<string>("cam0/distortion_model",
|
|
cam0_distortion_model, string("radtan"));
|
|
nh.param<string>("cam1/distortion_model",
|
|
cam1_distortion_model, string("radtan"));
|
|
|
|
vector<int> 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<int> 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<double> 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<double> 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<double> 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<double> 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];
|
|
|
|
vector<double> cam0_extrinsics(16);
|
|
cv::Matx33d R_imu_cam0;
|
|
cv::Vec3d t_imu_cam0;
|
|
nh.getParam("cam0/T_cam_imu", cam0_extrinsics);
|
|
R_imu_cam0(0, 0) = cam0_extrinsics[0];
|
|
R_imu_cam0(0, 1) = cam0_extrinsics[1];
|
|
R_imu_cam0(0, 2) = cam0_extrinsics[2];
|
|
R_imu_cam0(1, 0) = cam0_extrinsics[4];
|
|
R_imu_cam0(1, 1) = cam0_extrinsics[5];
|
|
R_imu_cam0(1, 2) = cam0_extrinsics[6];
|
|
R_imu_cam0(2, 0) = cam0_extrinsics[8];
|
|
R_imu_cam0(2, 1) = cam0_extrinsics[9];
|
|
R_imu_cam0(2, 2) = cam0_extrinsics[10];
|
|
t_imu_cam0(0) = cam0_extrinsics[3];
|
|
t_imu_cam0(1) = cam0_extrinsics[7];
|
|
t_imu_cam0(2) = cam0_extrinsics[11];
|
|
|
|
R_cam0_imu = R_imu_cam0.t();
|
|
t_cam0_imu = -R_imu_cam0.t() * t_imu_cam0;
|
|
|
|
vector<double> cam1_extrinsics(16);
|
|
cv::Matx33d R_imu_cam1;
|
|
cv::Vec3d t_imu_cam1;
|
|
nh.getParam("cam1/T_cam_imu", cam1_extrinsics);
|
|
R_imu_cam1(0, 0) = cam1_extrinsics[0];
|
|
R_imu_cam1(0, 1) = cam1_extrinsics[1];
|
|
R_imu_cam1(0, 2) = cam1_extrinsics[2];
|
|
R_imu_cam1(1, 0) = cam1_extrinsics[4];
|
|
R_imu_cam1(1, 1) = cam1_extrinsics[5];
|
|
R_imu_cam1(1, 2) = cam1_extrinsics[6];
|
|
R_imu_cam1(2, 0) = cam1_extrinsics[8];
|
|
R_imu_cam1(2, 1) = cam1_extrinsics[9];
|
|
R_imu_cam1(2, 2) = cam1_extrinsics[10];
|
|
t_imu_cam1(0) = cam1_extrinsics[3];
|
|
t_imu_cam1(1) = cam1_extrinsics[7];
|
|
t_imu_cam1(2) = cam1_extrinsics[11];
|
|
|
|
R_cam1_imu = R_imu_cam1.t();
|
|
t_cam1_imu = -R_imu_cam1.t() * t_imu_cam1;
|
|
|
|
// Processor parameters
|
|
nh.param<int>("grid_row", processor_config.grid_row, 4);
|
|
nh.param<int>("grid_col", processor_config.grid_col, 4);
|
|
nh.param<int>("grid_min_feature_num",
|
|
processor_config.grid_min_feature_num, 2);
|
|
nh.param<int>("grid_max_feature_num",
|
|
processor_config.grid_max_feature_num, 4);
|
|
nh.param<int>("pyramid_levels",
|
|
processor_config.pyramid_levels, 3);
|
|
nh.param<int>("patch_size",
|
|
processor_config.patch_size, 31);
|
|
nh.param<int>("fast_threshold",
|
|
processor_config.fast_threshold, 20);
|
|
nh.param<int>("max_iteration",
|
|
processor_config.max_iteration, 30);
|
|
nh.param<double>("track_precision",
|
|
processor_config.track_precision, 0.01);
|
|
nh.param<double>("ransac_threshold",
|
|
processor_config.ransac_threshold, 3);
|
|
nh.param<double>("stereo_threshold",
|
|
processor_config.stereo_threshold, 3);
|
|
|
|
ROS_INFO("===========================================");
|
|
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());
|
|
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]);
|
|
|
|
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());
|
|
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]);
|
|
|
|
cout << R_imu_cam0 << endl;
|
|
cout << t_imu_cam0.t() << endl;
|
|
|
|
ROS_INFO("grid_row: %d",
|
|
processor_config.grid_row);
|
|
ROS_INFO("grid_col: %d",
|
|
processor_config.grid_col);
|
|
ROS_INFO("grid_min_feature_num: %d",
|
|
processor_config.grid_min_feature_num);
|
|
ROS_INFO("grid_max_feature_num: %d",
|
|
processor_config.grid_max_feature_num);
|
|
ROS_INFO("pyramid_levels: %d",
|
|
processor_config.pyramid_levels);
|
|
ROS_INFO("patch_size: %d",
|
|
processor_config.patch_size);
|
|
ROS_INFO("fast_threshold: %d",
|
|
processor_config.fast_threshold);
|
|
ROS_INFO("max_iteration: %d",
|
|
processor_config.max_iteration);
|
|
ROS_INFO("track_precision: %f",
|
|
processor_config.track_precision);
|
|
ROS_INFO("ransac_threshold: %f",
|
|
processor_config.ransac_threshold);
|
|
ROS_INFO("stereo_threshold: %f",
|
|
processor_config.stereo_threshold);
|
|
ROS_INFO("===========================================");
|
|
return true;
|
|
}
|
|
|
|
bool ImageProcessor::createRosIO() {
|
|
feature_pub = nh.advertise<CameraMeasurement>(
|
|
"features", 3);
|
|
tracking_info_pub = nh.advertise<TrackingInfo>(
|
|
"tracking_info", 1);
|
|
image_transport::ImageTransport it(nh);
|
|
debug_stereo_pub = it.advertise("debug_stereo_image", 1);
|
|
|
|
cam0_img_sub.subscribe(nh, "cam0_image", 10);
|
|
cam1_img_sub.subscribe(nh, "cam1_image", 10);
|
|
stereo_sub.connectInput(cam0_img_sub, cam1_img_sub);
|
|
stereo_sub.registerCallback(&ImageProcessor::stereoCallback, this);
|
|
imu_sub = nh.subscribe("imu", 50,
|
|
&ImageProcessor::imuCallback, this);
|
|
|
|
return true;
|
|
}
|
|
|
|
bool ImageProcessor::initialize() {
|
|
if (!loadParameters()) return false;
|
|
ROS_INFO("Finish loading ROS parameters...");
|
|
|
|
// Create feature detector.
|
|
detector_ptr = FastFeatureDetector::create(
|
|
processor_config.fast_threshold);
|
|
|
|
if (!createRosIO()) return false;
|
|
ROS_INFO("Finish creating ROS IO...");
|
|
|
|
return true;
|
|
}
|
|
|
|
void ImageProcessor::stereoCallback(
|
|
const sensor_msgs::ImageConstPtr& cam0_img,
|
|
const sensor_msgs::ImageConstPtr& cam1_img) {
|
|
|
|
//cout << "==================================" << endl;
|
|
|
|
// Get the current image.
|
|
cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
|
|
sensor_msgs::image_encodings::MONO8);
|
|
cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
|
|
sensor_msgs::image_encodings::MONO8);
|
|
|
|
// 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();
|
|
initializeFirstFrame();
|
|
//ROS_INFO("Detection time: %f",
|
|
// (ros::Time::now()-start_time).toSec());
|
|
is_first_img = false;
|
|
|
|
// Draw results.
|
|
start_time = ros::Time::now();
|
|
drawFeaturesStereo();
|
|
//ROS_INFO("Draw features: %f",
|
|
// (ros::Time::now()-start_time).toSec());
|
|
} else {
|
|
// Track the feature in the previous image.
|
|
ros::Time start_time = ros::Time::now();
|
|
trackFeatures();
|
|
//ROS_INFO("Tracking time: %f",
|
|
// (ros::Time::now()-start_time).toSec());
|
|
|
|
// Add new features into the current image.
|
|
start_time = ros::Time::now();
|
|
addNewFeatures();
|
|
//ROS_INFO("Addition time: %f",
|
|
// (ros::Time::now()-start_time).toSec());
|
|
|
|
// Add new features into the current image.
|
|
start_time = ros::Time::now();
|
|
pruneGridFeatures();
|
|
//ROS_INFO("Prune grid features: %f",
|
|
// (ros::Time::now()-start_time).toSec());
|
|
|
|
// Draw results.
|
|
start_time = ros::Time::now();
|
|
drawFeaturesStereo();
|
|
//ROS_INFO("Draw features: %f",
|
|
// (ros::Time::now()-start_time).toSec());
|
|
}
|
|
|
|
//ros::Time start_time = ros::Time::now();
|
|
//updateFeatureLifetime();
|
|
//ROS_INFO("Statistics: %f",
|
|
// (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 <
|
|
processor_config.grid_row*processor_config.grid_col; ++code) {
|
|
(*curr_features_ptr)[code] = vector<FeatureMetaData>(0);
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::imuCallback(
|
|
const sensor_msgs::ImuConstPtr& msg) {
|
|
// Wait for the first image to be set.
|
|
if (is_first_img) return;
|
|
imu_msg_buffer.push_back(*msg);
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::createImagePyramids() {
|
|
const Mat& curr_cam0_img = cam0_curr_img_ptr->image;
|
|
buildOpticalFlowPyramid(
|
|
curr_cam0_img, curr_cam0_pyramid_,
|
|
Size(processor_config.patch_size, processor_config.patch_size),
|
|
processor_config.pyramid_levels, true, BORDER_REFLECT_101,
|
|
BORDER_CONSTANT, false);
|
|
|
|
const Mat& curr_cam1_img = cam1_curr_img_ptr->image;
|
|
buildOpticalFlowPyramid(
|
|
curr_cam1_img, curr_cam1_pyramid_,
|
|
Size(processor_config.patch_size, processor_config.patch_size),
|
|
processor_config.pyramid_levels, true, BORDER_REFLECT_101,
|
|
BORDER_CONSTANT, false);
|
|
}
|
|
|
|
void ImageProcessor::initializeFirstFrame() {
|
|
// Size of each grid.
|
|
const Mat& img = cam0_curr_img_ptr->image;
|
|
static int grid_height = img.rows / processor_config.grid_row;
|
|
static int grid_width = img.cols / processor_config.grid_col;
|
|
|
|
// Detect new features on the frist image.
|
|
vector<KeyPoint> new_features(0);
|
|
detector_ptr->detect(img, new_features);
|
|
|
|
// Find the stereo matched points for the newly
|
|
// detected features.
|
|
vector<cv::Point2f> cam0_points(new_features.size());
|
|
for (int i = 0; i < new_features.size(); ++i)
|
|
cam0_points[i] = new_features[i].pt;
|
|
|
|
vector<cv::Point2f> cam1_points(0);
|
|
vector<unsigned char> inlier_markers(0);
|
|
stereoMatch(cam0_points, cam1_points, inlier_markers);
|
|
|
|
vector<cv::Point2f> cam0_inliers(0);
|
|
vector<cv::Point2f> cam1_inliers(0);
|
|
vector<float> response_inliers(0);
|
|
for (int i = 0; i < inlier_markers.size(); ++i) {
|
|
if (inlier_markers[i] == 0) continue;
|
|
cam0_inliers.push_back(cam0_points[i]);
|
|
cam1_inliers.push_back(cam1_points[i]);
|
|
response_inliers.push_back(new_features[i].response);
|
|
}
|
|
|
|
// Group the features into grids
|
|
GridFeatures grid_new_features;
|
|
for (int code = 0; code <
|
|
processor_config.grid_row*processor_config.grid_col; ++code)
|
|
grid_new_features[code] = vector<FeatureMetaData>(0);
|
|
|
|
for (int i = 0; i < cam0_inliers.size(); ++i) {
|
|
const cv::Point2f& cam0_point = cam0_inliers[i];
|
|
const cv::Point2f& cam1_point = cam1_inliers[i];
|
|
const float& response = response_inliers[i];
|
|
|
|
int row = static_cast<int>(cam0_point.y / grid_height);
|
|
int col = static_cast<int>(cam0_point.x / grid_width);
|
|
int code = row*processor_config.grid_col + col;
|
|
|
|
FeatureMetaData new_feature;
|
|
new_feature.response = response;
|
|
new_feature.cam0_point = cam0_point;
|
|
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(),
|
|
&ImageProcessor::featureCompareByResponse);
|
|
|
|
// Collect new features within each grid with high response.
|
|
for (int code = 0; code <
|
|
processor_config.grid_row*processor_config.grid_col; ++code) {
|
|
vector<FeatureMetaData>& features_this_grid = (*curr_features_ptr)[code];
|
|
vector<FeatureMetaData>& new_features_this_grid = grid_new_features[code];
|
|
|
|
for (int k = 0; k < processor_config.grid_min_feature_num &&
|
|
k < new_features_this_grid.size(); ++k) {
|
|
features_this_grid.push_back(new_features_this_grid[k]);
|
|
features_this_grid.back().id = next_feature_id++;
|
|
features_this_grid.back().lifetime = 1;
|
|
}
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::predictFeatureTracking(
|
|
const vector<cv::Point2f>& input_pts,
|
|
const cv::Matx33f& R_p_c,
|
|
const cv::Vec4d& intrinsics,
|
|
vector<cv::Point2f>& compensated_pts) {
|
|
|
|
// Return directly if there are no input features.
|
|
if (input_pts.size() == 0) {
|
|
compensated_pts.clear();
|
|
return;
|
|
}
|
|
compensated_pts.resize(input_pts.size());
|
|
|
|
// Intrinsic matrix.
|
|
cv::Matx33f K(
|
|
intrinsics[0], 0.0, intrinsics[2],
|
|
0.0, intrinsics[1], intrinsics[3],
|
|
0.0, 0.0, 1.0);
|
|
cv::Matx33f H = K * R_p_c * K.inv();
|
|
|
|
for (int i = 0; i < input_pts.size(); ++i) {
|
|
cv::Vec3f p1(input_pts[i].x, input_pts[i].y, 1.0f);
|
|
cv::Vec3f p2 = H * p1;
|
|
compensated_pts[i].x = p2[0] / p2[2];
|
|
compensated_pts[i].y = p2[1] / p2[2];
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::trackFeatures() {
|
|
// Size of each grid.
|
|
static int grid_height =
|
|
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;
|
|
Matx33f cam1_R_p_c;
|
|
integrateImuData(cam0_R_p_c, cam1_R_p_c);
|
|
|
|
// Organize the features in the previous image.
|
|
vector<FeatureIDType> prev_ids(0);
|
|
vector<int> prev_lifetime(0);
|
|
vector<Point2f> prev_cam0_points(0);
|
|
vector<Point2f> prev_cam1_points(0);
|
|
|
|
for (const auto& item : *prev_features_ptr) {
|
|
for (const auto& prev_feature : item.second) {
|
|
prev_ids.push_back(prev_feature.id);
|
|
prev_lifetime.push_back(prev_feature.lifetime);
|
|
prev_cam0_points.push_back(prev_feature.cam0_point);
|
|
prev_cam1_points.push_back(prev_feature.cam1_point);
|
|
}
|
|
}
|
|
|
|
// Number of the features before tracking.
|
|
before_tracking = prev_cam0_points.size();
|
|
|
|
// Abort tracking if there is no features in
|
|
// the previous frame.
|
|
if (prev_ids.size() == 0) return;
|
|
|
|
// Track features using LK optical flow method.
|
|
vector<Point2f> curr_cam0_points(0);
|
|
vector<unsigned char> track_inliers(0);
|
|
|
|
predictFeatureTracking(prev_cam0_points,
|
|
cam0_R_p_c, cam0_intrinsics, curr_cam0_points);
|
|
|
|
calcOpticalFlowPyrLK(
|
|
prev_cam0_pyramid_, curr_cam0_pyramid_,
|
|
prev_cam0_points, curr_cam0_points,
|
|
track_inliers, noArray(),
|
|
Size(processor_config.patch_size, processor_config.patch_size),
|
|
processor_config.pyramid_levels,
|
|
TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,
|
|
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 < curr_cam0_points.size(); ++i) {
|
|
if (track_inliers[i] == 0) continue;
|
|
if (curr_cam0_points[i].y < 0 ||
|
|
curr_cam0_points[i].y > cam0_curr_img_ptr->image.rows-1 ||
|
|
curr_cam0_points[i].x < 0 ||
|
|
curr_cam0_points[i].x > cam0_curr_img_ptr->image.cols-1)
|
|
track_inliers[i] = 0;
|
|
}
|
|
|
|
// Collect the tracked points.
|
|
vector<FeatureIDType> prev_tracked_ids(0);
|
|
vector<int> prev_tracked_lifetime(0);
|
|
vector<Point2f> prev_tracked_cam0_points(0);
|
|
vector<Point2f> prev_tracked_cam1_points(0);
|
|
vector<Point2f> curr_tracked_cam0_points(0);
|
|
|
|
removeUnmarkedElements(
|
|
prev_ids, track_inliers, prev_tracked_ids);
|
|
removeUnmarkedElements(
|
|
prev_lifetime, track_inliers, prev_tracked_lifetime);
|
|
removeUnmarkedElements(
|
|
prev_cam0_points, track_inliers, prev_tracked_cam0_points);
|
|
removeUnmarkedElements(
|
|
prev_cam1_points, track_inliers, prev_tracked_cam1_points);
|
|
removeUnmarkedElements(
|
|
curr_cam0_points, track_inliers, curr_tracked_cam0_points);
|
|
|
|
// Number of features left after tracking.
|
|
after_tracking = curr_tracked_cam0_points.size();
|
|
|
|
|
|
// Outlier removal involves three steps, which forms a close
|
|
// loop between the previous and current frames of cam0 (left)
|
|
// and cam1 (right). Assuming the stereo matching between the
|
|
// previous cam0 and cam1 images are correct, the three steps are:
|
|
//
|
|
// prev frames cam0 ----------> cam1
|
|
// | |
|
|
// |ransac |ransac
|
|
// | stereo match |
|
|
// curr frames cam0 ----------> cam1
|
|
//
|
|
// 1) Stereo matching between current images of cam0 and cam1.
|
|
// 2) RANSAC between previous and current images of cam0.
|
|
// 3) RANSAC between previous and current images of cam1.
|
|
//
|
|
// For Step 3, tracking between the images is no longer needed.
|
|
// The stereo matching results are directly used in the RANSAC.
|
|
|
|
// Step 1: stereo matching.
|
|
vector<Point2f> curr_cam1_points(0);
|
|
vector<unsigned char> match_inliers(0);
|
|
stereoMatch(curr_tracked_cam0_points, curr_cam1_points, match_inliers);
|
|
|
|
vector<FeatureIDType> prev_matched_ids(0);
|
|
vector<int> prev_matched_lifetime(0);
|
|
vector<Point2f> prev_matched_cam0_points(0);
|
|
vector<Point2f> prev_matched_cam1_points(0);
|
|
vector<Point2f> curr_matched_cam0_points(0);
|
|
vector<Point2f> curr_matched_cam1_points(0);
|
|
|
|
removeUnmarkedElements(
|
|
prev_tracked_ids, match_inliers, prev_matched_ids);
|
|
removeUnmarkedElements(
|
|
prev_tracked_lifetime, match_inliers, prev_matched_lifetime);
|
|
removeUnmarkedElements(
|
|
prev_tracked_cam0_points, match_inliers, prev_matched_cam0_points);
|
|
removeUnmarkedElements(
|
|
prev_tracked_cam1_points, match_inliers, prev_matched_cam1_points);
|
|
removeUnmarkedElements(
|
|
curr_tracked_cam0_points, match_inliers, curr_matched_cam0_points);
|
|
removeUnmarkedElements(
|
|
curr_cam1_points, match_inliers, curr_matched_cam1_points);
|
|
|
|
// Number of features left after stereo matching.
|
|
after_matching = curr_matched_cam0_points.size();
|
|
|
|
// Step 2 and 3: RANSAC on temporal image pairs of cam0 and cam1.
|
|
vector<int> 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,
|
|
0.99, cam0_ransac_inliers);
|
|
|
|
vector<int> 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,
|
|
0.99, cam1_ransac_inliers);
|
|
|
|
// Number of features after ransac.
|
|
after_ransac = 0;
|
|
|
|
for (int i = 0; i < cam0_ransac_inliers.size(); ++i) {
|
|
if (cam0_ransac_inliers[i] == 0 ||
|
|
cam1_ransac_inliers[i] == 0) continue;
|
|
int row = static_cast<int>(
|
|
curr_matched_cam0_points[i].y / grid_height);
|
|
int col = static_cast<int>(
|
|
curr_matched_cam0_points[i].x / grid_width);
|
|
int code = row*processor_config.grid_col + col;
|
|
(*curr_features_ptr)[code].push_back(FeatureMetaData());
|
|
|
|
FeatureMetaData& grid_new_feature = (*curr_features_ptr)[code].back();
|
|
grid_new_feature.id = prev_matched_ids[i];
|
|
grid_new_feature.lifetime = ++prev_matched_lifetime[i];
|
|
grid_new_feature.cam0_point = curr_matched_cam0_points[i];
|
|
grid_new_feature.cam1_point = curr_matched_cam1_points[i];
|
|
|
|
++after_ransac;
|
|
}
|
|
|
|
// Compute the tracking rate.
|
|
int prev_feature_num = 0;
|
|
for (const auto& item : *prev_features_ptr)
|
|
prev_feature_num += item.second.size();
|
|
|
|
int curr_feature_num = 0;
|
|
for (const auto& item : *curr_features_ptr)
|
|
curr_feature_num += item.second.size();
|
|
|
|
ROS_INFO_THROTTLE(0.5,
|
|
"\033[0;32m candidates: %d; track: %d; match: %d; ransac: %d/%d=%f\033[0m",
|
|
before_tracking, after_tracking, after_matching,
|
|
curr_feature_num, prev_feature_num,
|
|
static_cast<double>(curr_feature_num)/
|
|
(static_cast<double>(prev_feature_num)+1e-5));
|
|
//printf(
|
|
// "\033[0;32m candidates: %d; raw track: %d; stereo match: %d; ransac: %d/%d=%f\033[0m\n",
|
|
// before_tracking, after_tracking, after_matching,
|
|
// curr_feature_num, prev_feature_num,
|
|
// static_cast<double>(curr_feature_num)/
|
|
// (static_cast<double>(prev_feature_num)+1e-5));
|
|
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::stereoMatch(
|
|
const vector<cv::Point2f>& cam0_points,
|
|
vector<cv::Point2f>& cam1_points,
|
|
vector<unsigned char>& inlier_markers) {
|
|
|
|
if (cam0_points.size() == 0) return;
|
|
|
|
if(cam1_points.size() == 0) {
|
|
// Initialize cam1_points by projecting cam0_points to cam1 using the
|
|
// rotation from stereo extrinsics
|
|
const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu;
|
|
vector<cv::Point2f> cam0_points_undistorted;
|
|
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_distortion_model, cam1_distortion_coeffs);
|
|
}
|
|
|
|
// Track features using LK optical flow method.
|
|
calcOpticalFlowPyrLK(curr_cam0_pyramid_, curr_cam1_pyramid_,
|
|
cam0_points, cam1_points,
|
|
inlier_markers, noArray(),
|
|
Size(processor_config.patch_size, processor_config.patch_size),
|
|
processor_config.pyramid_levels,
|
|
TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,
|
|
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) {
|
|
if (inlier_markers[i] == 0) continue;
|
|
if (cam1_points[i].y < 0 ||
|
|
cam1_points[i].y > cam1_curr_img_ptr->image.rows-1 ||
|
|
cam1_points[i].x < 0 ||
|
|
cam1_points[i].x > cam1_curr_img_ptr->image.cols-1)
|
|
inlier_markers[i] = 0;
|
|
}
|
|
|
|
// Compute the relative rotation between the cam0
|
|
// frame and cam1 frame.
|
|
const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu;
|
|
const cv::Vec3d t_cam0_cam1 = R_cam1_imu.t() * (t_cam0_imu-t_cam1_imu);
|
|
// Compute the essential matrix.
|
|
const cv::Matx33d t_cam0_cam1_hat(
|
|
0.0, -t_cam0_cam1[2], t_cam0_cam1[1],
|
|
t_cam0_cam1[2], 0.0, -t_cam0_cam1[0],
|
|
-t_cam0_cam1[1], t_cam0_cam1[0], 0.0);
|
|
const cv::Matx33d E = t_cam0_cam1_hat * R_cam0_cam1;
|
|
|
|
// Further remove outliers based on the known
|
|
// essential matrix.
|
|
vector<cv::Point2f> cam0_points_undistorted(0);
|
|
vector<cv::Point2f> cam1_points_undistorted(0);
|
|
undistortPoints(
|
|
cam0_points, cam0_intrinsics, cam0_distortion_model,
|
|
cam0_distortion_coeffs, cam0_points_undistorted);
|
|
undistortPoints(
|
|
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]);
|
|
|
|
for (int i = 0; i < cam0_points_undistorted.size(); ++i) {
|
|
if (inlier_markers[i] == 0) continue;
|
|
cv::Vec3d pt0(cam0_points_undistorted[i].x,
|
|
cam0_points_undistorted[i].y, 1.0);
|
|
cv::Vec3d pt1(cam1_points_undistorted[i].x,
|
|
cam1_points_undistorted[i].y, 1.0);
|
|
cv::Vec3d epipolar_line = E * pt0;
|
|
double error = fabs((pt1.t() * epipolar_line)[0]) / sqrt(
|
|
epipolar_line[0]*epipolar_line[0]+
|
|
epipolar_line[1]*epipolar_line[1]);
|
|
if (error > processor_config.stereo_threshold*norm_pixel_unit)
|
|
inlier_markers[i] = 0;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::addNewFeatures() {
|
|
const Mat& curr_img = cam0_curr_img_ptr->image;
|
|
|
|
// Size of each grid.
|
|
static int grid_height =
|
|
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) {
|
|
for (const auto& feature : features.second) {
|
|
const int y = static_cast<int>(feature.cam0_point.y);
|
|
const int x = static_cast<int>(feature.cam0_point.x);
|
|
|
|
int up_lim = y-2, bottom_lim = y+3,
|
|
left_lim = x-2, right_lim = x+3;
|
|
if (up_lim < 0) up_lim = 0;
|
|
if (bottom_lim > curr_img.rows) bottom_lim = curr_img.rows;
|
|
if (left_lim < 0) left_lim = 0;
|
|
if (right_lim > curr_img.cols) right_lim = curr_img.cols;
|
|
|
|
Range row_range(up_lim, bottom_lim);
|
|
Range col_range(left_lim, right_lim);
|
|
mask(row_range, col_range) = 0;
|
|
}
|
|
}
|
|
|
|
// Detect new features.
|
|
vector<KeyPoint> new_features(0);
|
|
detector_ptr->detect(curr_img, new_features, mask);
|
|
|
|
// Collect the new detected features based on the grid.
|
|
// Select the ones with top response within each grid afterwards.
|
|
vector<vector<KeyPoint> > new_feature_sieve(
|
|
processor_config.grid_row*processor_config.grid_col);
|
|
for (const auto& feature : new_features) {
|
|
int row = static_cast<int>(feature.pt.y / grid_height);
|
|
int col = static_cast<int>(feature.pt.x / grid_width);
|
|
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) {
|
|
std::sort(item.begin(), item.end(),
|
|
&ImageProcessor::keyPointCompareByResponse);
|
|
item.erase(
|
|
item.begin()+processor_config.grid_max_feature_num, item.end());
|
|
}
|
|
new_features.insert(new_features.end(), item.begin(), item.end());
|
|
}
|
|
|
|
int detected_new_features = new_features.size();
|
|
|
|
// Find the stereo matched points for the newly
|
|
// detected features.
|
|
vector<cv::Point2f> cam0_points(new_features.size());
|
|
for (int i = 0; i < new_features.size(); ++i)
|
|
cam0_points[i] = new_features[i].pt;
|
|
|
|
vector<cv::Point2f> cam1_points(0);
|
|
vector<unsigned char> inlier_markers(0);
|
|
stereoMatch(cam0_points, cam1_points, inlier_markers);
|
|
|
|
vector<cv::Point2f> cam0_inliers(0);
|
|
vector<cv::Point2f> cam1_inliers(0);
|
|
vector<float> response_inliers(0);
|
|
for (int i = 0; i < inlier_markers.size(); ++i) {
|
|
if (inlier_markers[i] == 0) continue;
|
|
cam0_inliers.push_back(cam0_points[i]);
|
|
cam1_inliers.push_back(cam1_points[i]);
|
|
response_inliers.push_back(new_features[i].response);
|
|
}
|
|
|
|
int matched_new_features = cam0_inliers.size();
|
|
|
|
if (matched_new_features < 5 &&
|
|
static_cast<double>(matched_new_features)/
|
|
static_cast<double>(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 <
|
|
processor_config.grid_row*processor_config.grid_col; ++code)
|
|
grid_new_features[code] = vector<FeatureMetaData>(0);
|
|
|
|
for (int i = 0; i < cam0_inliers.size(); ++i) {
|
|
const cv::Point2f& cam0_point = cam0_inliers[i];
|
|
const cv::Point2f& cam1_point = cam1_inliers[i];
|
|
const float& response = response_inliers[i];
|
|
|
|
int row = static_cast<int>(cam0_point.y / grid_height);
|
|
int col = static_cast<int>(cam0_point.x / grid_width);
|
|
int code = row*processor_config.grid_col + col;
|
|
|
|
FeatureMetaData new_feature;
|
|
new_feature.response = response;
|
|
new_feature.cam0_point = cam0_point;
|
|
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(),
|
|
&ImageProcessor::featureCompareByResponse);
|
|
|
|
int new_added_feature_num = 0;
|
|
// Collect new features within each grid with high response.
|
|
for (int code = 0; code <
|
|
processor_config.grid_row*processor_config.grid_col; ++code) {
|
|
vector<FeatureMetaData>& features_this_grid = (*curr_features_ptr)[code];
|
|
vector<FeatureMetaData>& new_features_this_grid = grid_new_features[code];
|
|
|
|
if (features_this_grid.size() >=
|
|
processor_config.grid_min_feature_num) continue;
|
|
|
|
int vacancy_num = processor_config.grid_min_feature_num -
|
|
features_this_grid.size();
|
|
for (int k = 0;
|
|
k < vacancy_num && k < new_features_this_grid.size(); ++k) {
|
|
features_this_grid.push_back(new_features_this_grid[k]);
|
|
features_this_grid.back().id = next_feature_id++;
|
|
features_this_grid.back().lifetime = 1;
|
|
|
|
++new_added_feature_num;
|
|
}
|
|
}
|
|
|
|
//printf("\033[0;33m detected: %d; matched: %d; new added feature: %d\033[0m\n",
|
|
// detected_new_features, matched_new_features, new_added_feature_num);
|
|
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::pruneGridFeatures() {
|
|
for (auto& item : *curr_features_ptr) {
|
|
auto& grid_features = item.second;
|
|
// Continue if the number of features in this grid does
|
|
// not exceed the upper bound.
|
|
if (grid_features.size() <=
|
|
processor_config.grid_max_feature_num) continue;
|
|
std::sort(grid_features.begin(), grid_features.end(),
|
|
&ImageProcessor::featureCompareByLifetime);
|
|
grid_features.erase(grid_features.begin()+
|
|
processor_config.grid_max_feature_num,
|
|
grid_features.end());
|
|
}
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::undistortPoints(
|
|
const vector<cv::Point2f>& pts_in,
|
|
const cv::Vec4d& intrinsics,
|
|
const string& distortion_model,
|
|
const cv::Vec4d& distortion_coeffs,
|
|
vector<cv::Point2f>& 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<cv::Point2f> ImageProcessor::distortPoints(
|
|
const vector<cv::Point2f>& 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<cv::Point2f> pts_out;
|
|
if (distortion_model == "radtan") {
|
|
vector<cv::Point3f> 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<cv::Point3f> 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.
|
|
auto begin_iter = imu_msg_buffer.begin();
|
|
while (begin_iter != imu_msg_buffer.end()) {
|
|
if ((begin_iter->header.stamp-
|
|
cam0_prev_img_ptr->header.stamp).toSec() < -0.01)
|
|
++begin_iter;
|
|
else
|
|
break;
|
|
}
|
|
|
|
auto end_iter = begin_iter;
|
|
while (end_iter != imu_msg_buffer.end()) {
|
|
if ((end_iter->header.stamp-
|
|
cam0_curr_img_ptr->header.stamp).toSec() < 0.005)
|
|
++end_iter;
|
|
else
|
|
break;
|
|
}
|
|
|
|
// Compute the mean angular velocity in the IMU frame.
|
|
Vec3f mean_ang_vel(0.0, 0.0, 0.0);
|
|
for (auto iter = begin_iter; iter < end_iter; ++iter)
|
|
mean_ang_vel += Vec3f(iter->angular_velocity.x,
|
|
iter->angular_velocity.y, iter->angular_velocity.z);
|
|
|
|
if (end_iter-begin_iter > 0)
|
|
mean_ang_vel *= 1.0f / (end_iter-begin_iter);
|
|
|
|
// Transform the mean angular velocity from the IMU
|
|
// frame to the cam0 and cam1 frames.
|
|
Vec3f cam0_mean_ang_vel = R_cam0_imu.t() * mean_ang_vel;
|
|
Vec3f cam1_mean_ang_vel = R_cam1_imu.t() * mean_ang_vel;
|
|
|
|
// Compute the relative rotation.
|
|
double dtime = (cam0_curr_img_ptr->header.stamp-
|
|
cam0_prev_img_ptr->header.stamp).toSec();
|
|
Rodrigues(cam0_mean_ang_vel*dtime, cam0_R_p_c);
|
|
Rodrigues(cam1_mean_ang_vel*dtime, cam1_R_p_c);
|
|
cam0_R_p_c = cam0_R_p_c.t();
|
|
cam1_R_p_c = cam1_R_p_c.t();
|
|
|
|
// Delete the useless and used imu messages.
|
|
imu_msg_buffer.erase(imu_msg_buffer.begin(), end_iter);
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::rescalePoints(
|
|
vector<Point2f>& pts1, vector<Point2f>& pts2,
|
|
float& scaling_factor) {
|
|
|
|
scaling_factor = 0.0f;
|
|
|
|
for (int i = 0; i < pts1.size(); ++i) {
|
|
scaling_factor += sqrt(pts1[i].dot(pts1[i]));
|
|
scaling_factor += sqrt(pts2[i].dot(pts2[i]));
|
|
}
|
|
|
|
scaling_factor = (pts1.size()+pts2.size()) /
|
|
scaling_factor * sqrt(2.0f);
|
|
|
|
for (int i = 0; i < pts1.size(); ++i) {
|
|
pts1[i] *= scaling_factor;
|
|
pts2[i] *= scaling_factor;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::twoPointRansac(
|
|
const vector<Point2f>& pts1, const vector<Point2f>& pts2,
|
|
const cv::Matx33f& R_p_c, const cv::Vec4d& intrinsics,
|
|
const std::string& distortion_model,
|
|
const cv::Vec4d& distortion_coeffs,
|
|
const double& inlier_error,
|
|
const double& success_probability,
|
|
vector<int>& inlier_markers) {
|
|
|
|
// Check the size of input point size.
|
|
if (pts1.size() != pts2.size())
|
|
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]);
|
|
int iter_num = static_cast<int>(
|
|
ceil(log(1-success_probability) / log(1-0.7*0.7)));
|
|
|
|
// Initially, mark all points as inliers.
|
|
inlier_markers.clear();
|
|
inlier_markers.resize(pts1.size(), 1);
|
|
|
|
// Undistort all the points.
|
|
vector<Point2f> pts1_undistorted(pts1.size());
|
|
vector<Point2f> pts2_undistorted(pts2.size());
|
|
undistortPoints(
|
|
pts1, intrinsics, distortion_model,
|
|
distortion_coeffs, pts1_undistorted);
|
|
undistortPoints(
|
|
pts2, intrinsics, distortion_model,
|
|
distortion_coeffs, pts2_undistorted);
|
|
|
|
// Compenstate the points in the previous image with
|
|
// the relative rotation.
|
|
for (auto& pt : pts1_undistorted) {
|
|
Vec3f pt_h(pt.x, pt.y, 1.0f);
|
|
//Vec3f pt_hc = dR * pt_h;
|
|
Vec3f pt_hc = R_p_c * pt_h;
|
|
pt.x = pt_hc[0];
|
|
pt.y = pt_hc[1];
|
|
}
|
|
|
|
// Normalize the points to gain numerical stability.
|
|
float scaling_factor = 0.0f;
|
|
rescalePoints(pts1_undistorted, pts2_undistorted, scaling_factor);
|
|
norm_pixel_unit *= scaling_factor;
|
|
|
|
// Compute the difference between previous and current points,
|
|
// which will be used frequently later.
|
|
vector<Point2d> pts_diff(pts1_undistorted.size());
|
|
for (int i = 0; i < pts1_undistorted.size(); ++i)
|
|
pts_diff[i] = pts1_undistorted[i] - pts2_undistorted[i];
|
|
|
|
// Mark the point pairs with large difference directly.
|
|
// BTW, the mean distance of the rest of the point pairs
|
|
// are computed.
|
|
double mean_pt_distance = 0.0;
|
|
int raw_inlier_cntr = 0;
|
|
for (int i = 0; i < pts_diff.size(); ++i) {
|
|
double distance = sqrt(pts_diff[i].dot(pts_diff[i]));
|
|
// 25 pixel distance is a pretty large tolerance for normal motion.
|
|
// However, to be used with aggressive motion, this tolerance should
|
|
// be increased significantly to match the usage.
|
|
if (distance > 50.0*norm_pixel_unit) {
|
|
inlier_markers[i] = 0;
|
|
} else {
|
|
mean_pt_distance += distance;
|
|
++raw_inlier_cntr;
|
|
}
|
|
}
|
|
mean_pt_distance /= raw_inlier_cntr;
|
|
|
|
// If the current number of inliers is less than 3, just mark
|
|
// all input as outliers. This case can happen with fast
|
|
// rotation where very few features are tracked.
|
|
if (raw_inlier_cntr < 3) {
|
|
for (auto& marker : inlier_markers) marker = 0;
|
|
return;
|
|
}
|
|
|
|
// Before doing 2-point RANSAC, we have to check if the motion
|
|
// is degenerated, meaning that there is no translation between
|
|
// the frames, in which case, the model of the RANSAC does not
|
|
// work. If so, the distance between the matched points will
|
|
// be almost 0.
|
|
//if (mean_pt_distance < inlier_error*norm_pixel_unit) {
|
|
if (mean_pt_distance < norm_pixel_unit) {
|
|
//ROS_WARN_THROTTLE(1.0, "Degenerated motion...");
|
|
for (int i = 0; i < pts_diff.size(); ++i) {
|
|
if (inlier_markers[i] == 0) continue;
|
|
if (sqrt(pts_diff[i].dot(pts_diff[i])) >
|
|
inlier_error*norm_pixel_unit)
|
|
inlier_markers[i] = 0;
|
|
}
|
|
return;
|
|
}
|
|
|
|
// In the case of general motion, the RANSAC model can be applied.
|
|
// The three column corresponds to tx, ty, and tz respectively.
|
|
MatrixXd coeff_t(pts_diff.size(), 3);
|
|
for (int i = 0; i < pts_diff.size(); ++i) {
|
|
coeff_t(i, 0) = pts_diff[i].y;
|
|
coeff_t(i, 1) = -pts_diff[i].x;
|
|
coeff_t(i, 2) = pts1_undistorted[i].x*pts2_undistorted[i].y -
|
|
pts1_undistorted[i].y*pts2_undistorted[i].x;
|
|
}
|
|
|
|
vector<int> raw_inlier_idx;
|
|
for (int i = 0; i < inlier_markers.size(); ++i) {
|
|
if (inlier_markers[i] != 0)
|
|
raw_inlier_idx.push_back(i);
|
|
}
|
|
|
|
vector<int> best_inlier_set;
|
|
double best_error = 1e10;
|
|
random_numbers::RandomNumberGenerator random_gen;
|
|
|
|
for (int iter_idx = 0; iter_idx < iter_num; ++iter_idx) {
|
|
// Randomly select two point pairs.
|
|
// Although this is a weird way of selecting two pairs, but it
|
|
// is able to efficiently avoid selecting repetitive pairs.
|
|
int pair_idx1 = raw_inlier_idx[random_gen.uniformInteger(
|
|
0, raw_inlier_idx.size()-1)];
|
|
int idx_diff = random_gen.uniformInteger(
|
|
1, raw_inlier_idx.size()-1);
|
|
int pair_idx2 = pair_idx1+idx_diff < raw_inlier_idx.size() ?
|
|
pair_idx1+idx_diff : pair_idx1+idx_diff-raw_inlier_idx.size();
|
|
|
|
// Construct the model;
|
|
Vector2d coeff_tx(coeff_t(pair_idx1, 0), coeff_t(pair_idx2, 0));
|
|
Vector2d coeff_ty(coeff_t(pair_idx1, 1), coeff_t(pair_idx2, 1));
|
|
Vector2d coeff_tz(coeff_t(pair_idx1, 2), coeff_t(pair_idx2, 2));
|
|
vector<double> coeff_l1_norm(3);
|
|
coeff_l1_norm[0] = coeff_tx.lpNorm<1>();
|
|
coeff_l1_norm[1] = coeff_ty.lpNorm<1>();
|
|
coeff_l1_norm[2] = coeff_tz.lpNorm<1>();
|
|
int base_indicator = min_element(coeff_l1_norm.begin(),
|
|
coeff_l1_norm.end())-coeff_l1_norm.begin();
|
|
|
|
Vector3d model(0.0, 0.0, 0.0);
|
|
if (base_indicator == 0) {
|
|
Matrix2d A;
|
|
A << coeff_ty, coeff_tz;
|
|
Vector2d solution = A.inverse() * (-coeff_tx);
|
|
model(0) = 1.0;
|
|
model(1) = solution(0);
|
|
model(2) = solution(1);
|
|
} else if (base_indicator ==1) {
|
|
Matrix2d A;
|
|
A << coeff_tx, coeff_tz;
|
|
Vector2d solution = A.inverse() * (-coeff_ty);
|
|
model(0) = solution(0);
|
|
model(1) = 1.0;
|
|
model(2) = solution(1);
|
|
} else {
|
|
Matrix2d A;
|
|
A << coeff_tx, coeff_ty;
|
|
Vector2d solution = A.inverse() * (-coeff_tz);
|
|
model(0) = solution(0);
|
|
model(1) = solution(1);
|
|
model(2) = 1.0;
|
|
}
|
|
|
|
// Find all the inliers among point pairs.
|
|
VectorXd error = coeff_t * model;
|
|
|
|
vector<int> inlier_set;
|
|
for (int i = 0; i < error.rows(); ++i) {
|
|
if (inlier_markers[i] == 0) continue;
|
|
if (std::abs(error(i)) < inlier_error*norm_pixel_unit)
|
|
inlier_set.push_back(i);
|
|
}
|
|
|
|
// If the number of inliers is small, the current
|
|
// model is probably wrong.
|
|
if (inlier_set.size() < 0.2*pts1_undistorted.size())
|
|
continue;
|
|
|
|
// Refit the model using all of the possible inliers.
|
|
VectorXd coeff_tx_better(inlier_set.size());
|
|
VectorXd coeff_ty_better(inlier_set.size());
|
|
VectorXd coeff_tz_better(inlier_set.size());
|
|
for (int i = 0; i < inlier_set.size(); ++i) {
|
|
coeff_tx_better(i) = coeff_t(inlier_set[i], 0);
|
|
coeff_ty_better(i) = coeff_t(inlier_set[i], 1);
|
|
coeff_tz_better(i) = coeff_t(inlier_set[i], 2);
|
|
}
|
|
|
|
Vector3d model_better(0.0, 0.0, 0.0);
|
|
if (base_indicator == 0) {
|
|
MatrixXd A(inlier_set.size(), 2);
|
|
A << coeff_ty_better, coeff_tz_better;
|
|
Vector2d solution =
|
|
(A.transpose() * A).inverse() * A.transpose() * (-coeff_tx_better);
|
|
model_better(0) = 1.0;
|
|
model_better(1) = solution(0);
|
|
model_better(2) = solution(1);
|
|
} else if (base_indicator ==1) {
|
|
MatrixXd A(inlier_set.size(), 2);
|
|
A << coeff_tx_better, coeff_tz_better;
|
|
Vector2d solution =
|
|
(A.transpose() * A).inverse() * A.transpose() * (-coeff_ty_better);
|
|
model_better(0) = solution(0);
|
|
model_better(1) = 1.0;
|
|
model_better(2) = solution(1);
|
|
} else {
|
|
MatrixXd A(inlier_set.size(), 2);
|
|
A << coeff_tx_better, coeff_ty_better;
|
|
Vector2d solution =
|
|
(A.transpose() * A).inverse() * A.transpose() * (-coeff_tz_better);
|
|
model_better(0) = solution(0);
|
|
model_better(1) = solution(1);
|
|
model_better(2) = 1.0;
|
|
}
|
|
|
|
// Compute the error and upate the best model if possible.
|
|
VectorXd new_error = coeff_t * model_better;
|
|
|
|
double this_error = 0.0;
|
|
for (const auto& inlier_idx : inlier_set)
|
|
this_error += std::abs(new_error(inlier_idx));
|
|
this_error /= inlier_set.size();
|
|
|
|
if (inlier_set.size() > best_inlier_set.size()) {
|
|
best_error = this_error;
|
|
best_inlier_set = inlier_set;
|
|
}
|
|
}
|
|
|
|
// Fill in the markers.
|
|
inlier_markers.clear();
|
|
inlier_markers.resize(pts1.size(), 0);
|
|
for (const auto& inlier_idx : best_inlier_set)
|
|
inlier_markers[inlier_idx] = 1;
|
|
|
|
//printf("inlier ratio: %lu/%lu\n",
|
|
// best_inlier_set.size(), inlier_markers.size());
|
|
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::publish() {
|
|
|
|
// Publish features.
|
|
CameraMeasurementPtr feature_msg_ptr(new CameraMeasurement);
|
|
feature_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;
|
|
|
|
vector<FeatureIDType> curr_ids(0);
|
|
vector<Point2f> curr_cam0_points(0);
|
|
vector<Point2f> curr_cam1_points(0);
|
|
|
|
for (const auto& grid_features : (*curr_features_ptr)) {
|
|
for (const auto& feature : grid_features.second) {
|
|
curr_ids.push_back(feature.id);
|
|
curr_cam0_points.push_back(feature.cam0_point);
|
|
curr_cam1_points.push_back(feature.cam1_point);
|
|
}
|
|
}
|
|
|
|
vector<Point2f> curr_cam0_points_undistorted(0);
|
|
vector<Point2f> curr_cam1_points_undistorted(0);
|
|
|
|
undistortPoints(
|
|
curr_cam0_points, cam0_intrinsics, cam0_distortion_model,
|
|
cam0_distortion_coeffs, curr_cam0_points_undistorted);
|
|
undistortPoints(
|
|
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());
|
|
feature_msg_ptr->features[i].id = curr_ids[i];
|
|
feature_msg_ptr->features[i].u0 = curr_cam0_points_undistorted[i].x;
|
|
feature_msg_ptr->features[i].v0 = curr_cam0_points_undistorted[i].y;
|
|
feature_msg_ptr->features[i].u1 = curr_cam1_points_undistorted[i].x;
|
|
feature_msg_ptr->features[i].v1 = curr_cam1_points_undistorted[i].y;
|
|
}
|
|
|
|
feature_pub.publish(feature_msg_ptr);
|
|
|
|
// Publish tracking info.
|
|
TrackingInfoPtr tracking_info_msg_ptr(new TrackingInfo());
|
|
tracking_info_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;
|
|
tracking_info_msg_ptr->before_tracking = before_tracking;
|
|
tracking_info_msg_ptr->after_tracking = after_tracking;
|
|
tracking_info_msg_ptr->after_matching = after_matching;
|
|
tracking_info_msg_ptr->after_ransac = after_ransac;
|
|
tracking_info_pub.publish(tracking_info_msg_ptr);
|
|
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::drawFeaturesMono() {
|
|
// Colors for different features.
|
|
Scalar tracked(0, 255, 0);
|
|
Scalar new_feature(0, 255, 255);
|
|
|
|
static int grid_height =
|
|
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 an output image.
|
|
int img_height = cam0_curr_img_ptr->image.rows;
|
|
int img_width = cam0_curr_img_ptr->image.cols;
|
|
Mat out_img(img_height, img_width, CV_8UC3);
|
|
cvtColor(cam0_curr_img_ptr->image, out_img, CV_GRAY2RGB);
|
|
|
|
// Draw grids on the image.
|
|
for (int i = 1; i < processor_config.grid_row; ++i) {
|
|
Point pt1(0, i*grid_height);
|
|
Point pt2(img_width, i*grid_height);
|
|
line(out_img, pt1, pt2, Scalar(255, 0, 0));
|
|
}
|
|
for (int i = 1; i < processor_config.grid_col; ++i) {
|
|
Point pt1(i*grid_width, 0);
|
|
Point pt2(i*grid_width, img_height);
|
|
line(out_img, pt1, pt2, Scalar(255, 0, 0));
|
|
}
|
|
|
|
// Collect features ids in the previous frame.
|
|
vector<FeatureIDType> prev_ids(0);
|
|
for (const auto& grid_features : *prev_features_ptr)
|
|
for (const auto& feature : grid_features.second)
|
|
prev_ids.push_back(feature.id);
|
|
|
|
// Collect feature points in the previous frame.
|
|
map<FeatureIDType, Point2f> prev_points;
|
|
for (const auto& grid_features : *prev_features_ptr)
|
|
for (const auto& feature : grid_features.second)
|
|
prev_points[feature.id] = feature.cam0_point;
|
|
|
|
// Collect feature points in the current frame.
|
|
map<FeatureIDType, Point2f> curr_points;
|
|
for (const auto& grid_features : *curr_features_ptr)
|
|
for (const auto& feature : grid_features.second)
|
|
curr_points[feature.id] = feature.cam0_point;
|
|
|
|
// Draw tracked features.
|
|
for (const auto& id : prev_ids) {
|
|
if (prev_points.find(id) != prev_points.end() &&
|
|
curr_points.find(id) != curr_points.end()) {
|
|
cv::Point2f prev_pt = prev_points[id];
|
|
cv::Point2f curr_pt = curr_points[id];
|
|
circle(out_img, curr_pt, 3, tracked);
|
|
line(out_img, prev_pt, curr_pt, tracked, 1);
|
|
|
|
prev_points.erase(id);
|
|
curr_points.erase(id);
|
|
}
|
|
}
|
|
|
|
// Draw new features.
|
|
for (const auto& new_curr_point : curr_points) {
|
|
cv::Point2f pt = new_curr_point.second;
|
|
circle(out_img, pt, 3, new_feature, -1);
|
|
}
|
|
|
|
imshow("Feature", out_img);
|
|
waitKey(5);
|
|
}
|
|
|
|
void ImageProcessor::drawFeaturesStereo() {
|
|
|
|
if(debug_stereo_pub.getNumSubscribers() > 0)
|
|
{
|
|
// Colors for different features.
|
|
Scalar tracked(0, 255, 0);
|
|
Scalar new_feature(0, 255, 255);
|
|
|
|
static int grid_height =
|
|
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 an output image.
|
|
int img_height = cam0_curr_img_ptr->image.rows;
|
|
int img_width = cam0_curr_img_ptr->image.cols;
|
|
Mat out_img(img_height, img_width*2, CV_8UC3);
|
|
cvtColor(cam0_curr_img_ptr->image,
|
|
out_img.colRange(0, img_width), CV_GRAY2RGB);
|
|
cvtColor(cam1_curr_img_ptr->image,
|
|
out_img.colRange(img_width, img_width*2), CV_GRAY2RGB);
|
|
|
|
// Draw grids on the image.
|
|
for (int i = 1; i < processor_config.grid_row; ++i) {
|
|
Point pt1(0, i*grid_height);
|
|
Point pt2(img_width*2, i*grid_height);
|
|
line(out_img, pt1, pt2, Scalar(255, 0, 0));
|
|
}
|
|
for (int i = 1; i < processor_config.grid_col; ++i) {
|
|
Point pt1(i*grid_width, 0);
|
|
Point pt2(i*grid_width, img_height);
|
|
line(out_img, pt1, pt2, Scalar(255, 0, 0));
|
|
}
|
|
for (int i = 1; i < processor_config.grid_col; ++i) {
|
|
Point pt1(i*grid_width+img_width, 0);
|
|
Point pt2(i*grid_width+img_width, img_height);
|
|
line(out_img, pt1, pt2, Scalar(255, 0, 0));
|
|
}
|
|
|
|
// Collect features ids in the previous frame.
|
|
vector<FeatureIDType> prev_ids(0);
|
|
for (const auto& grid_features : *prev_features_ptr)
|
|
for (const auto& feature : grid_features.second)
|
|
prev_ids.push_back(feature.id);
|
|
|
|
// Collect feature points in the previous frame.
|
|
map<FeatureIDType, Point2f> prev_cam0_points;
|
|
map<FeatureIDType, Point2f> prev_cam1_points;
|
|
for (const auto& grid_features : *prev_features_ptr)
|
|
for (const auto& feature : grid_features.second) {
|
|
prev_cam0_points[feature.id] = feature.cam0_point;
|
|
prev_cam1_points[feature.id] = feature.cam1_point;
|
|
}
|
|
|
|
// Collect feature points in the current frame.
|
|
map<FeatureIDType, Point2f> curr_cam0_points;
|
|
map<FeatureIDType, Point2f> curr_cam1_points;
|
|
for (const auto& grid_features : *curr_features_ptr)
|
|
for (const auto& feature : grid_features.second) {
|
|
curr_cam0_points[feature.id] = feature.cam0_point;
|
|
curr_cam1_points[feature.id] = feature.cam1_point;
|
|
}
|
|
|
|
// Draw tracked features.
|
|
for (const auto& id : prev_ids) {
|
|
if (prev_cam0_points.find(id) != prev_cam0_points.end() &&
|
|
curr_cam0_points.find(id) != curr_cam0_points.end()) {
|
|
cv::Point2f prev_pt0 = prev_cam0_points[id];
|
|
cv::Point2f prev_pt1 = prev_cam1_points[id] + Point2f(img_width, 0.0);
|
|
cv::Point2f curr_pt0 = curr_cam0_points[id];
|
|
cv::Point2f curr_pt1 = curr_cam1_points[id] + Point2f(img_width, 0.0);
|
|
|
|
circle(out_img, curr_pt0, 3, tracked, -1);
|
|
circle(out_img, curr_pt1, 3, tracked, -1);
|
|
line(out_img, prev_pt0, curr_pt0, tracked, 1);
|
|
line(out_img, prev_pt1, curr_pt1, tracked, 1);
|
|
|
|
prev_cam0_points.erase(id);
|
|
prev_cam1_points.erase(id);
|
|
curr_cam0_points.erase(id);
|
|
curr_cam1_points.erase(id);
|
|
}
|
|
}
|
|
|
|
// Draw new features.
|
|
for (const auto& new_cam0_point : curr_cam0_points) {
|
|
cv::Point2f pt0 = new_cam0_point.second;
|
|
cv::Point2f pt1 = curr_cam1_points[new_cam0_point.first] +
|
|
Point2f(img_width, 0.0);
|
|
|
|
circle(out_img, pt0, 3, new_feature, -1);
|
|
circle(out_img, pt1, 3, new_feature, -1);
|
|
}
|
|
|
|
cv_bridge::CvImage debug_image(cam0_curr_img_ptr->header, "bgr8", out_img);
|
|
debug_stereo_pub.publish(debug_image.toImageMsg());
|
|
}
|
|
//imshow("Feature", out_img);
|
|
//waitKey(5);
|
|
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::updateFeatureLifetime() {
|
|
for (int code = 0; code <
|
|
processor_config.grid_row*processor_config.grid_col; ++code) {
|
|
vector<FeatureMetaData>& features = (*curr_features_ptr)[code];
|
|
for (const auto& feature : features) {
|
|
if (feature_lifetime.find(feature.id) == feature_lifetime.end())
|
|
feature_lifetime[feature.id] = 1;
|
|
else
|
|
++feature_lifetime[feature.id];
|
|
}
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
void ImageProcessor::featureLifetimeStatistics() {
|
|
|
|
map<int, int> lifetime_statistics;
|
|
for (const auto& data : feature_lifetime) {
|
|
if (lifetime_statistics.find(data.second) ==
|
|
lifetime_statistics.end())
|
|
lifetime_statistics[data.second] = 1;
|
|
else
|
|
++lifetime_statistics[data.second];
|
|
}
|
|
|
|
for (const auto& data : lifetime_statistics)
|
|
cout << data.first << " : " << data.second << endl;
|
|
|
|
return;
|
|
}
|
|
|
|
} // end namespace msckf_vio
|