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

corrected pixel distance calcualtion

Browse Source
This commit is contained in:
Raphael Maenle 2019-05-24 09:49:47 +02:00
parent 82cd2c6771
commit 8bebf99c37
3 changed files with 24 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
include/msckf_vio/feature.hpp
View File

@ -788,32 +788,24 @@ cv::Point2f Feature::pixelDistanceAt(
Eigen::Vector3d& in_p) const
{
Eigen::Isometry3d T_c0_w;
cv::Point2f out_p;
cv::Point2f cam_p = projectPositionToCamera(cam_state, cam_state_id, cam, in_p);
cv::Point2f surroundingPoint;
// create vector of patch in pixel plane
surroundingPoint = cv::Point2f(cam_p.x+1, cam_p.y+1);
std::vector<cv::Point2f> surroundingPoints;
surroundingPoints.push_back(cv::Point2f(cam_p.x+1, cam_p.y));
surroundingPoints.push_back(cv::Point2f(cam_p.x-1, cam_p.y));
surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y+1));
surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y-1));
cv::Point2f pure_surroundingPoint;
image_handler::undistortPoint(surroundingPoint,
std::vector<cv::Point2f> pure;
image_handler::undistortPoints(surroundingPoints,
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs,
pure_surroundingPoint);
pure);
cv::Point2f pure_Point;
image_handler::undistortPoint(cam_p,
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs,
pure_Point);
cv::Point2f distance(fabs(pure_surroundingPoint.x - pure_Point.x), fabs(pure_surroundingPoint.y - pure_Point.y));
// returns the absolute pixel distance at pixels one metres away
cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y));
return distance;
}

2
launch/msckf_vio_tum.launch
View File

@ -21,7 +21,7 @@
<param name="PHOTOMETRIC" value="true"/>
<!-- Debugging Flaggs -->
<param name="PrintImages" value="true"/>
<param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="3"/>

15
src/msckf_vio.cpp
View File

@ -1225,6 +1225,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
const CAMState& cam_state = state_server.cam_states[cam_state_id];
const Feature& feature = map_server[feature_id];
const StateIDType anchor_state_id = feature.observations.begin()->first;
const CAMState anchor_state = state_server.cam_states[anchor_state_id];
// Cam0 pose.
Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
const Vector3d& t_c0_w = cam_state.position;
@ -1253,16 +1256,19 @@ void MsckfVio::PhotometricMeasurementJacobian(
{
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point);
Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
// add jacobians
cv::Point2f pixelDistance = feature.pixelDistanceAt(anchor_state, anchor_state_id, cam0, point);
cv::Point2f pixelDistance = feature.pixelDistanceAt(cam_state, cam_state_id, cam0, point);
std::cout << "pixelDistance: \n" << pixelDistance << std::endl;
// calculate derivation for anchor frame, use position for derivation calculation
// frame derivative calculated convoluting with kernel [-1, 0, 1]
dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame);
dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame);
dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), frame);
dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), frame);
dI_dhj(0, 0) = dx/pixelDistance.x;
dI_dhj(0, 1) = dy/pixelDistance.y;
@ -1690,6 +1696,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
}
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
return true;
MatrixXd P1 = H * state_server.state_cov * H.transpose();