diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp
index 44f2a9a..0014809 100644
--- a/include/msckf_vio/feature.hpp
+++ b/include/msckf_vio/feature.hpp
@@ -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));
diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch
index 8f59c97..9e8ef7a 100644
--- a/launch/msckf_vio_euroc.launch
+++ b/launch/msckf_vio_euroc.launch
@@ -19,11 +19,11 @@
-
+
-
+
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index c9cb9e9..eb33e53 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -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)