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
Download Patch File Download Diff File Expand all files Collapse all files

17
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();