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

added stereo camera residual and jacobi to twomsckf - works

Browse Source
This commit is contained in:
Raphael Maenle
2019-07-03 16:11:23 +02:00
parent 6bcc72f826
commit 6ee756941c
4 changed files with 99 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
include/msckf_vio/feature.hpp
View File

@ -184,9 +184,10 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
const CameraCalibration& cam,
Eigen::Vector3d& in_p) const;
double CompleteCvKernel(
double CompleteCvKernel(
const cv::Point2f pose,
const cv::Mat& frame,
const StateIDType& cam_state_id,
CameraCalibration& cam,
std::string type) const;
double cvKernel(
@ -495,40 +496,17 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const
double Feature::CompleteCvKernel(
const cv::Point2f pose,
const cv::Mat& frame,
const StateIDType& cam_state_id,
CameraCalibration& cam,
std::string type) const
{
double delta = 0;
cv::Mat xder;
cv::Mat yder;
cv::Mat deeper_frame;
frame.convertTo(deeper_frame,CV_16S);
//TODO remove this?
cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
xder/=8.;
yder/=8.;
/*
cv::Mat norm_abs_xderImage;
cv::Mat abs_xderImage2;
cv::convertScaleAbs(xder, abs_xderImage2);
cv::normalize(abs_xderImage2, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
cv::imshow("xder", norm_abs_xderImage);
cvWaitKey(0);
*/
if(type == "Sobel_x")
delta = ((double)xder.at<short>(pose.y, pose.x))/255.;
delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at<short>(pose.y, pose.x))/255.;
else if (type == "Sobel_y")
delta = ((double)yder.at<short>(pose.y, pose.x))/255.;
delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at<short>(pose.y, pose.x))/255.;
return delta;
}