diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index c6d2f7c..3cfa49c 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -35,6 +35,8 @@ struct CAMState { // Position of the camera frame in the world frame. Eigen::Vector3d position; + //Illumination illumination; + // These two variables should have the same physical // interpretation with `orientation` and `position`. // There two variables are used to modify the measurement diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 45abbaf..5a756c2 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -146,16 +146,46 @@ struct Feature { inline bool initializePosition( const CamStateServer& cam_states); + + inline cv::Point2f projectPositionToCamera( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + Eigen::Vector3d& in_p) const; + + /* + * @brief IrradianceAnchorPatch_Camera returns irradiance values + * of the Anchor Patch position in a camera frame + * + */ + + bool IrradianceOfAnchorPatch( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + const movingWindow& cam0_moving_window, + std::vector& anchorPatch_measurement) const; + /* * @brief projectPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em */ - bool projectPixelToPosition(cv::Point2f in_p, + inline bool projectPixelToPosition(cv::Point2f in_p, Eigen::Vector3d& out_p, const cv::Vec4d& intrinsics, const std::string& distortion_model, const cv::Vec4d& distortion_coeffs); + /* + * @brief Irradiance returns irradiance value of a pixel + */ + + inline uint8_t Irradiance(cv::Point2f pose, cv::Mat image) const; + // An unique identifier for the feature. // In case of long time running, the variable // type of id is set to FeatureIDType in order @@ -329,6 +359,61 @@ bool Feature::checkMotion( else return false; } +bool Feature::IrradianceOfAnchorPatch( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + const movingWindow& cam0_moving_window, + std::vector& anchorPatch_measurement) const +{ + //project every point in anchorPatch_3d. + for (auto point : anchorPatch_3d) + { + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, intrinsics, distortion_model, distortion_coeffs, point); + uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second); + anchorPatch_measurement.push_back(irradiance); + } +} + +uint8_t Feature::Irradiance(cv::Point2f pose, cv::Mat image) const +{ + return image.at(pose.x, pose.y); +} + +cv::Point2f Feature::projectPositionToCamera( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const cv::Vec4d& intrinsics, + const std::string& distortion_model, + const cv::Vec4d& distortion_coeffs, + Eigen::Vector3d& in_p) const +{ + Eigen::Isometry3d T_c0_w; + + cv::Point2f out_p; + + // transfrom position to camera frame + Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); + const Eigen::Vector3d& t_c0_w = cam_state.position; + Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w); + + out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); + std::vector out_v; + out_v.push_back(out_p); + std::vector my_p = image_handler::distortPoints( out_v, + intrinsics, + distortion_model, + distortion_coeffs); + + // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z()); + // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2)); + // printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y); + + return out_p; +} + bool Feature::projectPixelToPosition(cv::Point2f in_p, Eigen::Vector3d& out_p, const cv::Vec4d& intrinsics, @@ -342,9 +427,11 @@ bool Feature::projectPixelToPosition(cv::Point2f in_p, Eigen::Vector3d PositionInCamera(in_p.x/rho, in_p.y/rho, 1/rho); Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); anchorPatch_3d.push_back(PositionInWorld); - printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); + //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); } + +//@test center projection must always be initial feature projection bool Feature::initializeAnchor( const movingWindow& cam0_moving_window, const cv::Vec4d& intrinsics, @@ -352,7 +439,7 @@ bool Feature::initializeAnchor( const cv::Vec4d& distortion_coeffs) { - int N = 5; + int N = 3; int n = (int)(N-1)/2; auto anchor = observations.begin(); @@ -364,21 +451,22 @@ bool Feature::initializeAnchor( auto v = anchor->second(1)*intrinsics[1] + intrinsics[3]; int count = 0; - printf("estimated NxN position: \n"); + //go through surrounding pixels for(double u_run = u - n; u_run <= u + n; u_run = u_run + 1) { for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1) { anchorPatch.push_back(anchorImage.at((int)u_run,(int)v_run)); Eigen::Vector3d Npose; - projectPixelToPosition(cv::Point2f((u_run-intrinsics[2])/intrinsics[0], (v_run-intrinsics[1])/intrinsics[3]), + projectPixelToPosition(cv::Point2f((u_run-intrinsics[2])/intrinsics[0], (v_run-intrinsics[3])/intrinsics[1]), Npose, intrinsics, distortion_model, distortion_coeffs); } } - return true; + + return true; } bool Feature::initializePosition( diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index b3a37bc..f857a52 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -174,6 +175,20 @@ class MsckfVio { void featureJacobian(const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + + + void PhotometricMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::Matrix& H_x, + Eigen::Matrix& H_f, + Eigen::Vector4d& r); + + void PhotometricFeatureJacobian( + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + void measurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); bool gatingTest(const Eigen::MatrixXd& H, diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 6bd9030..cea396d 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -622,6 +622,7 @@ void ImageProcessor::stereoMatch( image_handler::undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs, cam0_points_undistorted, R_cam0_cam1); + cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1_intrinsics, cam1_distortion_model, cam1_distortion_coeffs); } diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 37b751e..f5391dd 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -443,12 +443,14 @@ void MsckfVio::initializeGravityAndBias() { // is consistent with the inertial frame. double gravity_norm = gravity_imu.norm(); IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm); - + Quaterniond q0_i_w = Quaterniond::FromTwoVectors( gravity_imu, -IMUState::gravity); state_server.imu_state.orientation = rotationToQuaternion(q0_i_w.toRotationMatrix().transpose()); - + // printf("gravity Norm %f\n", gravity_norm); + // printf("linear_acc %f, %f, %f\n", gravity_imu(0), gravity_imu(1), gravity_imu(2)); + // printf("quaterniond: %f, %f, %f, %f\n", q0_i_w.w(), q0_i_w.x(), q0_i_w.y(), q0_i_w.z()); return; } @@ -803,6 +805,7 @@ void MsckfVio::stateAugmentation(const double& time) { cam_state.orientation_null = cam_state.orientation; cam_state.position_null = cam_state.position; + // Update the covariance matrix of the state. // To simplify computation, the matrix J below is the nontrivial block // in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision @@ -874,6 +877,157 @@ void MsckfVio::addFeatureObservations( return; } +void MsckfVio::PhotometricMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Matrix& H_x, Matrix& H_f, Vector4d& r) { + + // Prepare all the required data. + const CAMState& cam_state = state_server.cam_states[cam_state_id]; + const Feature& feature = map_server[feature_id]; + + // Cam0 pose. + Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); + const Vector3d& t_c0_w = cam_state.position; + + // Cam1 pose. + Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); + Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0; + Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); + + // 3d feature position in the world frame. + // And its observation with the stereo cameras. + const Vector3d& p_w = feature.position; + + //observation + const Vector4d& z = feature.observations.find(cam_state_id)->second; + + //photometric observation + std::vector photo_z; + feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs, cam0_moving_window, photo_z); + + + // Convert the feature position from the world frame to + // the cam0 and cam1 frame. + Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w); + Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w); + + + //compute resulting esimtated position in image + cv::Point2f out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2)); + std::vector out_v; + out_v.push_back(out_p); + + /* + //prints the feature projection in pixel space + std::vector my_p = image_handler::distortPoints( out_v, + cam0_intrinsics, + cam0_distortion_model, + cam0_distortion_coeffs); + printf("projection: %f, %f\n", my_p[0].x, my_p[0].y); + */ + + // Compute the Jacobians. + Matrix dz_dpc0 = Matrix::Zero(); + dz_dpc0(0, 0) = 1 / p_c0(2); + dz_dpc0(1, 1) = 1 / p_c0(2); + dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2)); + dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2)); + + Matrix dz_dpc1 = Matrix::Zero(); + dz_dpc1(2, 0) = 1 / p_c1(2); + dz_dpc1(3, 1) = 1 / p_c1(2); + dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2)); + dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2)); + + Matrix dpc0_dxc = Matrix::Zero(); + dpc0_dxc.leftCols(3) = skewSymmetric(p_c0); + dpc0_dxc.rightCols(3) = -R_w_c0; + + Matrix dpc1_dxc = Matrix::Zero(); + dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0); + dpc1_dxc.rightCols(3) = -R_w_c1; + + Matrix3d dpc0_dpg = R_w_c0; + Matrix3d dpc1_dpg = R_w_c1; + + H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc; + H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg; + + // Modifty the measurement Jacobian to ensure + // observability constrain. + Matrix A = H_x; + Matrix u = Matrix::Zero(); + u.block<3, 1>(0, 0) = quaternionToRotation( + cam_state.orientation_null) * IMUState::gravity; + u.block<3, 1>(3, 0) = skewSymmetric( + p_w-cam_state.position_null) * IMUState::gravity; + H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose(); + H_f = -H_x.block<4, 3>(0, 3); + + // Compute the residual. + r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), + p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); + + return; +} + +void MsckfVio::PhotometricFeatureJacobian( + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + MatrixXd& H_x, VectorXd& r) { + + const auto& feature = map_server[feature_id]; + + // Check how many camera states in the provided camera + // id camera has actually seen this feature. + vector valid_cam_state_ids(0); + for (const auto& cam_id : cam_state_ids) { + if (feature.observations.find(cam_id) == + feature.observations.end()) continue; + + valid_cam_state_ids.push_back(cam_id); + } + + int jacobian_row_size = 0; + jacobian_row_size = 4 * valid_cam_state_ids.size(); + + MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size, + 21+state_server.cam_states.size()*6); + MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3); + VectorXd r_j = VectorXd::Zero(jacobian_row_size); + int stack_cntr = 0; + + for (const auto& cam_id : valid_cam_state_ids) { + + Matrix H_xi = Matrix::Zero(); + Matrix H_fi = Matrix::Zero(); + Vector4d r_i = Vector4d::Zero(); + PhotometricMeasurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i); + + auto cam_state_iter = state_server.cam_states.find(cam_id); + int cam_state_cntr = std::distance( + state_server.cam_states.begin(), cam_state_iter); + + // Stack the Jacobians. + H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi; + H_fj.block<4, 3>(stack_cntr, 0) = H_fi; + r_j.segment<4>(stack_cntr) = r_i; + stack_cntr += 4; + } + + // Project the residual and Jacobians onto the nullspace + // of H_fj. + JacobiSVD svd_helper(H_fj, ComputeFullU | ComputeThinV); + MatrixXd A = svd_helper.matrixU().rightCols( + jacobian_row_size - 3); + + H_x = A.transpose() * H_xj; + r = A.transpose() * r_j; + + return; +} + void MsckfVio::measurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, @@ -1199,7 +1353,7 @@ void MsckfVio::removeLostFeatures() { MatrixXd H_xj; VectorXd r_j; - featureJacobian(feature.id, cam_state_ids, H_xj, r_j); + PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; @@ -1316,6 +1470,13 @@ void MsckfVio::pruneCamStateBuffer() { } } + if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs)) + { + for (const auto& cam_id : involved_cam_state_ids) + feature.observations.erase(cam_id); + continue; + } + jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -1342,7 +1503,7 @@ void MsckfVio::pruneCamStateBuffer() { MatrixXd H_xj; VectorXd r_j; - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;