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

removed visu as result works so to not clutter the output

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-19 13:30:30 +02:00
parent 6f16f1b566
commit 1949e4c43d
2 changed files with 27 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
include/msckf_vio/feature.hpp
View File

@ -384,8 +384,10 @@ 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);
//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;
@ -400,26 +402,24 @@ bool Feature::FrameIrradiance(
CameraCalibration& cam0,
std::vector<float>& anchorPatch_measurement) const
{
// project every point in anchorPatch_3d.
// int count = 0;
cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
// visu
/*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)
{
// testing
//if(cam_state_id == observations.begin()->first)
//if(count == 4)
//printf("\n\ncenter:\n");
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
//visualization of feature
cv::Point xs(p_in_c0.x, p_in_c0.y);
// visu
/*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);
@ -432,11 +432,11 @@ bool Feature::FrameIrradiance(
//visu
//cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2));
if(cam0.featureVisu.empty())
/*if(cam0.featureVisu.empty())
cam0.featureVisu = dottedFrame.clone();
else
cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
*/
}
@ -547,10 +547,7 @@ bool Feature::initializeAnchor(
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))
{
printf("no good\n");
return false;
}
}
for(auto point : vec)
anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)point.x,(int)point.y));