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

added kernel visualization

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-12 18:39:40 +02:00
parent 44fffa19a2
commit acca4ab018
5 changed files with 157 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
include/msckf_vio/feature.hpp
View File

@ -206,6 +206,11 @@ bool MarkerGeneration(
ros::Publisher& marker_pub,
const CamStateServer& cam_states) const;
bool VisualizeKernel(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0) const;
bool VisualizePatch(
const CAMState& cam_state,
const StateIDType& cam_state_id,
@ -480,16 +485,16 @@ double Feature::Kernel(
{
Eigen::Matrix<double, 3, 3> kernel = Eigen::Matrix<double, 3, 3>::Zero();
if(type == "Sobel_x")
kernel << -1., 0., 1.,-2., 0., 2. , -1., 0., 1.;
kernel << -3., 0., 3.,-10., 0., 10. , -3., 0., 3.;
else if(type == "Sobel_y")
kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.;
kernel << -3., -10., -3., 0., 0., 0., 3., 10., 3.;
double delta = 0;
int offs = (int)(kernel.rows()-1)/2;
for(int i = 0; i < kernel.rows(); i++)
for(int j = 0; j < kernel.cols(); j++)
delta += ((float)frame.at<uint8_t>(pose.y+j-offs , pose.x+i-offs))/255 * (float)kernel(j,i);
delta += ((float)frame.at<uint8_t>(pose.y+j-offs , pose.x+i-offs))/255. * (float)kernel(j,i);
return delta;
}
@ -644,6 +649,41 @@ bool Feature::MarkerGeneration(
marker_pub.publish(ma);
}
bool Feature::VisualizeKernel(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0) const
{
auto anchor = observations.begin();
cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
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::Mat xderImage2(anchorImage.rows, anchorImage.cols, yderImage.type());
cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, yderImage.type());
cv::imshow("xder", xderImage);
//cv::imshow("yder", 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");
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");
cv::imshow("anchor", anchorImage);
cv::imshow("xder2", xderImage2);
//cv::imshow("yder2", yderImage2);
cvWaitKey(0);
}
bool Feature::VisualizePatch(
const CAMState& cam_state,
const StateIDType& cam_state_id,
@ -848,7 +888,7 @@ bool Feature::VisualizePatch(
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
{
return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
return ((float)image.at<uint8_t>(pose.y, pose.x))/255.;
}
cv::Point2f Feature::pixelDistanceAt(