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

added tum launch files, removed anchor procedure being called multiple times through a flag

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-18 11:06:45 +02:00
parent cfecefe29f
commit d91ff7ca9d
6 changed files with 170 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
include/msckf_vio/feature.hpp
View File

@ -59,11 +59,11 @@ struct Feature {
// Constructors for the struct.
Feature(): id(0), position(Eigen::Vector3d::Zero()),
is_initialized(false) {}
is_initialized(false), is_anchored(false) {}
Feature(const FeatureIDType& new_id): id(new_id),
position(Eigen::Vector3d::Zero()),
is_initialized(false) {}
is_initialized(false), is_anchored(false) {}
/*
* @brief cost Compute the cost of the camera observations
@ -217,7 +217,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
// A indicator to show if the 3d postion of the feature
// has been initialized or not.
bool is_initialized;
bool is_anchored;
// Noise for a normalized feature measurement.
static double observation_noise;
@ -380,6 +380,8 @@ bool Feature::estimate_FrameIrradiance(
double b_A = 0;
double a_l =frameExposureTime_ms;
double b_l = 0;
printf("frames: %lld, %lld\n", anchor->first, cam_state_id);
printf("exposure: %f, %f\n", a_A, a_l);
for (double anchorPixel : anchorPatch)
{
float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l;
@ -396,6 +398,8 @@ bool Feature::FrameIrradiance(
std::vector<float>& anchorPatch_measurement) const
{
//project every point in anchorPatch_3d.
if(!is_anchored)
printf("not anchored!\n");
for (auto point : anchorPatch_3d)
{
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
@ -471,12 +475,19 @@ 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];
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;
}
//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)
{
printf("ADDING\n");
// add irradiance information
anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)u_run,(int)v_run));
@ -489,8 +500,8 @@ bool Feature::initializeAnchor(
anchorPatch_3d.push_back(Npose);
}
}
//TODO test if NxN patch can be selected
printf("set to true!!!\n");
is_anchored = true;
return true;
}