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

changed visualization

Browse Source
This commit is contained in:
Raphael Maenle
2019-07-12 14:01:11 +02:00
parent 1380ec426f
commit 02156bd89a
4 changed files with 25 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
include/msckf_vio/feature.hpp
View File

@ -224,7 +224,7 @@ bool VisualizeKernel(
bool VisualizePatch(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
CameraCalibration& cam,
const Eigen::VectorXd& photo_r,
std::stringstream& ss) const;
/*
@ -740,7 +740,7 @@ bool Feature::VisualizeKernel(
bool Feature::VisualizePatch(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
CameraCalibration& cam,
const Eigen::VectorXd& photo_r,
std::stringstream& ss) const
{
@ -749,7 +749,7 @@ bool Feature::VisualizePatch(
//visu - anchor
auto anchor = observations.begin();
cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
@ -761,10 +761,10 @@ bool Feature::VisualizePatch(
cv::Point ys(point.x, point.y);
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255));
}
cam0.featureVisu = dottedFrame.clone();
cam.featureVisu = dottedFrame.clone();
// visu - feature
cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
cv::Mat current_image = cam.moving_window.find(cam_state_id)->second.image;
cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
// set position in frame
@ -772,7 +772,7 @@ bool Feature::VisualizePatch(
std::vector<double> projectionPatch;
for(auto point : anchorPatch_3d)
{
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point);
projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image));
// visu - feature
cv::Point xs(p_in_c0.x, p_in_c0.y);
@ -780,7 +780,7 @@ bool Feature::VisualizePatch(
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
}
cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
cv::hconcat(cam.featureVisu, dottedFrame, cam.featureVisu);
// patches visualization
@ -829,7 +829,7 @@ bool Feature::VisualizePatch(
cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
// move to real pixels
p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);
p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs);
for(int i = 0; i<N; i++)
{
@ -853,17 +853,17 @@ bool Feature::VisualizePatch(
for(int i = 0; i<N; i++)
for(int j = 0; j<N; j++)
if(photo_r(i*N+j)>0)
if(photo_r(2*(i*N+j))>0)
cv::rectangle(irradianceFrame,
cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)),
cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)),
cv::Scalar(255 - photo_r(i*N+j)*255, 255 - photo_r(i*N+j)*255, 255),
cv::Scalar(255 - photo_r(2*(i*N+j)+1)*255, 255 - photo_r(2*(i*N+j)+1)*255, 255),
CV_FILLED);
else
cv::rectangle(irradianceFrame,
cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)),
cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)),
cv::Scalar(255, 255 + photo_r(i*N+j)*255, 255 + photo_r(i*N+j)*255),
cv::Scalar(255, 255 + photo_r(2*(i*N+j)+1)*255, 255 + photo_r(2*(i*N+j)+1)*255),
CV_FILLED);
// gradient arrow
@ -884,7 +884,7 @@ bool Feature::VisualizePatch(
3);
*/
cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
cv::hconcat(cam.featureVisu, irradianceFrame, cam.featureVisu);
/*
// visualize position of used observations and resulting feature position
@ -916,15 +916,15 @@ bool Feature::VisualizePatch(
// draw, x y position and arrow with direction - write z next to it
cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
cv::resize(cam.featureVisu, cam.featureVisu, cv::Size(), rescale, rescale);
cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu);
cv::hconcat(cam.featureVisu, positionFrame, cam.featureVisu);
*/
// write feature position
std::stringstream pos_s;
pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
cv::putText(cam0.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30),
cv::putText(cam.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30),
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA);
// create line?
@ -932,9 +932,9 @@ bool Feature::VisualizePatch(
std::stringstream loc;
// loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
//cv::imwrite(loc.str(), cam0.featureVisu);
//cv::imwrite(loc.str(), cam.featureVisu);
cv::imshow("patch", cam0.featureVisu);
cv::imshow("patch", cam.featureVisu);
cvWaitKey(1);
}