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

Jakobi Calculation done

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-24 15:30:25 +02:00
parent 5958adb57c
commit 1ffc4fb37f
2 changed files with 105 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
include/msckf_vio/feature.hpp
View File

@ -165,7 +165,8 @@ struct Feature {
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
std::vector<float>& anchorPatch_estimate) const;
std::vector<double>& anchorPatch_estimate,
IlluminationParameter& estimatedIllumination) const;
bool VisualizePatch(
const CAMState& cam_state,
@ -202,6 +203,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
// NxN Patch of Anchor Image
std::vector<double> anchorPatch;
std::vector<cv::Point2f> anchorPatch_ideal;
// Position of NxN Patch in 3D space
std::vector<Eigen::Vector3d> anchorPatch_3d;
@ -365,7 +367,8 @@ bool Feature::estimate_FrameIrradiance(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
std::vector<float>& anchorPatch_estimate) const
std::vector<double>& anchorPatch_estimate,
IlluminationParameter& estimated_illumination) const
{
// get irradiance of patch in anchor frame
// subtract estimated b and divide by a of anchor frame
@ -381,15 +384,19 @@ bool Feature::estimate_FrameIrradiance(
double a_A = anchorExposureTime_ms;
double b_A = 0;
double a_l =frameExposureTime_ms;
double a_l = frameExposureTime_ms;
double b_l = 0;
estimated_illumination.frame_gain = a_l;
estimated_illumination.frame_bias = b_l;
estimated_illumination.feature_gain = 1;
estimated_illumination.feature_bias = 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;
float irradiance = (anchorPixel - b_A) / a_A ;
anchorPatch_estimate.push_back(irradiance);
}
@ -536,6 +543,8 @@ bool Feature::initializeAnchor(
// save anchor position for later visualisaztion
anchor_center_pos = vec[4];
// save true pixel Patch position
for(auto point : vec)
{
if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
@ -546,8 +555,10 @@ bool Feature::initializeAnchor(
// project patch pixel to 3D space
for(auto point : und_v)
{
anchorPatch_ideal.push_back(point);
anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
}
is_anchored = true;
return true;
}