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

minor output changes, added arrows for gradient and residual visualization

Browse Source
This commit is contained in:
Raphael Maenle
2019-05-21 13:34:58 +02:00
parent 9c7f67d2fd
commit 0f96c916f1
4 changed files with 151 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

129
include/msckf_vio/feature.hpp
View File

@ -15,7 +15,7 @@
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <math.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/Point.h>
@ -182,12 +182,14 @@ bool MarkerGeneration(
const StateIDType& cam_state_id,
CameraCalibration& cam0,
const std::vector<double> photo_r,
std::stringstream& ss) const;
std::stringstream& ss,
cv::Point2f gradientVector,
cv::Point2f residualVector) const;
/*
* @brief projectPixelToPosition uses the calcualted pixels
* @brief AnchorPixelToPosition uses the calcualted pixels
* of the anchor patch to generate 3D positions of all of em
*/
inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p,
const CameraCalibration& cam);
/*
@ -533,7 +535,9 @@ bool Feature::VisualizePatch(
const StateIDType& cam_state_id,
CameraCalibration& cam0,
const std::vector<double> photo_r,
std::stringstream& ss) const
std::stringstream& ss,
cv::Point2f gradientVector,
cv::Point2f residualVector) const
{
double rescale = 1;
@ -573,45 +577,107 @@ bool Feature::VisualizePatch(
cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
// irradiance grid anchor
// patches visualization
int N = sqrt(anchorPatch_3d.size());
int scale = 20;
int scale = 30;
cv::Mat irradianceFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
cv::resize(irradianceFrame, irradianceFrame, cv::Size(), rescale, rescale);
// irradiance grid anchor
std::stringstream namer;
namer << "anchor";
cv::putText(irradianceFrame, namer.str() , cvPoint(30, 25),
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
for(int i = 0; i<N; i++)
for(int j = 0; j<N; j++)
cv::rectangle(irradianceFrame,
cv::Point(10+scale*(i+1), 10+scale*j),
cv::Point(10+scale*i, 10+scale*(j+1)),
cv::Point(30+scale*(i+1), 30+scale*j),
cv::Point(30+scale*i, 30+scale*(j+1)),
cv::Scalar(anchorPatch[i*N+j]*255, anchorPatch[i*N+j]*255, anchorPatch[i*N+j]*255),
CV_FILLED);
// irradiance grid projection
namer.str(std::string());
namer << "projection";
cv::putText(irradianceFrame, namer.str() , cvPoint(30, 45+scale*N),
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
for(int i = 0; i<N; i++)
for(int j = 0; j<N ; j++)
cv::rectangle(irradianceFrame,
cv::Point(10+scale*(i+1), 20+scale*(N+j)),
cv::Point(10+scale*(i), 20+scale*(N+j+1)),
cv::Point(30+scale*(i+1), 50+scale*(N+j)),
cv::Point(30+scale*(i), 50+scale*(N+j+1)),
cv::Scalar(projectionPatch[i*N+j]*255, projectionPatch[i*N+j]*255, projectionPatch[i*N+j]*255),
CV_FILLED);
// true irradiance at feature
// get current observation
namer.str(std::string());
namer << "feature";
cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N),
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
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);
for(int i = 0; i<N; i++)
{
for(int j = 0; j<N ; j++)
{
float irr = PixelIrradiance(cv::Point2f(p_f.x + (i-(N-1)/2), p_f.y + (j-(N-1)/2)), current_image);
cv::rectangle(irradianceFrame,
cv::Point(30+scale*(i+1), 70+scale*(2*N+j)),
cv::Point(30+scale*(i), 70+scale*(2*N+j+1)),
cv::Scalar(irr*255, irr*255, irr*255),
CV_FILLED);
}
}
// residual grid projection, positive - red, negative - blue colored
namer.str(std::string());
namer << "residual";
cv::putText(irradianceFrame, namer.str() , cvPoint(30+scale*N, scale*N/2-5),
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
for(int i = 0; i<N; i++)
for(int j = 0; j<N; j++)
if(photo_r[i*N+j]>0)
cv::rectangle(irradianceFrame,
cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)),
cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)),
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_FILLED);
else
cv::rectangle(irradianceFrame,
cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)),
cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)),
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_FILLED);
cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
// gradient arrow
/*
cv::arrowedLine(irradianceFrame,
cv::Point(30+scale*(N/2 +0.5), 50+scale*(N+(N/2)+0.5)),
cv::Point(30+scale*(N/2+0.5)+scale*gradientVector.x, 50+scale*(N+(N/2)+0.5)+scale*gradientVector.y),
cv::Scalar(100, 0, 255),
1);
*/
// residual gradient direction
cv::arrowedLine(irradianceFrame,
cv::Point(40+scale*(N+N/2+0.5), 15+scale*((N-0.5))),
cv::Point(40+scale*(N+N/2+0.5)+scale*residualVector.x, 15+scale*(N-0.5)+scale*residualVector.y),
cv::Scalar(0, 255, 175),
3);
cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
/*
// visualize position of used observations and resulting feature position
@ -703,7 +769,7 @@ cv::Point2f Feature::projectPositionToCamera(
return my_p;
}
Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
{
// use undistorted position of point of interest
// project it back into 3D space using pinhole model
@ -742,27 +808,19 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v),
cam.intrinsics,
cam.distortion_model,
0);
cam.distortion_coeffs);
// create vector of patch in pixel plane
std::vector<cv::Point2f>und_pix_v;
for(double u_run = -n; u_run <= n; u_run++)
for(double v_run = -n; v_run <= n; v_run++)
und_pix_v.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
//create undistorted pure points
std::vector<cv::Point2f> und_v;
image_handler::undistortPoints(und_pix_v,
image_handler::undistortPoints(anchorPatch_real,
cam.intrinsics,
cam.distortion_model,
0,
und_v);
//create distorted pixel points
anchorPatch_real = image_handler::distortPoints(und_v,
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs);
cam.distortion_coeffs,
anchorPatch_ideal);
// save anchor position for later visualisaztion
@ -770,19 +828,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
// save true pixel Patch position
for(auto point : anchorPatch_real)
{
if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
return false;
}
for(auto point : anchorPatch_real)
anchorPatch.push_back(PixelIrradiance(point, anchorImage));
// project patch pixel to 3D space in camera coordinate system
for(auto point : und_v)
{
anchorPatch_ideal.push_back(point);
anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
}
for(auto point : anchorPatch_ideal)
anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam));
is_anchored = true;
return true;
}