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

fixed pixel position return value

Browse Source
This commit is contained in:
Raphael Maenle 2019-04-17 09:03:27 +02:00
parent 7f2140ae88
commit 819e43bb3b
2 changed files with 22 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
include/msckf_vio/feature.hpp
View File

@ -127,9 +127,7 @@ struct Feature {
bool initializeAnchor( bool initializeAnchor(
const movingWindow& cam0_moving_window, const movingWindow& cam0_moving_window,
const cv::Vec4d& intrinsics, const CameraCalibration& cam);
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs);
/* /*
@ -150,9 +148,7 @@ struct Feature {
inline cv::Point2f projectPositionToCamera( inline cv::Point2f projectPositionToCamera(
const CAMState& cam_state, const CAMState& cam_state,
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
const cv::Vec4d& intrinsics, const CameraCalibration& cam,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs,
Eigen::Vector3d& in_p) const; Eigen::Vector3d& in_p) const;
/* /*
@ -164,9 +160,7 @@ struct Feature {
bool IrradianceOfAnchorPatch( bool IrradianceOfAnchorPatch(
const CAMState& cam_state, const CAMState& cam_state,
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
const cv::Vec4d& intrinsics, const CameraCalibration& cam,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs,
const movingWindow& cam0_moving_window, const movingWindow& cam0_moving_window,
std::vector<uint8_t>& anchorPatch_measurement) const; std::vector<uint8_t>& anchorPatch_measurement) const;
@ -174,11 +168,8 @@ struct Feature {
* @brief projectPixelToPosition uses the calcualted pixels * @brief projectPixelToPosition uses the calcualted pixels
* of the anchor patch to generate 3D positions of all of em * of the anchor patch to generate 3D positions of all of em
*/ */
inline bool projectPixelToPosition(cv::Point2f in_p, inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
Eigen::Vector3d& out_p, const CameraCalibration& cam);
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs);
/* /*
* @brief Irradiance returns irradiance value of a pixel * @brief Irradiance returns irradiance value of a pixel
@ -362,16 +353,14 @@ bool Feature::checkMotion(
bool Feature::IrradianceOfAnchorPatch( bool Feature::IrradianceOfAnchorPatch(
const CAMState& cam_state, const CAMState& cam_state,
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
const cv::Vec4d& intrinsics, const CameraCalibration& cam,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs,
const movingWindow& cam0_moving_window, const movingWindow& cam0_moving_window,
std::vector<uint8_t>& anchorPatch_measurement) const std::vector<uint8_t>& anchorPatch_measurement) const
{ {
//project every point in anchorPatch_3d. //project every point in anchorPatch_3d.
for (auto point : anchorPatch_3d) for (auto point : anchorPatch_3d)
{ {
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, intrinsics, distortion_model, distortion_coeffs, point); cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point);
uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second); uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second);
anchorPatch_measurement.push_back(irradiance); anchorPatch_measurement.push_back(irradiance);
} }
@ -385,9 +374,7 @@ uint8_t Feature::Irradiance(cv::Point2f pose, cv::Mat image) const
cv::Point2f Feature::projectPositionToCamera( cv::Point2f Feature::projectPositionToCamera(
const CAMState& cam_state, const CAMState& cam_state,
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
const cv::Vec4d& intrinsics, const CameraCalibration& cam,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs,
Eigen::Vector3d& in_p) const Eigen::Vector3d& in_p) const
{ {
Eigen::Isometry3d T_c0_w; Eigen::Isometry3d T_c0_w;
@ -402,23 +389,17 @@ cv::Point2f Feature::projectPositionToCamera(
out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
std::vector<cv::Point2f> out_v; std::vector<cv::Point2f> out_v;
out_v.push_back(out_p); out_v.push_back(out_p);
std::vector<cv::Point2f> my_p = image_handler::distortPoints( out_v, std::vector<cv::Point2f> my_p = image_handler::distortPoints(out_v, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs);
intrinsics,
distortion_model,
distortion_coeffs);
// printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); // 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("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); // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y);
return out_p; return my_p[0];
} }
bool Feature::projectPixelToPosition(cv::Point2f in_p, Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
Eigen::Vector3d& out_p, const CameraCalibration& cam)
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs)
{ {
// use undistorted position of point of interest // use undistorted position of point of interest
// project it back into 3D space using pinhole model // project it back into 3D space using pinhole model
@ -426,7 +407,7 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p,
Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho); Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho);
Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
anchorPatch_3d.push_back(PositionInWorld); return PositionInWorld;
//printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
} }
@ -434,9 +415,7 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p,
//@test center projection must always be initial feature projection //@test center projection must always be initial feature projection
bool Feature::initializeAnchor( bool Feature::initializeAnchor(
const movingWindow& cam0_moving_window, const movingWindow& cam0_moving_window,
const cv::Vec4d& intrinsics, const CameraCalibration& cam)
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs)
{ {
int N = 3; int N = 3;
@ -447,8 +426,8 @@ bool Feature::initializeAnchor(
return false; return false;
cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second; cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second;
auto u = anchor->second(0)*intrinsics[0] + intrinsics[2]; auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2];
auto v = anchor->second(1)*intrinsics[1] + intrinsics[3]; auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3];
int count = 0; int count = 0;
//go through surrounding pixels //go through surrounding pixels
@ -457,12 +436,9 @@ bool Feature::initializeAnchor(
for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1) for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1)
{ {
anchorPatch.push_back(anchorImage.at<uint8_t>((int)u_run,(int)v_run)); anchorPatch.push_back(anchorImage.at<uint8_t>((int)u_run,(int)v_run));
Eigen::Vector3d Npose; cv::Point2f currentPoint((u_run-cam.intrinsics[2])/cam.intrinsics[0], (v_run-cam.intrinsics[3])/cam.intrinsics[1]);
projectPixelToPosition(cv::Point2f((u_run-intrinsics[2])/intrinsics[0], (v_run-intrinsics[3])/intrinsics[1]), Eigen::Vector3d Npose = projectPixelToPosition(currentPoint, cam);
Npose, anchorPatch_3d.push_back(Npose);
intrinsics,
distortion_model,
distortion_coeffs);
} }
} }

7
src/msckf_vio.cpp
View File

@ -904,8 +904,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
//photometric observation //photometric observation
std::vector<uint8_t> photo_z; std::vector<uint8_t> photo_z;
feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs, cam0_moving_window, photo_z); feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z);
// Convert the feature position from the world frame to // Convert the feature position from the world frame to
// the cam0 and cam1 frame. // the cam0 and cam1 frame.
@ -1316,7 +1315,7 @@ void MsckfVio::removeLostFeatures() {
} }
} }
if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs)) if(!feature.initializeAnchor(cam0_moving_window, cam0))
{ {
invalid_feature_ids.push_back(feature.id); invalid_feature_ids.push_back(feature.id);
continue; continue;
@ -1470,7 +1469,7 @@ void MsckfVio::pruneCamStateBuffer() {
} }
} }
if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs)) if(!feature.initializeAnchor(cam0_moving_window, cam0))
{ {
for (const auto& cam_id : involved_cam_state_ids) for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id); feature.observations.erase(cam_id);