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

fixed anchor initialization and added image resizing for incomming into msckf

Browse Source
This commit is contained in:
Raphael Maenle
2019-07-12 18:02:29 +02:00
parent 5a80f97b68
commit 0ef71b9220
3 changed files with 38 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
include/msckf_vio/feature.hpp
View File

@ -823,7 +823,11 @@ bool Feature::VisualizePatch(
cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N),
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
cv::Point2f p_f;
if(cam.id == 0)
p_f = cv::Point2f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
else if(cam.id == 1)
p_f = cv::Point2f(observations.find(cam_state_id)->second(2),observations.find(cam_state_id)->second(3));
// move to real pixels
p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs);
@ -1042,8 +1046,9 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
auto anchor = observations.begin();
if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
{
return false;
}
cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
cv::Mat anchorImage_deeper;
anchorImage.convertTo(anchorImage_deeper,CV_16S);
@ -1066,17 +1071,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
// check if image has been pre-undistorted
if(cam.distortion_model.substr(0,3) == "pre-")
if(cam.distortion_model.substr(0,3) == "pre")
{
std::cout << "is a pre" << std::endl;
//project onto pixel plane
undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]);
// create vector of patch in pixel plane
for(double u_run = -n; u_run <= n; u_run++)
for(double v_run = -n; v_run <= n; v_run++)
anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run));
//project back into u,v
for(int i = 0; i < N*N; i++)
anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1]));
@ -1112,7 +1116,7 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
for(auto point : anchorPatch_real)
if(point.x - n < 0 || point.x + n >= cam.resolution(0)-1 || point.y - n < 0 || point.y + n >= cam.resolution(1)-1)
return false;
for(auto point : anchorPatch_real)
anchorPatch.push_back(PixelIrradiance(point, anchorImage));