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

added kernel calculation visualization; changed sobel filter to cv implementation, added octave export

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-13 16:20:37 +02:00
parent acca4ab018
commit 07f4927128
2 changed files with 98 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
include/msckf_vio/feature.hpp
View File

@ -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<short>(pose.y, pose.x))/255. << std::endl;
delta = ((double)xderImage.at<short>(pose.y, pose.x))/255.;
}
else if (type == "Sobel_y")
{
std::cout << "image value y" << ((double)yderImage.at<short>(pose.y, pose.x))/255. << std::endl;
delta = ((double)yderImage.at<short>(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<double, 3, 3> kernel = Eigen::Matrix<double, 3, 3>::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<uint8_t>(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_x");
xderImage2.at<uint8_t>(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<uint8_t>(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_y");
yderImage2.at<uint8_t>(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];