added tum launch files, removed anchor procedure being called multiple times through a flag
This commit is contained in:
@ -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;
|
||||
|
Reference in New Issue
Block a user