diff --git a/GPATH b/GPATH new file mode 100644 index 0000000..de48055 Binary files /dev/null and b/GPATH differ diff --git a/GRTAGS b/GRTAGS new file mode 100644 index 0000000..6894142 Binary files /dev/null and b/GRTAGS differ diff --git a/GSYMS b/GSYMS new file mode 100644 index 0000000..65789a0 Binary files /dev/null and b/GSYMS differ diff --git a/GTAGS b/GTAGS new file mode 100644 index 0000000..d31c9ce Binary files /dev/null and b/GTAGS differ diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index ce090ae..fff17b8 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -148,6 +148,14 @@ struct Feature { inline bool initializePosition( const CamStateServer& cam_states); + cv::Point2f pixelDistanceAt( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const CameraCalibration& cam, + Eigen::Vector3d& in_p) const; + + + /* * @brief project PositionToCamera Takes a 3d position in a world frame * and projects it into the passed camera frame using pinhole projection @@ -737,6 +745,37 @@ float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const return ((float)image.at(pose.y, pose.x))/255; } +cv::Point2f Feature::pixelDistanceAt( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const CameraCalibration& cam, + Eigen::Vector3d& in_p) const +{ + + cv::Point2f cam_p = projectPositionToCamera(cam_state, cam_state_id, cam, in_p); + + // create vector of patch in pixel plane + std::vector 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)); + + std::vector pure; + image_handler::undistortPoints(surroundingPoints, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs, + pure); + + // 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; +} + + + cv::Point2f Feature::projectPositionToCamera( const CAMState& cam_state, const StateIDType& cam_state_id, diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 9e103b7..1d458d2 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,13 +18,13 @@ output="screen"> - + - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index b233848..d56d1a9 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -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; @@ -1263,7 +1266,7 @@ void MsckfVio::PhotometricMeasurementJacobian( Eigen::MatrixXd H_pA(N*N, 6); auto frame = cam0.moving_window.find(cam_state_id)->second.image; - + auto anchor_frame = cam0.moving_window.find(anchor_state_id)->second.image; //observation const Vector4d& z = feature.observations.find(cam_state_id)->second; @@ -1291,8 +1294,10 @@ void MsckfVio::PhotometricMeasurementJacobian( for (auto point : feature.anchorPatch_3d) { + //cout << "____feature-measurement_____\n" << endl; 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); //add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); @@ -1300,13 +1305,17 @@ void MsckfVio::PhotometricMeasurementJacobian( //calculate photom. residual photo_r.push_back(photo_z[count] - estimate_photo_z[count]); - // add jacobians + //cout << "residual: " << photo_r.back() << endl; + // add jacobians + cv::Point2f pixelDistance = feature.pixelDistanceAt(anchor_state, anchor_state_id, cam0, point); + + // 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); - dI_dhj(0, 0) = dx; - dI_dhj(0, 1) = dy; + dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); + dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); + dI_dhj(0, 0) = dx/pixelDistance.x; + dI_dhj(0, 1) = dy/pixelDistance.y; gradientVector.x += dx; gradientVector.y += dy; @@ -1332,8 +1341,9 @@ void MsckfVio::PhotometricMeasurementJacobian( dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta; dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC; - //d{}^Gp_P{ij} / \rho_i + // d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; + // Isometry T_anchor_w takes a vector in anchor frame to world frame dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); @@ -1345,13 +1355,18 @@ void MsckfVio::PhotometricMeasurementJacobian( // Intermediate Jakobians H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1 - H_plj = dI_dhj * dh_dXplj; // 1 x 6 - H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 + H_plj = dI_dhj * dh_dXplj; // 1 x 6 + H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; H_pA.block<1, 6>(count, 0) = H_pAj; + //cout << "H_pl\n" << H_plj << endl; + + //cout << "H_pA\n" << H_pAj << endl; + + //cout << "H_rho\n" << H_rhoj << endl; count++; } @@ -1491,15 +1506,18 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd A = svd_helper.matrixU().rightCols(jacobian_row_size - singularValues.size()); */ + H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; -/* - ofstream myfile; - myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl; - myfile.close(); - cout << "---------- LOGGED -------- " << endl; -*/ + + if(PRINTIMAGES) + { + ofstream myfile; + myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); + myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; + myfile.close(); + cout << "---------- LOGGED -------- " << endl; + } if(PRINTIMAGES) { std::cout << "resume playback" << std::endl; @@ -1642,7 +1660,7 @@ void MsckfVio::featureJacobian( ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.ldlt().solve(r) << endl; + myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; myfile.close(); cout << "---------- LOGGED -------- " << endl; return; @@ -1866,7 +1884,7 @@ void MsckfVio::removeLostFeatures() { else featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) { + if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); @@ -2028,7 +2046,7 @@ void MsckfVio::pruneCamStateBuffer() { else featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) { + if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows();