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

minor changes to visualization, jakobi tests

Browse Source
This commit is contained in:
Raphael Maenle
2019-05-03 16:42:27 +02:00
parent ee40dff740
commit 53b26f7613
4 changed files with 172 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
include/msckf_vio/feature.hpp
View File

@ -172,7 +172,8 @@ struct Feature {
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
const std::vector<double> photo_r) const;
const std::vector<double> photo_r,
std::stringstream& ss) const;
/*
* @brief projectPixelToPosition uses the calcualted pixels
* of the anchor patch to generate 3D positions of all of em
@ -410,7 +411,8 @@ bool Feature::VisualizePatch(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
const std::vector<double> photo_r) const
const std::vector<double> photo_r,
std::stringstream& ss) const
{
double rescale = 1;
@ -421,14 +423,15 @@ bool Feature::VisualizePatch(
cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
// visualize the true anchor points (the surrounding of the original measurements)
for(auto point : anchorPatch_real)
{
// visu - feature
cv::Point xs(point.x, point.y);
cv::Point ys(point.x, point.y);
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255));
}
cam0.featureVisu = dottedFrame.clone();
cam0.featureVisu = dottedFrame.clone();
// visu - feature
cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
@ -487,23 +490,52 @@ bool Feature::VisualizePatch(
cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255),
CV_FILLED);
// visualize position of used observations and resulting feature position
cv::Mat positionFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
cv::resize(positionFrame, positionFrame, cv::Size(), rescale, rescale);
// draw world zero
cv::line(positionFrame,
cv::Point(20,20),
cv::Point(20,30),
cv::Scalar(0,0,255),
CV_FILLED);
cv::line(positionFrame,
cv::Point(20,20),
cv::Point(30,20),
cv::Scalar(255,0,0),
CV_FILLED);
// for every observation, get cam state
for(auto obs : observations)
{
cam_state.find(obs->first);
cv::line(positionFrame,
cv::Point(20,20),
cv::Point(30,20),
cv::Scalar(255,0,0),
CV_FILLED);
}
// draw, x y position and arrow with direction - write z next to it
cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu);
// write feature position
std::stringstream pos_s;
pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
cv::putText(cam0.featureVisu, pos_s.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30),
cv::putText(cam0.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?
//save image
std::stringstream ss;
ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
cv::imwrite(ss.str(), cam0.featureVisu);
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::imshow("patch", cam0.featureVisu);
//cvWaitKey(1);