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

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

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-18 11:06:45 +02:00
parent cfecefe29f
commit d91ff7ca9d
6 changed files with 170 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
src/image_processor.cpp
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;

34
src/msckf_vio.cpp
View File

@@ -306,7 +306,6 @@ void MsckfVio::imageCallback(
const sensor_msgs::ImageConstPtr& cam1_img,
const CameraMeasurementConstPtr& feature_msg)
{
// Return if the gravity vector has not been set.
if (!is_gravity_set) return;
@@ -344,7 +343,7 @@ void MsckfVio::imageCallback(
// Add new images to moving window
start_time = ros::Time::now();
//manageMovingWindow(cam0_img, cam1_img, feature_msg);
manageMovingWindow(cam0_img, cam1_img, feature_msg);
double manage_moving_window_time = (
ros::Time::now()-start_time).toSec();
@@ -398,16 +397,16 @@ void MsckfVio::manageMovingWindow(
const CameraMeasurementConstPtr& feature_msg) {
//save exposure Time into moving window
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000;
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000;
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000000;
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000000;
if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms < 1)
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 1;
if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms < 1)
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 1;
if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 500)
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 500;
if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 500)
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 500;
if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 100)
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 100;
if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 100)
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 100;
// Get the current image.
cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img,
@@ -981,6 +980,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
printf("-----\n");
//estimate photometric measurement
std::vector<float> estimate_photo_z;
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z);
@@ -990,9 +991,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
for(int i = 0; i < photo_z.size(); i++)
photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
// printf("-----\n");
// for(int i = 0; i < photo_z.size(); i++)
// printf("%.4f - %.4f\n", photo_z[i], estimate_photo_z[i]);
for(int i = 0; i < photo_z.size(); i++)
printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
photo_z.clear();
return;
@@ -1341,6 +1342,9 @@ void MsckfVio::removeLostFeatures() {
continue;
}
}
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
invalid_feature_ids.push_back(feature.id);
@@ -1477,7 +1481,6 @@ void MsckfVio::pruneCamStateBuffer() {
feature.observations.erase(involved_cam_state_ids[0]);
continue;
}
if (!feature.is_initialized) {
// Check if the feature can be initialize.
if (!feature.checkMotion(state_server.cam_states)) {
@@ -1494,6 +1497,9 @@ void MsckfVio::pruneCamStateBuffer() {
continue;
}
}
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
for (const auto& cam_id : involved_cam_state_ids)
@@ -1501,7 +1507,6 @@ void MsckfVio::pruneCamStateBuffer() {
continue;
}
}
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
}
@@ -1512,7 +1517,6 @@ void MsckfVio::pruneCamStateBuffer() {
21+6*state_server.cam_states.size());
VectorXd r = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
ros::Time start_time = ros::Time::now();
for (auto& item : map_server) {
auto& feature = item.second;
// Check how many camera states to be removed are associated
@@ -1539,8 +1543,6 @@ void MsckfVio::pruneCamStateBuffer() {
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
}
double anchorPrune_processing_time = (ros::Time::now()-start_time).toSec();
printf("FeatureJacobian Time: %f\n", anchorPrune_processing_time);
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);