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

added jakobi x calculation, y calculation (of photometric update) still missing

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-23 19:16:46 +02:00
parent 8defb20c8e
commit 5958adb57c
3 changed files with 109 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
include/msckf_vio/feature.hpp
View File

@ -167,11 +167,10 @@ struct Feature {
CameraCalibration& cam0,
std::vector<float>& anchorPatch_estimate) const;
bool FrameIrradiance(
bool VisualizePatch(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
std::vector<float>& anchorPatch_measurement) const;
CameraCalibration& cam0) const;
/*
* @brief projectPixelToPosition uses the calcualted pixels
@ -396,48 +395,41 @@ bool Feature::estimate_FrameIrradiance(
}
bool Feature::FrameIrradiance(
bool Feature::VisualizePatch(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
std::vector<float>& anchorPatch_measurement) const
CameraCalibration& cam0) const
{
// visu - feature
/*cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
cv::Mat dottedFrame(current_image.size(), CV_8UC3);
cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
*/
// project every point in anchorPatch_3d.
for (auto point : anchorPatch_3d)
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
for(auto point : anchorPatch_3d)
{
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
// visu - feature
/*cv::Point xs(p_in_c0.x, p_in_c0.y);
cv::Point xs(p_in_c0.x, p_in_c0.y);
cv::Point ys(p_in_c0.x, p_in_c0.y);
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
*/
float irradiance = PixelIrradiance(p_in_c0, cam0.moving_window.find(cam_state_id)->second.image);
anchorPatch_measurement.push_back(irradiance);
// testing
//if(cam_state_id == observations.begin()->first)
//if(count++ == 4)
//printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y);
}
}
// testing
//if(cam_state_id == observations.begin()->first)
//if(count++ == 4)
//printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y);
// visu - feature
//cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2));
/*if(cam0.featureVisu.empty())
cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2));
if(cam0.featureVisu.empty())
cam0.featureVisu = dottedFrame.clone();
else
cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
*/
}
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const