diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index e84a847..f278b37 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -171,8 +171,8 @@ struct Feature { bool VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0) const; - + CameraCalibration& cam0, + const std::vector photo_r) const; /* * @brief projectPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em @@ -204,6 +204,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, // NxN Patch of Anchor Image std::vector anchorPatch; std::vector anchorPatch_ideal; + std::vector anchorPatch_real; // Position of NxN Patch in 3D space std::vector anchorPatch_3d; @@ -408,43 +409,104 @@ bool Feature::estimate_FrameIrradiance( bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0) const + CameraCalibration& cam0, + const std::vector photo_r) const { + double rescale = 1; + + //visu - anchor + auto anchor = observations.begin(); + cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image; + cv::Mat dottedFrame(anchorImage.size(), CV_8UC3); + cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB); + + for(auto point : anchorPatch_real) + { + // visu - feature + cv::Point xs(point.x, point.y); + cv::Point ys(point.x, point.y); + cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); + } + cam0.featureVisu = dottedFrame.clone(); + // visu - feature cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; - cv::Mat dottedFrame(current_image.size(), CV_8UC3); cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB); - - // project every point in anchorPatch_3d. - auto frame = cam0.moving_window.find(cam_state_id)->second.image; + // set position in frame + // save irradiance of projection + std::vector projectionPatch; for(auto point : anchorPatch_3d) { cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); - + projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image)); // visu - feature cv::Point xs(p_in_c0.x, p_in_c0.y); cv::Point ys(p_in_c0.x, p_in_c0.y); cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); } - // testing - //if(cam_state_id == observations.begin()->first) - //if(count++ == 4) - //printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y); - // visu - feature - cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2)); - if(cam0.featureVisu.empty()) - cam0.featureVisu = dottedFrame.clone(); - else - cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); + cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); + + // irradiance grid anchor + int N = sqrt(anchorPatch_3d.size()); + int scale = 20; + cv::Mat irradianceFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255)); + cv::resize(irradianceFrame, irradianceFrame, cv::Size(), rescale, rescale); + for(int i = 0; i0) + cv::rectangle(irradianceFrame, + cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), + cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), + cv::Scalar(255 - photo_r[i*N+j]*255, 255 - photo_r[i*N+j]*255, 255), + CV_FILLED); + else + cv::rectangle(irradianceFrame, + cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), + cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), + cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), + CV_FILLED); + + cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale); + + cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); + + // create line? + + //save image + + std::stringstream ss; + ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; + cv::imwrite(ss.str(), cam0.featureVisu); + + //cv::imshow("patch", cam0.featureVisu); + //cvWaitKey(1); } float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const { - - return ((float)image.at(pose.x, pose.y))/256; + + return ((float)image.at(pose.y, pose.x))/255; } cv::Point2f Feature::projectPositionToCamera( @@ -514,16 +576,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) int count = 0; // get feature in undistorted pixel space + // this only reverts from 'pure' space into undistorted pixel space using camera matrix cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), cam.intrinsics, cam.distortion_model, 0); // create vector of patch in pixel plane - std::vector und_pix_v; + std::vectorund_pix_v; for(double u_run = -n; u_run <= n; u_run++) for(double v_run = -n; v_run <= n; v_run++) - und_pix_v.push_back(cv::Point2f(und_pix_p.x-u_run, und_pix_p.y-v_run)); - + und_pix_v.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run)); //create undistorted pure points @@ -533,23 +595,24 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) cam.distortion_model, 0, und_v); + //create distorted pixel points - std::vector vec = image_handler::distortPoints(und_v, - cam.intrinsics, - cam.distortion_model, - cam.distortion_coeffs); + anchorPatch_real = image_handler::distortPoints(und_v, + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); // save anchor position for later visualisaztion - anchor_center_pos = vec[4]; + anchor_center_pos = anchorPatch_real[(N*N-1)/2]; // save true pixel Patch position - for(auto point : vec) + for(auto point : anchorPatch_real) { if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1)) return false; } - for(auto point : vec) + for(auto point : anchorPatch_real) anchorPatch.push_back(PixelIrradiance(point, anchorImage)); // project patch pixel to 3D space diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index ba88474..cd5bb92 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -202,7 +202,9 @@ class MsckfVio { void onlineReset(); // Photometry flag + // visualization flag bool PHOTOMETRIC; + bool PRINTIMAGES; bool nan_flag; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 9c95b1c..0032c70 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -19,8 +19,9 @@ + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 9cd6334..0990c45 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -63,7 +63,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh): bool MsckfVio::loadParameters() { //Photometry Flag nh.param("PHOTOMETRIC", PHOTOMETRIC, false); - + nh.param("PrintImages", PRINTIMAGES, false); // Frame id nh.param("fixed_frame_id", fixed_frame_id, "world"); @@ -1027,11 +1027,11 @@ void MsckfVio::PhotometricMeasurementJacobian( //add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - //add jacobian + // add jacobian // 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_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; @@ -1043,7 +1043,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //dh / d X_{pl} dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point); - dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose(); + dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation);//.transpose(); //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; @@ -1074,6 +1074,7 @@ void MsckfVio::PhotometricMeasurementJacobian( 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 + @@ -1121,6 +1122,9 @@ void MsckfVio::PhotometricMeasurementJacobian( for(auto data : photo_r) r[count++] = data; + if(PRINTIMAGES) + feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r); + return; } @@ -1151,15 +1155,8 @@ void MsckfVio::PhotometricFeatureJacobian( VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; - // visu - residual - //printf("_____FEATURE:_____\n"); - // visu - feature - //cam0.featureVisu.release(); - for (const auto& cam_id : valid_cam_state_ids) { - //Matrix H_xi = Matrix::Zero(); - //Matrix H_fi = Matrix::Zero(); MatrixXd H_xl; MatrixXd H_yl; Eigen::VectorXd r_l = VectorXd::Zero(N*N); @@ -1176,27 +1173,11 @@ void MsckfVio::PhotometricFeatureJacobian( r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; } - // visu - feature - /* - if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000) - imshow("feature", cam0.featureVisu); - cvWaitKey(1); - - if((ros::Time::now() - image_save_time).toSec() > 1) - { - std::stringstream ss; - ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; - imwrite(ss.str(), cam0.featureVisu); - image_save_time = ros::Time::now(); - } - */ // Project the residual and Jacobians onto the nullspace // of H_yj. // get Nullspace - - JacobiSVD svd_helper(H_yi, ComputeFullU | ComputeThinV); int sv_size = 0; Eigen::VectorXd singularValues = svd_helper.singularValues(); @@ -1339,8 +1320,6 @@ void MsckfVio::featureJacobian( H_x = A.transpose() * H_xj; r = A.transpose() * r_j; - cout << "A: \n" << A.transpose() << endl; - return; } @@ -1348,8 +1327,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { if (H.rows() == 0 || r.rows() == 0) return; - - cout << "Updating..."; // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin;