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
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(
const movingWindow& cam0_moving_window,
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs);
const CameraCalibration& cam);
/*
@@ -150,9 +148,7 @@ struct Feature {
inline cv::Point2f projectPositionToCamera(
const CAMState& cam_state,
const StateIDType& cam_state_id,
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs,
const CameraCalibration& cam,
Eigen::Vector3d& in_p) const;
/*
@@ -164,9 +160,7 @@ struct Feature {
bool IrradianceOfAnchorPatch(
const CAMState& cam_state,
const StateIDType& cam_state_id,
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs,
const CameraCalibration& cam,
const movingWindow& cam0_moving_window,
std::vector<uint8_t>& anchorPatch_measurement) const;
@@ -174,11 +168,8 @@ struct Feature {
* @brief projectPixelToPosition uses the calcualted pixels
* of the anchor patch to generate 3D positions of all of em
*/
inline bool projectPixelToPosition(cv::Point2f in_p,
Eigen::Vector3d& out_p,
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs);
inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
const CameraCalibration& cam);
/*
* @brief Irradiance returns irradiance value of a pixel
@@ -362,16 +353,14 @@ bool Feature::checkMotion(
bool Feature::IrradianceOfAnchorPatch(
const CAMState& cam_state,
const StateIDType& cam_state_id,
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs,
const CameraCalibration& cam,
const movingWindow& cam0_moving_window,
std::vector<uint8_t>& anchorPatch_measurement) const
{
//project every point in 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);
anchorPatch_measurement.push_back(irradiance);
}
@@ -385,9 +374,7 @@ uint8_t Feature::Irradiance(cv::Point2f pose, cv::Mat image) const
cv::Point2f Feature::projectPositionToCamera(
const CAMState& cam_state,
const StateIDType& cam_state_id,
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs,
const CameraCalibration& cam,
Eigen::Vector3d& in_p) const
{
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));
std::vector<cv::Point2f> out_v;
out_v.push_back(out_p);
std::vector<cv::Point2f> my_p = image_handler::distortPoints( out_v,
intrinsics,
distortion_model,
distortion_coeffs);
std::vector<cv::Point2f> my_p = image_handler::distortPoints(out_v, 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 out_p;
return my_p[0];
}
bool Feature::projectPixelToPosition(cv::Point2f in_p,
Eigen::Vector3d& out_p,
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs)
Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
const CameraCalibration& cam)
{
// use undistorted position of point of interest
// 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 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]);
}
@@ -434,9 +415,7 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p,
//@test center projection must always be initial feature projection
bool Feature::initializeAnchor(
const movingWindow& cam0_moving_window,
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs)
const CameraCalibration& cam)
{
int N = 3;
@@ -447,8 +426,8 @@ bool Feature::initializeAnchor(
return false;
cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second;
auto u = anchor->second(0)*intrinsics[0] + intrinsics[2];
auto v = anchor->second(1)*intrinsics[1] + intrinsics[3];
auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2];
auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3];
int count = 0;
//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)
{
anchorPatch.push_back(anchorImage.at<uint8_t>((int)u_run,(int)v_run));
Eigen::Vector3d Npose;
projectPixelToPosition(cv::Point2f((u_run-intrinsics[2])/intrinsics[0], (v_run-intrinsics[3])/intrinsics[1]),
Npose,
intrinsics,
distortion_model,
distortion_coeffs);
cv::Point2f currentPoint((u_run-cam.intrinsics[2])/cam.intrinsics[0], (v_run-cam.intrinsics[3])/cam.intrinsics[1]);
Eigen::Vector3d Npose = projectPixelToPosition(currentPoint, cam);
anchorPatch_3d.push_back(Npose);
}
}

7
src/msckf_vio.cpp
View File

@@ -904,8 +904,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
//photometric observation
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
// 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);
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)
feature.observations.erase(cam_id);