diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index b094d49..218157b 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -184,6 +184,10 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, const CameraCalibration& cam, Eigen::Vector3d& in_p) const; + double cvKernel( + const cv::Point2f pose, + std::string type) const; + double Kernel( const cv::Point2f pose, const cv::Mat frame, @@ -269,6 +273,14 @@ inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p, bool is_initialized; bool is_anchored; + + cv::Mat abs_xderImage; + cv::Mat abs_yderImage; + + cv::Mat xderImage; + cv::Mat yderImage; + + cv::Mat anchorImage_blurred; cv::Point2f anchor_center_pos; cv::Point2f undist_anchor_center_pos; // Noise for a normalized feature measurement. @@ -478,6 +490,27 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const else return false; } +double Feature::cvKernel( + const cv::Point2f pose, + std::string type) const +{ + + double delta = 0; + + if(type == "Sobel_x") + { + std::cout << "image value x" << ((double)xderImage.at(pose.y, pose.x))/255. << std::endl; + delta = ((double)xderImage.at(pose.y, pose.x))/255.; + } + else if (type == "Sobel_y") + { + std::cout << "image value y" << ((double)yderImage.at(pose.y, pose.x))/255. << std::endl; + delta = ((double)yderImage.at(pose.y, pose.x))/255.; + } + return delta; + +} + double Feature::Kernel( const cv::Point2f pose, const cv::Mat frame, @@ -485,9 +518,9 @@ double Feature::Kernel( { Eigen::Matrix kernel = Eigen::Matrix::Zero(); if(type == "Sobel_x") - kernel << -3., 0., 3.,-10., 0., 10. , -3., 0., 3.; + kernel << -1., 0., 1.,-2., 0., 2. , -1., 0., 1.; else if(type == "Sobel_y") - kernel << -3., -10., -3., 0., 0., 0., 3., 10., 3.; + kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.; double delta = 0; int offs = (int)(kernel.rows()-1)/2; @@ -658,29 +691,29 @@ bool Feature::VisualizeKernel( auto anchor = observations.begin(); cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image; - cv::Mat xderImage; - cv::Mat yderImage; + //cv::Mat xderImage; + //cv::Mat yderImage; - cv::Sobel(anchorImage, xderImage, CV_8UC1, 1, 0, 3); - cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3); + //cv::Sobel(anchorImage, xderImage, CV_8UC1, 1, 0, 3); + //cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3); - cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, yderImage.type()); - cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, yderImage.type()); + cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type()); + cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type()); - cv::imshow("xder", xderImage); - //cv::imshow("yder", yderImage); + cv::imshow("xder", abs_xderImage); + cv::imshow("yder", abs_yderImage); for(int i = 1; i < anchorImage.rows-1; i++) for(int j = 1; j < anchorImage.cols-1; j++) - xderImage2.at(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_x"); + xderImage2.at(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_x")); for(int i = 1; i < anchorImage.rows-1; i++) for(int j = 1; j < anchorImage.cols-1; j++) - yderImage2.at(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_y"); + yderImage2.at(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_y")); cv::imshow("anchor", anchorImage); cv::imshow("xder2", xderImage2); - //cv::imshow("yder2", yderImage2); + cv::imshow("yder2", yderImage2); cvWaitKey(0); } @@ -824,12 +857,13 @@ bool Feature::VisualizePatch( */ // residual gradient direction + /* cv::arrowedLine(irradianceFrame, cv::Point(40+scale*(N+N/2+0.5), 15+scale*((N-0.5))), cv::Point(40+scale*(N+N/2+0.5)+scale*residualVector.x, 15+scale*(N-0.5)+scale*residualVector.y), cv::Scalar(0, 255, 175), 3); - + */ cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); @@ -921,7 +955,7 @@ cv::Point2f Feature::pixelDistanceAt( Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); // returns the distance between the pixel points in space - cv::Point2f distance(fabs(pure[0].x - pure[1].x)*p_c0[2], fabs(pure[2].y - pure[3].y)*p_c0[2]); + cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y)); return distance; } @@ -976,6 +1010,8 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal bool Feature::initializeAnchor(const CameraCalibration& cam, int N) { + + //initialize patch Size int n = (int)(N-1)/2; @@ -984,6 +1020,18 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) return false; cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image; + cv::Mat anchorImage_deeper; + anchorImage.convertTo(anchorImage_deeper,CV_16S); + //TODO remove this? + + + cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3); + cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3); + + cv::convertScaleAbs(xderImage, abs_xderImage); + cv::convertScaleAbs(yderImage, abs_yderImage); + + cv::GaussianBlur(anchorImage, anchorImage_blurred, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT); auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2]; auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3]; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index a0442fc..5b3c065 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1314,14 +1314,14 @@ void MsckfVio::PhotometricMeasurementJacobian( // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_x"); - dy = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_y"); + dx = feature.cvKernel(p_in_anchor, "Sobel_x"); + dy = feature.cvKernel(p_in_anchor, "Sobel_y"); // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); - dI_dhj(0, 0) = dx;// /(pixelDistance.x); - dI_dhj(0, 1) = dy;// /(pixelDistance.y); + dI_dhj(0, 0) = dx/(pixelDistance.x); + dI_dhj(0, 1) = dy/(pixelDistance.y); gradientVector.x += dx; gradientVector.y += dy; @@ -1423,8 +1423,8 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); - feature.VisualizeKernel(cam_state, cam_state_id, cam0); + feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); + // feature.VisualizeKernel(cam_state, cam_state_id, cam0); } return; @@ -1496,7 +1496,7 @@ void MsckfVio::PhotometricFeatureJacobian( H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; - cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; + //cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; if(PRINTIMAGES) { @@ -1519,8 +1519,34 @@ void MsckfVio::PhotometricFeatureJacobian( myfile << A_null_space.block(i, 0, 1, A_null_space.cols()) << ";"; + myfile.close(); + + //octave + myfile.open("/home/raphael/dev/octave/log2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: Hx\n" + << "# type: matrix\n" + << "# rows: " << H_xi.rows() << "\n" + << "# columns: " << H_xi.cols() << "\n" + << H_xi << endl; + + myfile << "# name: Hy\n" + << "# type: matrix\n" + << "# rows: " << H_yi.rows() << "\n" + << "# columns: " << H_yi.cols() << "\n" + << H_yi << endl; + + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r_i.rows() << "\n" + << "# columns: " << 1 << "\n" + << r_i << endl; + myfile.close(); cout << "---------- LOGGED -------- " << endl; + cvWaitKey(0); + } if(PRINTIMAGES) { @@ -1808,6 +1834,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) cout << dof << " " << gamma << " " << chi_squared_test_table[dof] << endl; + if (chi_squared_test_table[dof] == 0) + return true; if (gamma < chi_squared_test_table[dof]) { //cout << "passed" << endl; return true;