diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index d727646..2cf42fd 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -165,7 +165,8 @@ struct Feature { const CAMState& cam_state, const StateIDType& cam_state_id, CameraCalibration& cam0, - std::vector& anchorPatch_estimate) const; + std::vector& anchorPatch_estimate, + IlluminationParameter& estimatedIllumination) const; bool VisualizePatch( const CAMState& cam_state, @@ -202,6 +203,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, // NxN Patch of Anchor Image std::vector anchorPatch; + std::vector anchorPatch_ideal; // Position of NxN Patch in 3D space std::vector anchorPatch_3d; @@ -365,7 +367,8 @@ bool Feature::estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, CameraCalibration& cam0, - std::vector& anchorPatch_estimate) const + std::vector& anchorPatch_estimate, + IlluminationParameter& estimated_illumination) const { // get irradiance of patch in anchor frame // subtract estimated b and divide by a of anchor frame @@ -381,15 +384,19 @@ bool Feature::estimate_FrameIrradiance( double a_A = anchorExposureTime_ms; double b_A = 0; - double a_l =frameExposureTime_ms; + double a_l = frameExposureTime_ms; double b_l = 0; - + + estimated_illumination.frame_gain = a_l; + estimated_illumination.frame_bias = b_l; + estimated_illumination.feature_gain = 1; + estimated_illumination.feature_bias = 0; //printf("frames: %lld, %lld\n", anchor->first, cam_state_id); //printf("exposure: %f, %f\n", a_A, a_l); for (double anchorPixel : anchorPatch) { - float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l; + float irradiance = (anchorPixel - b_A) / a_A ; anchorPatch_estimate.push_back(irradiance); } @@ -536,6 +543,8 @@ bool Feature::initializeAnchor( // save anchor position for later visualisaztion anchor_center_pos = vec[4]; + + // save true pixel Patch position for(auto point : vec) { if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) @@ -546,8 +555,10 @@ bool Feature::initializeAnchor( // project patch pixel to 3D space for(auto point : und_v) + { + anchorPatch_ideal.push_back(point); anchorPatch_3d.push_back(projectPixelToPosition(point, cam)); - + } is_anchored = true; return true; } diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index d7eaf0b..de5e217 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -901,11 +901,8 @@ void MsckfVio::PhotometricMeasurementJacobian( // 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; + std::vector photo_z; // individual Jacobians Matrix dI_dhj = Matrix::Zero(); @@ -921,17 +918,17 @@ void MsckfVio::PhotometricMeasurementJacobian( Eigen::Matrix H_pAj; // combined Jacobians - Eigen::MatrixXd H_rho(N*N, 3); + Eigen::MatrixXd H_rho(N*N, 1); Eigen::MatrixXd H_pl(N*N, 6); Eigen::MatrixXd H_pA(N*N, 6); auto frame = cam0.moving_window.find(cam_state_id)->second.image; int count = 0; - float dx, dy; + double dx, dy; for (auto point : feature.anchorPatch_3d) { - + Eigen::Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w); cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); //add observation @@ -947,8 +944,8 @@ void MsckfVio::PhotometricMeasurementJacobian( //dh / d{}^Cp_{ij} dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix::Identity(); - dh_dCpij(0, 2) = -(point(0))/(point(2)*point(2)); - dh_dCpij(1, 2) = -(point(1))/(point(2)*point(2)); + dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); + dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose(); //dh / d X_{pl} @@ -957,9 +954,9 @@ void MsckfVio::PhotometricMeasurementJacobian( //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; - dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(p_in_c0.x/(rho*rho), p_in_c0.y/(rho*rho), 1/(rho*rho)); + dGpi_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)); - dGpi_XpAj.block<3, 3>(3, 0) = skewSymmetric(Eigen::Vector3d(p_in_c0.x/(rho), p_in_c0.y/(rho), 1/(rho))); + dGpi_XpAj.block<3, 3>(3, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho))); dGpi_XpAj.block<3, 3>(3, 3) = Matrix::Identity(); // Intermediate Jakobians @@ -974,25 +971,69 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } + // calculate residual + // visu -residual + //printf("-----\n"); + + //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); + 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); + + std::vector photo_r; + //calculate photom. residual + for(int i = 0; i < photo_z.size(); i++) + photo_r.push_back(photo_z[i] - estimate_photo_z[i]); + + // visu- residual + //for(int i = 0; i < photo_z.size(); i++) + // printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); + //Final Jakobians + // cout << "------------------------" << endl; + // cout << "rho" << H_rho.rows() << "x" << H_rho.cols() << "\n" << H_rho << endl; + // cout << "l" << H_pl.rows() << "x" << H_pl.cols() << "\n" << H_pl << endl; + // cout << "A" << H_pA.rows() << "x" << H_pA.cols() << "\n" << H_pA << endl; + MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+2); - auto cam_state_iter = state_server.cam_states.find(feature.observations.begin()->first); - int cam_state_cntr = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_state_id)); + MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); // set anchor Jakobi - H_xl.block(0,21+cam_state_cntr*7) = -H_pA; - //H_yl - - cam_state_iter = state_server.cam_states.find(cam_state_id); - cam_state_cntr = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_state_id)); + // get position of anchor in cam states + auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); + int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); + H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; // set frame Jakobi - H_xl.block(N*N, 6, 0, 21+cam_state_cntr*7) = -H_pl; + //get position of current frame in cam states + auto cam_state_iter = state_server.cam_states.find(cam_state_id); + int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); + + // set jakobi of state + H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl; - H_xl.block(N*N, 1, 0, 21+cam_state_cntr*7) = Eigen::ArrayXd::Ones(N*N); + // set ones for irradiance bias + H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); + + // set irradiance error Block + H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N); + + // TODO make this calculation more fluent + for(int i = 0; i< N*N; i++) + H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; + H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho; + + // Original calculation // Convert the feature position from the world frame to // the cam0 and cam1 frame. @@ -1003,21 +1044,33 @@ void MsckfVio::PhotometricMeasurementJacobian( 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)); - // visu -residual - //printf("-----\n"); - //estimate photometric measurement - std::vector estimate_photo_z; - feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_photo_z); - std::vector photo_r; - - //calculate photom. residual - for(int i = 0; i < photo_z.size(); i++) - photo_r.push_back(photo_z[i] - estimate_photo_z[i]); +// 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)); - // visu- residual - //for(int i = 0; i < photo_z.size(); i++) - // printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); + 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; photo_z.clear(); return; @@ -1051,7 +1104,7 @@ void MsckfVio::PhotometricFeatureJacobian( // visu - residual - //printf("_____FEATURE:_____\n"); + printf("_____FEATURE:_____\n"); // visu - feature //cam0.featureVisu.release();