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,
const Eigen::Vector3d& x, const Eigen::Vector2d& z,
double& e) const {
double& e) const
{
// Compute hi1, hi2, and hi3 as Equation (37).
const double& alpha = x(0);
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,
const Eigen::Vector3d& x, const Eigen::Vector2d& z,
Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r,
double& w) const {
double& w) const
{
// Compute hi1, hi2, and hi3 as Equation (37).
const double& alpha = x(0);
@ -300,7 +302,8 @@ void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
void Feature::generateInitialGuess(
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.
Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0);
@ -320,8 +323,8 @@ void Feature::generateInitialGuess(
return;
}
bool Feature::checkMotion(
const CamStateServer& cam_states) const {
bool Feature::checkMotion(const CamStateServer& cam_states) const
{
const StateIDType& first_cam_id = observations.begin()->first;
const StateIDType& last_cam_id = (--observations.end())->first;
@ -399,7 +402,6 @@ bool Feature::estimate_FrameIrradiance(
float irradiance = (anchorPixel - b_A) / a_A ;
anchorPatch_estimate.push_back(irradiance);
}
}
@ -441,6 +443,7 @@ bool Feature::VisualizePatch(
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
{
return ((float)image.at<uint8_t>(pose.x, pose.y))/256;
}
@ -476,8 +479,7 @@ cv::Point2f Feature::projectPositionToCamera(
return my_p;
}
Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
const CameraCalibration& cam)
Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
{
// use undistorted position of point of interest
// 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
bool Feature::initializeAnchor(
const CameraCalibration& cam, int N)
bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
{
//initialize patch Size
@ -532,7 +533,7 @@ bool Feature::initializeAnchor(
cam.distortion_model,
0,
und_v);
//create distorted pixel points
//create distorted pixel points
std::vector<cv::Point2f> vec = image_handler::distortPoints(und_v,
cam.intrinsics,
cam.distortion_model,
@ -561,8 +562,7 @@ bool Feature::initializeAnchor(
return true;
}
bool Feature::initializePosition(
const CamStateServer& cam_states) {
bool Feature::initializePosition(const CamStateServer& cam_states) {
// Organize camera poses and feature observations properly.
std::vector<Eigen::Isometry3d,
@ -711,6 +711,7 @@ bool Feature::initializePosition(
return is_valid_solution;
}
} // namespace msckf_vio
#endif // MSCKF_VIO_FEATURE_H