diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index b555767..0e8979c 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -157,6 +157,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 @@ -191,9 +199,7 @@ struct Feature { const StateIDType& cam_state_id, CameraCalibration& cam0, const std::vector photo_r, - std::stringstream& ss, - cv::Point2f gradientVector, - cv::Point2f residualVector) const; + std::stringstream& ss) const; /* @brief takes a pure pixel position (1m from image) @@ -407,8 +413,11 @@ bool Feature::estimate_FrameIrradiance( auto anchor = observations.begin(); if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end()) + { + std::cout << "anchor not in buffer anymore!" << std::endl; return false; - + } + double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms; double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms; @@ -550,9 +559,7 @@ bool Feature::VisualizePatch( const StateIDType& cam_state_id, CameraCalibration& cam0, const std::vector photo_r, - std::stringstream& ss, - cv::Point2f gradientVector, - cv::Point2f residualVector) const + std::stringstream& ss) const { double rescale = 1; @@ -615,10 +622,9 @@ bool Feature::VisualizePatch( CV_FILLED); // irradiance grid projection - namer.str(std::string()); namer << "projection"; - cv::putText(irradianceFrame, namer.str() , cvPoint(30, 45+scale*N), + cv::putText(irradianceFrame, namer.str(), cvPoint(30, 45+scale*N), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); for(int i = 0; isecond(0) << " v: " << observations.begin()->second(1); @@ -771,6 +781,44 @@ 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 +{ + + 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); + + cv::Point2f pure_surroundingPoint; + image_handler::undistortPoint(surroundingPoint, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs, + pure_surroundingPoint); + + 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)); + + return distance; +} + + cv::Point2f Feature::projectPositionToCamera( const CAMState& cam_state, const StateIDType& cam_state_id, diff --git a/include/msckf_vio/math_utils.hpp b/include/msckf_vio/math_utils.hpp index d6cb57a..72bc6ce 100644 --- a/include/msckf_vio/math_utils.hpp +++ b/include/msckf_vio/math_utils.hpp @@ -13,6 +13,13 @@ namespace msckf_vio { + +inline double absoluted(double a){ + if(a>0) + return a; + else return -a; +} + /* * @brief Create a skew-symmetric matrix from a 3-element vector. * @note Performs the operation: diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 9e103b7..0fc464a 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -24,7 +24,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index cced538..5d7d4c2 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1232,29 +1232,12 @@ void MsckfVio::PhotometricMeasurementJacobian( // one line of the NxN Jacobians Matrix H_xi; Matrix H_xA; - Matrix H_fi; MatrixXd H_xl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6); MatrixXd H_xAl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6); - MatrixXd H_yl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 3); auto frame = cam0.moving_window.find(cam_state_id)->second.image; - - //observation - const Vector4d& z = feature.observations.find(cam_state_id)->second; - - //estimate photometric measurement - std::vector estimate_irradiance; - std::vector estimate_photo_z; - IlluminationParameter estimated_illumination; - feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); - // calculated here, because we need true 'estimate_irradiance' later for jacobi - for (auto& estimate_irradiance_j : estimate_irradiance) - estimate_photo_z.push_back (estimate_irradiance_j * - estimated_illumination.frame_gain * estimated_illumination.feature_gain + - estimated_illumination.frame_bias + estimated_illumination.feature_bias); - int count = 0; double dx, dy; @@ -1271,19 +1254,17 @@ 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); - z_irr_est.push_back(feature.PixelIrradiance(p_in_c0, frame)); - Matrix dI_dhj = Matrix::Zero(); - //calculate photom. residual - photo_r.push_back(photo_z[count] - estimate_photo_z[count]); // add jacobians + cv::Point2f pixelDistance = feature.pixelDistanceAt(cam_state, cam_state_id, cam0, point); + // 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; + dI_dhj(0, 0) = dx/pixelDistance.x; + dI_dhj(0, 1) = dy/pixelDistance.y; // Compute the Jacobians. Matrix dz_dpc0 = Matrix::Zero(); @@ -1298,17 +1279,16 @@ void MsckfVio::PhotometricMeasurementJacobian( dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); dpc0_dxc.rightCols(3) = -R_w_c0; - Matrix3d dpc0_dpg = R_w_c0; - H_xi = dI_dhj*dz_dpc0*dpc0_dxc; - H_fi = dI_dhj*dz_dpc0*dpc0_dpg; - dCpij_dGpij = quaternionToRotation(cam_state.orientation); + Matrix3d dCpij_dGpij = quaternionToRotation(cam_state.orientation); //orientation takes camera frame to world frame, we wa - dh_dGpij = dz_dpc0 * dCpij_dGpij; + Matrix dh_dGpij = dz_dpc0 * dCpij_dGpij; - double rho = feature.rh + double rho = feature.anchor_rho; + + Matrix dGpj_XpAj = Matrix::Zero(); dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), @@ -1316,42 +1296,43 @@ void MsckfVio::PhotometricMeasurementJacobian( 1/(rho))); dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); - H_xA = dI_dhj*dh_dGpij*dGpj_XpAj + H_xA = dI_dhj*dh_dGpij*dGpj_XpAj; H_xl.block<1, 6>(count, 0) = H_xi; - H_yl.block<1, 3>(count, 0) = H_fi; H_xAl.block<1, 6>(count, 0) = H_xA; count++; } - // Compute the residual. - std::vector z_irr; - cv::Point2f z = cv::Point2f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); - feature.PatchAroundPurePixel(z, N, cam0, cam_state_id, z_irr); + // calculate projected irradiance + std::vector projectionPatch; + for(auto point : feature.anchorPatch_3d) + { + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + projectionPatch.push_back(feature.PixelIrradiance(p_in_c0, frame)); + } + + Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count); - Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count-1); + std::vector residual_v; for(int i = 0; i < r_l.size(); i++) - r_l(i) = z_irr[i]- feature.anchorPatch[i]; - + { + residual_v.push_back(projectionPatch[i]- feature.anchorPatch[i]); + r_l(i) = projectionPatch[i] - feature.anchorPatch[i]; + } r = r_l; H_x = H_xl; H_y = H_xAl; - //TODO make this more fluent as well - count = 0; - for(auto data : photo_r) - r[count++] = data; - std::stringstream ss; - ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; + ss << "INFO:"; // << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss, gradientVector, residualVector); + feature.VisualizePatch(cam_state, cam_state_id, cam0, residual_v, ss); } return; @@ -1426,9 +1407,8 @@ void MsckfVio::PhotometricFeatureJacobian( // Stack the Jacobians. - H_xi.block(stack_cntr, 21+cam_state_cntr_anchor*7, H_yl.rows(), H_yl.cols()) = -H_yi; - H_xi.block(stack_cntr, 21+cam_state_cntr*7, H_xl.rows(), H_xl.cols()) = H_xl; - + H_xi.block(stack_cntr, 21+cam_state_cntr_anchor*7, H_yl.rows(), H_yl.cols()) = H_yl; + H_xi.block(stack_cntr, 21+cam_state_cntr*7, H_xl.rows(), H_xl.cols()) = -H_xl; r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; @@ -1439,7 +1419,7 @@ void MsckfVio::PhotometricFeatureJacobian( 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 << "Hx\n" << H_x << "r\n" << r << "\n x\n" << H_x.colPivHouseholderQr().solve(r) << endl; myfile.close(); cout << "---------- LOGGED -------- " << endl; @@ -1710,7 +1690,6 @@ 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(); @@ -1819,13 +1798,8 @@ void MsckfVio::removeLostFeatures() { else featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - cout << "\n" << endl; - cout << "H_xj: \n" << H_xj << endl; - cout << "res: \n" << endl; - cout << r_j << endl; - - if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) { + if (gatingTest(H_xj, r_j, r_j.size()-1)){ //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(); @@ -1986,14 +1960,8 @@ void MsckfVio::pruneCamStateBuffer() { PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); else featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - - cout << "\n" << endl; - cout << "H_xj: \n" << H_xj << endl; - cout << "res: \n" << endl; - cout << r_j << endl; - - if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) { + if (gatingTest(H_xj, r_j, r_j.size()-1)) { //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();