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

sublime folding changes

Browse Source
This commit is contained in:
Raphael Maenle 2019-04-26 14:42:31 +02:00
parent e3ac604dbf
commit 750d8ecfb1
1 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
include/msckf_vio/feature.hpp
View File

@ -240,7 +240,8 @@ typedef std::map<FeatureIDType, Feature, std::less<int>,
void Feature::cost(const Eigen::Isometry3d& T_c0_ci, void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
const Eigen::Vector3d& x, const Eigen::Vector2d& z, const Eigen::Vector3d& x, const Eigen::Vector2d& z,
double& e) const { double& e) const
{
// Compute hi1, hi2, and hi3 as Equation (37). // Compute hi1, hi2, and hi3 as Equation (37).
const double& alpha = x(0); const double& alpha = x(0);
const double& beta = x(1); const double& beta = x(1);
@ -263,7 +264,8 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci, void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
const Eigen::Vector3d& x, const Eigen::Vector2d& z, const Eigen::Vector3d& x, const Eigen::Vector2d& z,
Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r, Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r,
double& w) const { double& w) const
{
// Compute hi1, hi2, and hi3 as Equation (37). // Compute hi1, hi2, and hi3 as Equation (37).
const double& alpha = x(0); const double& alpha = x(0);
@ -300,7 +302,8 @@ void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
void Feature::generateInitialGuess( void Feature::generateInitialGuess(
const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1, const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
const Eigen::Vector2d& z2, Eigen::Vector3d& p) const { const Eigen::Vector2d& z2, Eigen::Vector3d& p) const
{
// Construct a least square problem to solve the depth. // Construct a least square problem to solve the depth.
Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0); Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0);
@ -320,8 +323,8 @@ void Feature::generateInitialGuess(
return; return;
} }
bool Feature::checkMotion( bool Feature::checkMotion(const CamStateServer& cam_states) const
const CamStateServer& cam_states) const { {
const StateIDType& first_cam_id = observations.begin()->first; const StateIDType& first_cam_id = observations.begin()->first;
const StateIDType& last_cam_id = (--observations.end())->first; const StateIDType& last_cam_id = (--observations.end())->first;
@ -399,7 +402,6 @@ bool Feature::estimate_FrameIrradiance(
float irradiance = (anchorPixel - b_A) / a_A ; float irradiance = (anchorPixel - b_A) / a_A ;
anchorPatch_estimate.push_back(irradiance); anchorPatch_estimate.push_back(irradiance);
} }
} }
@ -441,6 +443,7 @@ bool Feature::VisualizePatch(
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
{ {
return ((float)image.at<uint8_t>(pose.x, pose.y))/256; return ((float)image.at<uint8_t>(pose.x, pose.y))/256;
} }
@ -476,8 +479,7 @@ cv::Point2f Feature::projectPositionToCamera(
return my_p; return my_p;
} }
Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
const CameraCalibration& cam)
{ {
// use undistorted position of point of interest // use undistorted position of point of interest
// project it back into 3D space using pinhole model // project it back into 3D space using pinhole model
@ -490,8 +492,7 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
} }
//@test center projection must always be initial feature projection //@test center projection must always be initial feature projection
bool Feature::initializeAnchor( bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
const CameraCalibration& cam, int N)
{ {
//initialize patch Size //initialize patch Size
@ -532,7 +533,7 @@ bool Feature::initializeAnchor(
cam.distortion_model, cam.distortion_model,
0, 0,
und_v); und_v);
//create distorted pixel points //create distorted pixel points
std::vector<cv::Point2f> vec = image_handler::distortPoints(und_v, std::vector<cv::Point2f> vec = image_handler::distortPoints(und_v,
cam.intrinsics, cam.intrinsics,
cam.distortion_model, cam.distortion_model,
@ -561,8 +562,7 @@ bool Feature::initializeAnchor(
return true; return true;
} }
bool Feature::initializePosition( bool Feature::initializePosition(const CamStateServer& cam_states) {
const CamStateServer& cam_states) {
// Organize camera poses and feature observations properly. // Organize camera poses and feature observations properly.
std::vector<Eigen::Isometry3d, std::vector<Eigen::Isometry3d,
@ -711,6 +711,7 @@ bool Feature::initializePosition(
return is_valid_solution; return is_valid_solution;
} }
} // namespace msckf_vio } // namespace msckf_vio
#endif // MSCKF_VIO_FEATURE_H #endif // MSCKF_VIO_FEATURE_H