diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 7702ddc..662caed 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -403,7 +403,7 @@ bool Feature::FrameIrradiance( std::vector& anchorPatch_measurement) const { - // visu + // visu - feature /*cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; cv::Mat dottedFrame(current_image.size(), CV_8UC3); cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB); @@ -415,7 +415,7 @@ bool Feature::FrameIrradiance( cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); - // visu + // visu - feature /*cv::Point xs(p_in_c0.x, p_in_c0.y); cv::Point ys(p_in_c0.x, p_in_c0.y); cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); @@ -430,7 +430,7 @@ bool Feature::FrameIrradiance( } - //visu + // visu - feature //cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2)); /*if(cam0.featureVisu.empty()) cam0.featureVisu = dottedFrame.clone(); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 70e167c..6e49438 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -977,7 +977,7 @@ void MsckfVio::PhotometricMeasurementJacobian( r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); - // visu + // visu -residual //printf("-----\n"); //estimate photometric measurement @@ -989,7 +989,7 @@ void MsckfVio::PhotometricMeasurementJacobian( for(int i = 0; i < photo_z.size(); i++) photo_r.push_back(photo_z[i] - estimate_photo_z[i]); - // visu + // visu- residual //for(int i = 0; i < photo_z.size(); i++) // printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); @@ -1024,11 +1024,11 @@ void MsckfVio::PhotometricFeatureJacobian( int stack_cntr = 0; - // visu - /* - printf("_____FEATURE:_____\n"); - cam0.featureVisu.release(); - */ + // visu - residual + //printf("_____FEATURE:_____\n"); + // visu - feature + //cam0.featureVisu.release(); + for (const auto& cam_id : valid_cam_state_ids) { Matrix H_xi = Matrix::Zero(); @@ -1046,7 +1046,7 @@ void MsckfVio::PhotometricFeatureJacobian( r_j.segment<4>(stack_cntr) = r_i; stack_cntr += 4; } - // visu + // visu - feature /* if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000) imshow("feature", cam0.featureVisu);