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
Show Stats 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));

4
launch/msckf_vio_euroc.launch
View File

@ -19,11 +19,11 @@
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
<param name="FILTER" value="1"/>
<param name="FILTER" value="2"/>
<!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/>
<param name="PrintImages" value="true"/>
<param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="3"/>

36
src/msckf_vio.cpp
View File

@ -425,6 +425,10 @@ void MsckfVio::imageCallback(
// the origin.
if (is_first_img) {
is_first_img = false;
cam0.intrinsics *=0.5;
cam1.intrinsics *=0.5;
cam0.resolution *=0.5;
cam1.resolution *=0.5;
state_server.imu_state.time = feature_msg->header.stamp.toSec();
}
static double max_processing_time = 0.0;
@ -546,12 +550,26 @@ void MsckfVio::manageMovingWindow(
cv::Mat new_cam0;
cv::Mat new_cam1;
cam0.intrinsics *=2;
cam1.intrinsics *=2;
cam0.resolution *=2;
cam1.resolution *=2;
image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
cam0.intrinsics *=0.5;
cam1.intrinsics *=0.5;
cam0.resolution *=0.5;
cam1.resolution *=0.5;
//resize
cv::Mat new_cam02;
cv::Mat new_cam12;
cv::resize(new_cam0, new_cam02, cv::Size(), 0.5, 0.5);
cv::resize(new_cam1, new_cam12, cv::Size(), 0.5, 0.5);
// save image information into moving window
cam0.moving_window[state_server.imu_state.id].image = new_cam0.clone();
cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone();
cam0.moving_window[state_server.imu_state.id].image = new_cam02.clone();
cam1.moving_window[state_server.imu_state.id].image = new_cam12.clone();
cv::Mat xder;
cv::Mat yder;
@ -576,14 +594,15 @@ void MsckfVio::manageMovingWindow(
yder/=96.;
/*
/*
cv::Mat norm_abs_xderImage;
cv::convertScaleAbs(xder, norm_abs_xderImage);
cv::normalize(norm_abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
cv::imshow("xder", norm_abs_xderImage);
cvWaitKey(0);
*/
*/
// save into moving window
cam1.moving_window[state_server.imu_state.id].dximage = xder.clone();
cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone();
@ -1705,9 +1724,6 @@ bool MsckfVio::PhotometricPatchPointJacobian(
dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0];
dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1];
cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl;
cout << dI_dhj(1, 2) << ", " << dI_dhj(1, 3) << endl;
// add jacobian
//dh / d{}^Cp_{ij}
@ -1784,8 +1800,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo))
return false;
cout << "-\n" << r_photo << "-" << endl;
//cout << "r\n" << r_photo << endl;
// calculate jacobian for patch
int count = 0;
@ -1929,7 +1943,9 @@ bool MsckfVio::PhotometricFeatureJacobian(
// skip observation if measurement is not valid
if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l))
continue;
{
continue;
}
// set size of stacking jacobians, once the returned jacobians are known
if(first)