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

fixed incorrect undistortion/distortion. residual should now be calculated correctly

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-18 16:22:41 +02:00
parent d91ff7ca9d
commit 1fa2518215
4 changed files with 120 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

118
include/msckf_vio/feature.hpp
View File

@ -144,7 +144,12 @@ struct Feature {
inline bool initializePosition(
const CamStateServer& cam_states);
/*
* @brief project PositionToCamera Takes a 3d position in a world frame
* and projects it into the passed camera frame using pinhole projection
* then distorts it using camera information to get
* the resulting distorted pixel position
*/
inline cv::Point2f projectPositionToCamera(
const CAMState& cam_state,
const StateIDType& cam_state_id,
@ -212,12 +217,15 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
Eigen::Vector3d position;
// inverse depth representation
double rho;
double anchor_rho;
// A indicator to show if the 3d postion of the feature
// has been initialized or not.
bool is_initialized;
bool is_anchored;
cv::Point2f anchor_center_pos;
cv::Point2f undist_anchor_center_pos;
// Noise for a normalized feature measurement.
static double observation_noise;
@ -397,14 +405,24 @@ bool Feature::FrameIrradiance(
const movingWindow& cam0_moving_window,
std::vector<float>& anchorPatch_measurement) const
{
//project every point in anchorPatch_3d.
if(!is_anchored)
printf("not anchored!\n");
// project every point in anchorPatch_3d.
// int count = 0;
for (auto point : anchorPatch_3d)
{
// testing
//if(cam_state_id == observations.begin()->first)
//if(count == 4)
//printf("\n\ncenter:\n");
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
float irradiance = PixelIrradiance(p_in_c0, cam0_moving_window.find(cam_state_id)->second.image);
anchorPatch_measurement.push_back(irradiance);
// testing
//if(cam_state_id == observations.begin()->first)
//if(count++ == 4)
//printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y);
}
}
@ -429,18 +447,20 @@ cv::Point2f Feature::projectPositionToCamera(
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));
std::vector<cv::Point2f> out_v;
out_v.push_back(out_p);
std::vector<cv::Point2f> my_p = image_handler::distortPoints(out_v,
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs);
// 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 = image_handler::distortPoint(out_p,
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs);
// 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[0];
return my_p;
}
Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
@ -450,7 +470,7 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
// project it back into 3D space using pinhole model
// save resulting NxN positions for this feature
Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho);
Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho);
Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
return PositionInWorld;
//printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
@ -473,34 +493,56 @@ bool Feature::initializeAnchor(
return false;
cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second.image;
auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2];
auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3];
printf("initializing anchor\n");
if(u - n < 0 || u + n >= cam.resolution(0) || v - n < 0 || v + n >= cam.resolution(1))
{
printf("no good: \n");
printf("%f, %f\n", u, v);
return false;
}
auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2];
auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3];
//testing
undist_anchor_center_pos = cv::Point2f(u,v);
//for NxN patch pixels around feature
for(double u_run = u - n; u_run <= u + n; u_run = u_run + 1)
int count = 0;
// get feature in undistorted pixel space
cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v),
cam.intrinsics,
cam.distortion_model,
0);
// create vector of patch in pixel plane
std::vector<cv::Point2f> und_pix_v;
for(double u_run = -n; u_run <= n; u_run++)
for(double v_run = -n; v_run <= n; v_run++)
und_pix_v.push_back(cv::Point2f(und_pix_p.x-u_run, und_pix_p.y-v_run));
//create undistorted pure points
std::vector<cv::Point2f> und_v;
image_handler::undistortPoints(und_pix_v,
cam.intrinsics,
cam.distortion_model,
0,
und_v);
//create distorted pixel points
std::vector<cv::Point2f> vec = image_handler::distortPoints(und_v,
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs);
anchor_center_pos = vec[4];
for(auto point : vec)
{
for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1)
if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
{
printf("ADDING\n");
// add irradiance information
anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)u_run,(int)v_run));
// project patch pixel to 3D space
auto intr = cam.intrinsics;
cv::Point2f currentPoint((u_run-intr[2])/intr[0], (v_run-intr[3])/intr[1]);
Eigen::Vector3d Npose = projectPixelToPosition(currentPoint, cam);
//save position
anchorPatch_3d.push_back(Npose);
printf("no good\n");
return false;
}
}
printf("set to true!!!\n");
for(auto point : vec)
anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)point.x,(int)point.y));
// project patch pixel to 3D space
for(auto point : und_v)
anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
is_anchored = true;
return true;
}
@ -645,7 +687,7 @@ bool Feature::initializePosition(
}
//save inverse depth distance from camera
rho = solution(2);
anchor_rho = solution(2);
// Convert the feature position to the world frame.
position = T_c0_w.linear()*final_position + T_c0_w.translation();

6
include/msckf_vio/image_handler.h
View File

@ -30,6 +30,12 @@ std::vector<cv::Point2f> distortPoints(
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs);
cv::Point2f distortPoint(
const cv::Point2f& pt_in,
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs);
}
}
#endif