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

not working, added stop and go to image processing, added undistorted image to image processing

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-26 19:23:50 +02:00
parent 273bbf8a94
commit ebc73c0c5e
11 changed files with 260 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

138
include/msckf_vio/feature.hpp
View File

@ -137,6 +137,9 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
inline bool checkMotion(
const CamStateServer& cam_states) const;
bool initializeAnchorUndistort(const CameraCalibration& cam, int N);
/*
* @brief InitializeAnchor generates the NxN patch around the
* feature in the Anchor image
@ -171,6 +174,12 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
Eigen::Vector3d& in_p) const;
cv::Point2f projectPositionToCameraUndistorted(
const CAMState& cam_state,
const StateIDType& cam_state_id,
const CameraCalibration& cam,
Eigen::Vector3d& in_p) const;
/*
* @brief project PositionToCamera Takes a 3d position in a world frame
@ -692,26 +701,22 @@ bool Feature::VisualizeKernel(
//cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3);
cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
cv::Mat norm_abs_xderImage;
cv::normalize(abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
cv::imshow("xder", norm_abs_xderImage);
cv::imshow("yder", abs_yderImage);
// cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
// cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
/*
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.*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.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_y"));
cv::imshow("anchor", anchorImage);
cv::imshow("xder2", xderImage2);
cv::imshow("yder2", yderImage2);
*/
//cv::imshow("anchor", anchorImage);
cv::imshow("xder2", xderImage);
cv::imshow("yder2", yderImage);
cvWaitKey(0);
}
@ -752,7 +757,7 @@ bool Feature::VisualizePatch(
std::vector<double> projectionPatch;
for(auto point : anchorPatch_3d)
{
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
cv::Point2f p_in_c0 = projectPositionToCameraUndistorted(cam_state, cam_state_id, cam0, point);
projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image));
// visu - feature
cv::Point xs(p_in_c0.x, p_in_c0.y);
@ -807,10 +812,11 @@ bool Feature::VisualizePatch(
cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N),
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
cv::Point2f p_f(observations.find(cam_state_id)->second(0)*cam0.intrinsics[0] + cam0.intrinsics[2],observations.find(cam_state_id)->second(1)*cam0.intrinsics[1] + cam0.intrinsics[3]);
// move to real pixels
p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);
// without distort
// p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);
for(int i = 0; i<N; i++)
{
for(int j = 0; j<N ; j++)
@ -958,7 +964,34 @@ cv::Point2f Feature::pixelDistanceAt(
return distance;
}
cv::Point2f Feature::projectPositionToCameraUndistorted(
const CAMState& cam_state,
const StateIDType& cam_state_id,
const CameraCalibration& cam,
Eigen::Vector3d& in_p) const
{
Eigen::Isometry3d T_c0_w;
cv::Point2f out_p;
// transfrom position to camera frame
Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
const Eigen::Vector3d& t_c0_w = cam_state.position;
Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
// if(cam_state_id == observations.begin()->first)
//printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y);
cv::Point2f my_p(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]);
// printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z());
// printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2));
// printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y);
return my_p;
}
cv::Point2f Feature::projectPositionToCamera(
const CAMState& cam_state,
@ -1080,6 +1113,79 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
return true;
}
//@test center projection must always be initial feature projection
bool Feature::initializeAnchorUndistort(const CameraCalibration& cam, int N)
{
//initialize patch Size
int n = (int)(N-1)/2;
auto anchor = observations.begin();
if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
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::Mat hen = cv::Mat::zeros(cv::Size(3, 3), CV_16S);
hen.at<int>(1,0) = 1;
//sobel test
/*
cv::Mat newhen;
cv::Sobel(hen, newhen, -1, 1, 0, 3);
std::cout << "newhen" << newhen << std::endl;
*/
cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3);
cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3);
xderImage/=4;
yderImage/=4;
cv::convertScaleAbs(xderImage, abs_xderImage);
cv::convertScaleAbs(yderImage, abs_yderImage);
cvWaitKey(0);
auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2];
auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3];
//project onto pixel plane
undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]);
// create vector of patch in pixel plane
for(double u_run = -n; u_run <= n; u_run++)
for(double v_run = -n; v_run <= n; v_run++)
anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run));
//project back into u,v
for(int i = 0; i < N*N; i++)
anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1]));
// save anchor position for later visualisaztion
anchor_center_pos = anchorPatch_real[(N*N-1)/2];
// save true pixel Patch position
for(auto point : anchorPatch_real)
if(point.x - n < 0 || point.x + n >= cam.resolution(0)-1 || point.y - n < 0 || point.y + n >= cam.resolution(1)-1)
return false;
for(auto point : anchorPatch_real)
anchorPatch.push_back(PixelIrradiance(point, anchorImage));
// project patch pixel to 3D space in camera coordinate system
for(auto point : anchorPatch_ideal)
anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam));
is_anchored = true;
return true;
}
bool Feature::initializeRho(const CamStateServer& cam_states) {
// Organize camera poses and feature observations properly.

3
include/msckf_vio/image_handler.h
View File

@ -16,6 +16,9 @@ namespace msckf_vio {
*/
namespace image_handler {
cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics);
cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics);
void undistortPoints(
const std::vector<cv::Point2f>& pts_in,
const cv::Vec4d& intrinsics,

2
include/msckf_vio/image_processor.h
View File

@ -320,6 +320,8 @@ private:
return;
}
bool STREAMPAUSE;
// Indicate if this is the first image message.
bool is_first_img;

2
include/msckf_vio/msckf_vio.h
View File

@ -207,7 +207,7 @@ class MsckfVio {
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
void PhotometricMeasurementJacobian(
bool PhotometricMeasurementJacobian(
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
Eigen::MatrixXd& H_x,