added tum launch files, removed anchor procedure being called multiple times through a flag

This commit is contained in:
2019-04-18 11:06:45 +02:00
parent cfecefe29f
commit d91ff7ca9d
6 changed files with 170 additions and 100 deletions

View File

@ -390,7 +390,6 @@ void ImageProcessor::predictFeatureTracking(
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();
@ -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<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) {
@ -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<KeyPoint> 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<cv::Point2f> cam0_points(new_features.size());
@ -777,7 +771,6 @@ void ImageProcessor::addNewFeatures() {
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 <
@ -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<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.
@ -967,7 +892,6 @@ void ImageProcessor::integrateImuData(
void ImageProcessor::rescalePoints(
vector<Point2f>& pts1, vector<Point2f>& 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;