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

moved camera calibration information into a struct to make data handling smoother

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-16 19:05:11 +02:00
parent 010d36d216
commit 7f2140ae88
5 changed files with 107 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

114
src/image_processor.cpp
View File

@@ -44,49 +44,49 @@ ImageProcessor::~ImageProcessor() {
bool ImageProcessor::loadParameters() {
// Camera calibration parameters
nh.param<string>("cam0/distortion_model",
cam0_distortion_model, string("radtan"));
cam0.distortion_model, string("radtan"));
nh.param<string>("cam1/distortion_model",
cam1_distortion_model, string("radtan"));
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];
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];
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];
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];
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];
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];
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];
cv::Mat T_imu_cam0 = utils::getTransformCV(nh, "cam0/T_cam_imu");
cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3)));
@@ -124,27 +124,27 @@ bool ImageProcessor::loadParameters() {
processor_config.stereo_threshold, 3);
ROS_INFO("===========================================");
ROS_INFO("cam0_resolution: %d, %d",
cam0_resolution[0], cam0_resolution[1]);
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());
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]);
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.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());
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]);
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;
@@ -455,7 +455,7 @@ void ImageProcessor::trackFeatures() {
vector<unsigned char> track_inliers(0);
predictFeatureTracking(prev_cam0_points,
cam0_R_p_c, cam0_intrinsics, curr_cam0_points);
cam0_R_p_c, cam0.intrinsics, curr_cam0_points);
calcOpticalFlowPyrLK(
prev_cam0_pyramid_, curr_cam0_pyramid_,
@@ -550,14 +550,14 @@ void ImageProcessor::trackFeatures() {
// 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,
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,
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.
@@ -619,12 +619,12 @@ void ImageProcessor::stereoMatch(
// rotation from stereo extrinsics
const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu;
vector<cv::Point2f> cam0_points_undistorted;
image_handler::undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model,
cam0_distortion_coeffs, cam0_points_undistorted,
image_handler::undistortPoints(cam0_points, cam0.intrinsics, cam0.distortion_model,
cam0.distortion_coeffs, cam0_points_undistorted,
R_cam0_cam1);
cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1_intrinsics,
cam1_distortion_model, cam1_distortion_coeffs);
cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1.intrinsics,
cam1.distortion_model, cam1.distortion_coeffs);
}
// Track features using LK optical flow method.
@@ -665,15 +665,15 @@ void ImageProcessor::stereoMatch(
vector<cv::Point2f> cam0_points_undistorted(0);
vector<cv::Point2f> cam1_points_undistorted(0);
image_handler::undistortPoints(
cam0_points, cam0_intrinsics, cam0_distortion_model,
cam0_distortion_coeffs, cam0_points_undistorted);
cam0_points, cam0.intrinsics, cam0.distortion_model,
cam0.distortion_coeffs, cam0_points_undistorted);
image_handler::undistortPoints(
cam1_points, cam1_intrinsics, cam1_distortion_model,
cam1_distortion_coeffs, cam1_points_undistorted);
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]);
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;
@@ -1253,11 +1253,11 @@ void ImageProcessor::publish() {
vector<Point2f> curr_cam1_points_undistorted(0);
image_handler::undistortPoints(
curr_cam0_points, cam0_intrinsics, cam0_distortion_model,
cam0_distortion_coeffs, curr_cam0_points_undistorted);
curr_cam0_points, cam0.intrinsics, cam0.distortion_model,
cam0.distortion_coeffs, curr_cam0_points_undistorted);
image_handler::undistortPoints(
curr_cam1_points, cam1_intrinsics, cam1_distortion_model,
cam1_distortion_coeffs, curr_cam1_points_undistorted);
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());

56
src/msckf_vio.cpp
View File

@@ -119,49 +119,49 @@ bool MsckfVio::loadParameters() {
// get camera information (used for back projection)
nh.param<string>("cam0/distortion_model",
cam0_distortion_model, string("radtan"));
cam0.distortion_model, string("radtan"));
nh.param<string>("cam1/distortion_model",
cam1_distortion_model, string("radtan"));
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];
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];
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];
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];
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];
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];
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];
@@ -904,7 +904,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
//photometric observation
std::vector<uint8_t> photo_z;
feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs, cam0_moving_window, photo_z);
feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs, cam0_moving_window, photo_z);
// Convert the feature position from the world frame to
@@ -921,9 +921,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
/*
//prints the feature projection in pixel space
std::vector<cv::Point2f> my_p = image_handler::distortPoints( out_v,
cam0_intrinsics,
cam0_distortion_model,
cam0_distortion_coeffs);
cam0.intrinsics,
cam0.distortion_model,
cam0.distortion_coeffs);
printf("projection: %f, %f\n", my_p[0].x, my_p[0].y);
*/
@@ -1316,7 +1316,7 @@ void MsckfVio::removeLostFeatures() {
}
}
if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs))
if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs))
{
invalid_feature_ids.push_back(feature.id);
continue;
@@ -1470,7 +1470,7 @@ void MsckfVio::pruneCamStateBuffer() {
}
}
if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs))
if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs))
{
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);