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

removed merge conflicts

Browse Source
This commit is contained in:
Raphael Maenle
2019-05-22 11:34:00 +02:00
parent 976108bffe 2aef2089aa
commit 8ff0e9d816
6 changed files with 234 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
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>
@ -191,8 +191,10 @@ struct Feature {
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 takes a pure pixel position (1m from image)
* converts to actual pixel value and returns patch irradiance
@ -203,11 +205,12 @@ struct Feature {
const CameraCalibration& cam,
const StateIDType& cam_state_id,
std::vector<float>& return_i) 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);
/*
@ -553,7 +556,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;
@ -593,45 +598,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
@ -742,7 +809,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

10
include/msckf_vio/image_handler.h
View File

@ -36,6 +36,16 @@ cv::Point2f distortPoint(
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs);
void undistortPoint(
const cv::Point2f& pt_in,
const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs,
cv::Point2f& pt_out,
const cv::Matx33d &rectification_matrix = cv::Matx33d::eye(),
const cv::Vec4d &new_intrinsics = cv::Vec4d(1,1,0,0));
}
}
#endif

4
include/msckf_vio/msckf_vio.h
View File

@ -14,7 +14,7 @@
#include <string>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <math.h>
#include <boost/shared_ptr.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/video.hpp>
@ -38,6 +38,8 @@
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#define PI 3.14159265
namespace msckf_vio {
/*
* @brief MsckfVio Implements the algorithm in