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

added position calculation

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-12 17:37:01 +02:00
parent a85a4745f2
commit 8227a8e48d
8 changed files with 241 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
src/image_processor.cpp
View File

@ -17,6 +17,7 @@
#include <msckf_vio/TrackingInfo.h>
#include <msckf_vio/image_processor.h>
#include <msckf_vio/utils.h>
#include <msckf_vio/image_handler.h>
using namespace std;
using namespace cv;
@ -618,10 +619,10 @@ 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;
undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model,
image_handler::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_points = image_handler::distortPoints(cam0_points_undistorted, cam1_intrinsics,
cam1_distortion_model, cam1_distortion_coeffs);
}
@ -662,10 +663,10 @@ void ImageProcessor::stereoMatch(
// essential matrix.
vector<cv::Point2f> cam0_points_undistorted(0);
vector<cv::Point2f> cam1_points_undistorted(0);
undistortPoints(
image_handler::undistortPoints(
cam0_points, cam0_intrinsics, cam0_distortion_model,
cam0_distortion_coeffs, cam0_points_undistorted);
undistortPoints(
image_handler::undistortPoints(
cam1_points, cam1_intrinsics, cam1_distortion_model,
cam1_distortion_coeffs, cam1_points_undistorted);
@ -1009,10 +1010,10 @@ void ImageProcessor::twoPointRansac(
// Undistort all the points.
vector<Point2f> pts1_undistorted(pts1.size());
vector<Point2f> pts2_undistorted(pts2.size());
undistortPoints(
image_handler::undistortPoints(
pts1, intrinsics, distortion_model,
distortion_coeffs, pts1_undistorted);
undistortPoints(
image_handler::undistortPoints(
pts2, intrinsics, distortion_model,
distortion_coeffs, pts2_undistorted);
@ -1250,10 +1251,10 @@ void ImageProcessor::publish() {
vector<Point2f> curr_cam0_points_undistorted(0);
vector<Point2f> curr_cam1_points_undistorted(0);
undistortPoints(
image_handler::undistortPoints(
curr_cam0_points, cam0_intrinsics, cam0_distortion_model,
cam0_distortion_coeffs, curr_cam0_points_undistorted);
undistortPoints(
image_handler::undistortPoints(
curr_cam1_points, cam1_intrinsics, cam1_distortion_model,
cam1_distortion_coeffs, curr_cam1_points_undistorted);