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

deactivated most to find reason for slowdown

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-17 16:16:45 +02:00
parent 6ae83f0db7
commit f4a17f8512
2 changed files with 123 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
include/msckf_vio/feature.hpp
View File

@ -157,12 +157,19 @@ struct Feature {
*
*/
bool IrradianceOfAnchorPatch(
bool estimate_FrameIrradiance(
const CAMState& cam_state,
const StateIDType& cam_state_id,
const CameraCalibration& cam,
const CameraCalibration& cam0,
const movingWindow& cam0_moving_window,
std::vector<uint8_t>& anchorPatch_measurement) const;
std::vector<float>& anchorPatch_estimate) const;
bool FrameIrradiance(
const CAMState& cam_state,
const StateIDType& cam_state_id,
const CameraCalibration& cam0,
const movingWindow& cam0_moving_window,
std::vector<float>& anchorPatch_measurement) const;
/*
* @brief projectPixelToPosition uses the calcualted pixels
@ -175,7 +182,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
* @brief Irradiance returns irradiance value of a pixel
*/
inline uint8_t Irradiance(cv::Point2f pose, cv::Mat image) const;
inline float PixelIrradiance(cv::Point2f pose, cv::Mat image) const;
// An unique identifier for the feature.
// In case of long time running, the variable
@ -350,25 +357,56 @@ bool Feature::checkMotion(
else return false;
}
bool Feature::IrradianceOfAnchorPatch(
bool Feature::estimate_FrameIrradiance(
const CAMState& cam_state,
const StateIDType& cam_state_id,
const CameraCalibration& cam,
const CameraCalibration& cam0,
const movingWindow& cam0_moving_window,
std::vector<uint8_t>& anchorPatch_measurement) const
std::vector<float>& anchorPatch_estimate) const
{
// get irradiance of patch in anchor frame
// subtract estimated b and divide by a of anchor frame
// muliply by a and add b of this frame
auto anchor = observations.begin();
if(cam0_moving_window.find(anchor->first) == cam0_moving_window.end())
return false;
double anchorExposureTime_ms = cam0_moving_window.find(anchor->first)->second.exposureTime_ms;
double frameExposureTime_ms = cam0_moving_window.find(cam_state_id)->second.exposureTime_ms;
double a_A = anchorExposureTime_ms;
double b_A = 0;
double a_l =frameExposureTime_ms;
double b_l = 0;
for (double anchorPixel : anchorPatch)
{
float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l;
anchorPatch_estimate.push_back(irradiance);
}
}
bool Feature::FrameIrradiance(
const CAMState& cam_state,
const StateIDType& cam_state_id,
const CameraCalibration& cam0,
const movingWindow& cam0_moving_window,
std::vector<float>& 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, cam, point);
uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second.image);
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);
}
}
uint8_t Feature::Irradiance(cv::Point2f pose, cv::Mat image) const
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
{
return image.at<uint8_t>(pose.x, pose.y);
return (float)image.at<uint8_t>(pose.x, pose.y);
}
cv::Point2f Feature::projectPositionToCamera(
@ -389,7 +427,10 @@ 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, cam.intrinsics, cam.distortion_model, cam.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));
@ -418,6 +459,8 @@ bool Feature::initializeAnchor(
const CameraCalibration& cam)
{
//initialize patch Size
//TODO make N size a ros parameter
int N = 3;
int n = (int)(N-1)/2;
@ -428,25 +471,32 @@ bool Feature::initializeAnchor(
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];
int count = 0;
//go through surrounding pixels
//for NxN patch pixels around feature
for(double u_run = u - n; u_run <= u + n; u_run = u_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));
cv::Point2f currentPoint((u_run-cam.intrinsics[2])/cam.intrinsics[0], (v_run-cam.intrinsics[3])/cam.intrinsics[1]);
// 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);
}
}
//TODO test if NxN patch can be selected
return true;
}
bool Feature::initializePosition(
const CamStateServer& cam_states) {
// Organize camera poses and feature observations properly.
std::vector<Eigen::Isometry3d,
Eigen::aligned_allocator<Eigen::Isometry3d> > cam_poses(0);