diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index 40b2dc0..f5ea7e8 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -33,4 +33,4 @@ T_imu_body: [1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, - 0.0000, 0.0000, 0.0000, 1.0000] \ No newline at end of file + 0.0000, 0.0000, 0.0000, 1.0000] diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index 723f252..9eda6f5 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -18,6 +18,8 @@ namespace msckf_vio { struct Frame{ cv::Mat image; + cv::Mat dximage; + cv::Mat dyimage; double exposureTime_ms; }; @@ -39,6 +41,7 @@ struct CameraCalibration{ cv::Vec4d distortion_coeffs; movingWindow moving_window; cv::Mat featureVisu; + int id; }; diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 839f564..5cc997f 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -184,6 +184,12 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, const CameraCalibration& cam, Eigen::Vector3d& in_p) const; + double CompleteCvKernel( + const cv::Point2f pose, + const StateIDType& cam_state_id, + CameraCalibration& cam, + std::string type) const; + double cvKernel( const cv::Point2f pose, std::string type) const; @@ -202,7 +208,7 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci, bool estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, std::vector& anchorPatch_estimate, IlluminationParameter& estimatedIllumination) const; @@ -218,11 +224,9 @@ bool VisualizeKernel( bool VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, const Eigen::VectorXd& photo_r, - std::stringstream& ss, - cv::Point2f gradientVector, - cv::Point2f residualVector) const; + std::stringstream& ss) const; /* * @brief AnchorPixelToPosition uses the calcualted pixels * of the anchor patch to generate 3D positions of all of em @@ -490,6 +494,23 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const else return false; } +double Feature::CompleteCvKernel( + const cv::Point2f pose, + const StateIDType& cam_state_id, + CameraCalibration& cam, + std::string type) const +{ + + double delta = 0; + + if(type == "Sobel_x") + delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at(pose.y, pose.x))/255.; + else if (type == "Sobel_y") + delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at(pose.y, pose.x))/255.; + + return delta; +} + double Feature::cvKernel( const cv::Point2f pose, std::string type) const @@ -528,7 +549,7 @@ return delta; bool Feature::estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, std::vector& anchorPatch_estimate, IlluminationParameter& estimated_illumination) const { @@ -537,11 +558,11 @@ bool Feature::estimate_FrameIrradiance( // muliply by a and add b of this frame auto anchor = observations.begin(); - if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end()) + if(cam.moving_window.find(anchor->first) == cam.moving_window.end()) 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; + double anchorExposureTime_ms = cam.moving_window.find(anchor->first)->second.exposureTime_ms; + double frameExposureTime_ms = cam.moving_window.find(cam_state_id)->second.exposureTime_ms; double a_A = anchorExposureTime_ms; @@ -711,21 +732,20 @@ bool Feature::VisualizeKernel( cvWaitKey(0); } + bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, const Eigen::VectorXd& photo_r, - std::stringstream& ss, - cv::Point2f gradientVector, - cv::Point2f residualVector) const + std::stringstream& ss) const { double rescale = 1; //visu - anchor auto anchor = observations.begin(); - cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image; + cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image; cv::Mat dottedFrame(anchorImage.size(), CV_8UC3); cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB); @@ -737,10 +757,10 @@ bool Feature::VisualizePatch( cv::Point ys(point.x, point.y); cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255)); } - cam0.featureVisu = dottedFrame.clone(); + cam.featureVisu = dottedFrame.clone(); // visu - feature - cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image; + cv::Mat current_image = cam.moving_window.find(cam_state_id)->second.image; cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB); // set position in frame @@ -748,7 +768,7 @@ bool Feature::VisualizePatch( std::vector projectionPatch; for(auto point : anchorPatch_3d) { - cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point); projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image)); // visu - feature cv::Point xs(p_in_c0.x, p_in_c0.y); @@ -756,7 +776,7 @@ bool Feature::VisualizePatch( cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0)); } - cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu); + cv::hconcat(cam.featureVisu, dottedFrame, cam.featureVisu); // patches visualization @@ -803,11 +823,11 @@ bool Feature::VisualizePatch( cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); - cv::Point2f p_f(observations.find(cam_state_id)->second(0)*cam0.intrinsics[0] + cam0.intrinsics[2],observations.find(cam_state_id)->second(1)*cam0.intrinsics[1] + cam0.intrinsics[3]); + cv::Point2f p_f(observations.find(cam_state_id)->second(0)*cam.intrinsics[0] + cam.intrinsics[2],observations.find(cam_state_id)->second(1)*cam.intrinsics[1] + cam.intrinsics[3]); // move to real pixels - - // without distort - // p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); + + p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); + for(int i = 0; i0) + if(photo_r(2*(i*N+j))>0) cv::rectangle(irradianceFrame, cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), cv::Point(40+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::Scalar(255 - photo_r(2*(i*N+j)+1)*255, 255 - photo_r(2*(i*N+j)+1)*255, 255), CV_FILLED); else cv::rectangle(irradianceFrame, cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), cv::Point(40+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::Scalar(255, 255 + photo_r(2*(i*N+j)+1)*255, 255 + photo_r(2*(i*N+j)+1)*255), CV_FILLED); // gradient arrow @@ -860,7 +881,7 @@ bool Feature::VisualizePatch( 3); */ - cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); + cv::hconcat(cam.featureVisu, irradianceFrame, cam.featureVisu); /* // visualize position of used observations and resulting feature position @@ -892,15 +913,15 @@ bool Feature::VisualizePatch( // draw, x y position and arrow with direction - write z next to it - cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale); + cv::resize(cam.featureVisu, cam.featureVisu, cv::Size(), rescale, rescale); - cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu); + cv::hconcat(cam.featureVisu, positionFrame, cam.featureVisu); */ // write feature position std::stringstream pos_s; pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1); - cv::putText(cam0.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), + cv::putText(cam.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA); // create line? @@ -908,10 +929,10 @@ bool Feature::VisualizePatch( std::stringstream loc; // loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; - //cv::imwrite(loc.str(), cam0.featureVisu); + //cv::imwrite(loc.str(), cam.featureVisu); - cv::imshow("patch", cam0.featureVisu); - cvWaitKey(0); + cv::imshow("patch", cam.featureVisu); + cvWaitKey(1); } float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const @@ -964,28 +985,40 @@ cv::Point2f Feature::projectPositionToCamera( Eigen::Isometry3d T_c0_w; cv::Point2f out_p; - + cv::Point2f my_p; // transfrom position to camera frame + + // cam0 position 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)); - cv::Point2f my_p; + // project point according to model + if(cam.id == 0) + { + 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)); - // test is prewarped or not + } + // if camera is one, calcualte the cam1 position from cam0 position first + else if(cam.id == 1) + { + // cam1 position + Eigen::Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); + Eigen::Matrix3d R_w_c1 = R_c0_c1 * R_w_c0; + Eigen::Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); + + Eigen::Vector3d p_c1 = R_w_c1 * (in_p-t_c1_w); + out_p = cv::Point2f(p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); + } + + // undistort point according to camera model if (cam.distortion_model.substr(0,3) == "pre-") my_p = cv::Point2f(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]); else my_p = image_handler::distortPoint(out_p, - cam.intrinsics, - cam.distortion_model, - cam.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); - + cam.intrinsics, + cam.distortion_model, + cam.distortion_coeffs); return my_p; } @@ -1021,8 +1054,8 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3); cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3); - xderImage/=8; - yderImage/=8; + xderImage/=8.; + yderImage/=8.; cv::convertScaleAbs(xderImage, abs_xderImage); cv::convertScaleAbs(yderImage, abs_yderImage); @@ -1093,6 +1126,7 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) return true; } + bool Feature::initializeRho(const CamStateServer& cam_states) { // Organize camera poses and feature observations properly. diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 3394a0e..c7fdcea 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -202,28 +202,67 @@ class MsckfVio { Eigen::Vector4d& r); // This function computes the Jacobian of all measurements viewed // in the given camera states of this feature. - void featureJacobian(const FeatureIDType& feature_id, + void featureJacobian( + const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); - bool PhotometricMeasurementJacobian( - const StateIDType& cam_state_id, - const FeatureIDType& feature_id, - Eigen::MatrixXd& H_x, - Eigen::MatrixXd& H_y, - Eigen::VectorXd& r); + void twodotMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); - void PhotometricFeatureJacobian( - const FeatureIDType& feature_id, - const std::vector& cam_state_ids, - Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + bool ConstructJacobians( + Eigen::MatrixXd& H_rho, + Eigen::MatrixXd& H_pl, + Eigen::MatrixXd& H_pA, + const Feature& feature, + const StateIDType& cam_state_id, + Eigen::MatrixXd& H_xl, + Eigen::MatrixXd& H_yl); + + bool PhotometricPatchPointResidual( + const StateIDType& cam_state_id, + const Feature& feature, + Eigen::VectorXd& r); + + bool PhotometricPatchPointJacobian( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const Feature& feature, + Eigen::Vector3d point, + int count, + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj, + Eigen::Matrix& dI_dhj); + + bool PhotometricMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::MatrixXd& H_x, + Eigen::MatrixXd& H_y, + Eigen::VectorXd& r); + + + void twodotFeatureJacobian( + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + + bool PhotometricFeatureJacobian( + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); void measurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); + void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); + bool gatingTest(const Eigen::MatrixXd& H, - const Eigen::VectorXd&r, const int& dof); + const Eigen::VectorXd&r, const int& dof, int filter=0); void removeLostFeatures(); void findRedundantCamStates( std::vector& rm_cam_state_ids); @@ -234,7 +273,7 @@ class MsckfVio { void onlineReset(); // Photometry flag - bool PHOTOMETRIC; + int FILTER; // debug flag bool STREAMPAUSE; @@ -251,6 +290,18 @@ class MsckfVio { // Chi squared test table. static std::map chi_squared_test_table; + double eval_time; + + IMUState timed_old_imu_state; + IMUState timed_old_true_state; + + IMUState old_imu_state; + IMUState old_true_state; + + // change in position + Eigen::Vector3d delta_position; + Eigen::Vector3d delta_orientation; + // State vector StateServer state_server; StateServer photometric_state_server; @@ -316,6 +367,7 @@ class MsckfVio { // Subscribers and publishers ros::Subscriber imu_sub; ros::Subscriber truth_sub; + ros::Publisher truth_odom_pub; ros::Publisher odom_pub; ros::Publisher marker_pub; ros::Publisher feature_pub; diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 7500ff2..ec463d9 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -27,9 +27,10 @@ - - - + + + + diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index c9e9923..864f15e 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -17,15 +17,16 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - + + + - + - + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index beaaef4..5e5e3b3 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -17,15 +17,15 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - + + - + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 1096e37..7f239c7 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -17,15 +17,16 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - + + + - + diff --git a/src/image_handler.cpp b/src/image_handler.cpp index 324ffdb..21f7238 100644 --- a/src/image_handler.cpp +++ b/src/image_handler.cpp @@ -40,6 +40,12 @@ void undistortImage( cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K); else if (distortion_model == "equidistant") src.copyTo(dst); + + else if (distortion_model == "pre-radtan") + cv::undistort(src, dst, K, distortion_coeffs, K); + else if (distortion_model == "radtan") + src.copyTo(dst); + } void undistortPoint( @@ -91,7 +97,8 @@ void undistortPoint( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) @@ -152,7 +159,7 @@ void undistortPoints( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) @@ -205,7 +212,7 @@ std::vector distortPoints( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) @@ -262,7 +269,7 @@ cv::Point2f distortPoint( pts_out.push_back(newPoint); } } - else if (distortion_model == "pre-equidistant") + else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan") { std::vector temp_pts_out; for(int i = 0; i < pts_in.size(); i++) @@ -282,5 +289,5 @@ cv::Point2f distortPoint( return pts_out[0]; } - } // namespace image_handler -} // namespace msckf_vio \ No newline at end of file + } // namespace image_handler +} // namespace msckf_vio diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 0e01957..65acd43 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -224,20 +224,19 @@ void ImageProcessor::stereoCallback( cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img, sensor_msgs::image_encodings::MONO8); - - cv::Mat undistortedCam0; - cv::Mat undistortedCam1; + ros::Time start_time = ros::Time::now(); - - //image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); - //image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + //ROS_INFO("Publishing: %f", + // (ros::Time::now()-start_time).toSec()); // Build the image pyramids once since they're used at multiple places createImagePyramids(); // Detect features in the first frame. if (is_first_img) { - ros::Time start_time = ros::Time::now(); + start_time = ros::Time::now(); initializeFirstFrame(); //ROS_INFO("Detection time: %f", // (ros::Time::now()-start_time).toSec()); @@ -250,7 +249,7 @@ void ImageProcessor::stereoCallback( // (ros::Time::now()-start_time).toSec()); } else { // Track the feature in the previous image. - ros::Time start_time = ros::Time::now(); + start_time = ros::Time::now(); trackFeatures(); //ROS_INFO("Tracking time: %f", // (ros::Time::now()-start_time).toSec()); @@ -281,8 +280,7 @@ void ImageProcessor::stereoCallback( // (ros::Time::now()-start_time).toSec()); // Publish features in the current image. - - ros::Time start_time = ros::Time::now(); + start_time = ros::Time::now(); publish(); //ROS_INFO("Publishing: %f", // (ros::Time::now()-start_time).toSec()); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index d6f8326..f6fa93f 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -50,7 +50,7 @@ Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity(); // Static member variables in Feature class. FeatureIDType Feature::next_id = 0; -double Feature::observation_noise = 0.01; +double Feature::observation_noise = 0.05; Feature::OptimizationConfig Feature::optimization_config; map MsckfVio::chi_squared_test_table; @@ -67,7 +67,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh): bool MsckfVio::loadParameters() { //Photometry Flag - nh.param("PHOTOMETRIC", PHOTOMETRIC, false); + nh.param("FILTER", FILTER, 0); nh.param("PrintImages", PRINTIMAGES, false); nh.param("StreamPause", STREAMPAUSE, false); nh.param("GroundTruth", GROUNDTRUTH, false); @@ -184,6 +184,8 @@ bool MsckfVio::loadParameters() { cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; + cam0.id = 0; + cam1.id = 1; state_server.state_cov = MatrixXd::Zero(21, 21); @@ -260,7 +262,9 @@ bool MsckfVio::createRosIO() { // activating bag playing parameter, for debugging nh.setParam("/play_bag", true); + eval_time = 0; odom_pub = nh.advertise("odom", 10); + truth_odom_pub = nh.advertise("true_odom", 10); feature_pub = nh.advertise( "feature_point_cloud", 10); marker_pub = nh.advertise("/visualization_marker_array", 10); @@ -273,9 +277,9 @@ bool MsckfVio::createRosIO() { truth_sub = nh.subscribe("ground_truth", 100, &MsckfVio::truthCallback, this); - cam0_img_sub.subscribe(nh, "cam0_image", 10); - cam1_img_sub.subscribe(nh, "cam1_image", 10); - feature_sub.subscribe(nh, "features", 10); + cam0_img_sub.subscribe(nh, "cam0_image", 100); + cam1_img_sub.subscribe(nh, "cam1_image", 100); + feature_sub.subscribe(nh, "features", 100); image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub); image_sub.registerCallback(&MsckfVio::imageCallback, this); @@ -351,11 +355,6 @@ void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){ void MsckfVio::truthCallback(const geometry_msgs::TransformStampedPtr& msg){ static bool first_truth_odom_msg = true; - // errorstate - /*if(not ErrorState) - return; - */ - // If this is the first mocap odometry messsage, set // the initial frame. if (first_truth_odom_msg) { @@ -405,15 +404,22 @@ void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - // Return if the gravity vector has not been set. - if (!is_gravity_set) return; - - // stop playing bagfile if printing images + // stop playing bagfile if printing images if(STREAMPAUSE) nh.setParam("/play_bag", false); - - nh.param("PHOTOMETRIC", PHOTOMETRIC, false); + // Return if the gravity vector has not been set. + if (!is_gravity_set) + { + if(STREAMPAUSE) + nh.setParam("/play_bag", true); + return; + } + + old_imu_state = state_server.imu_state; + old_true_state = true_state_server.imu_state; + + // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -421,7 +427,6 @@ void MsckfVio::imageCallback( is_first_img = false; state_server.imu_state.time = feature_msg->header.stamp.toSec(); } - static double max_processing_time = 0.0; static int critical_time_cntr = 0; double processing_start_time = ros::Time::now().toSec(); @@ -429,28 +434,14 @@ void MsckfVio::imageCallback( // Propogate the IMU state. // that are received before the image feature_msg. ros::Time start_time = ros::Time::now(); - - + + nh.param("FILTER", FILTER, 0); + batchImuProcessing(feature_msg->header.stamp.toSec()); - - // save true state in seperate state vector - batchTruthProcessing(feature_msg->header.stamp.toSec()); - - if(GROUNDTRUTH) - { - state_server.imu_state.position = true_state_server.imu_state.position; - state_server.imu_state.orientation = true_state_server.imu_state.orientation; - state_server.imu_state.position_null = true_state_server.imu_state.position_null; - state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null; - - state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0; - state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu; - } double imu_processing_time = ( ros::Time::now()-start_time).toSec(); - cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); @@ -459,7 +450,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "2" << endl; + // cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -468,7 +459,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "3" << endl; + // cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); @@ -476,20 +467,25 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "4" << endl; + // cout << "4" << endl; + // Perform measurement update if necessary. start_time = ros::Time::now(); removeLostFeatures(); double remove_lost_features_time = ( ros::Time::now()-start_time).toSec(); - //cout << "5" << endl; + // cout << "5" << endl; start_time = ros::Time::now(); pruneCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); - //cout << "6" << endl; + // save true state in seperate state vector and calculate relative error in change + batchTruthProcessing(feature_msg->header.stamp.toSec()); + + + // cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -504,20 +500,20 @@ void MsckfVio::imageCallback( processing_end_time - processing_start_time; if (processing_time > 1.0/frame_rate) { ++critical_time_cntr; - ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", - processing_time, critical_time_cntr); - printf("IMU processing time: %f/%f\n", - imu_processing_time, imu_processing_time/processing_time); - printf("State augmentation time: %f/%f\n", - state_augmentation_time, state_augmentation_time/processing_time); - printf("Add observations time: %f/%f\n", - add_observations_time, add_observations_time/processing_time); - printf("Remove lost features time: %f/%f\n", - remove_lost_features_time, remove_lost_features_time/processing_time); - printf("Remove camera states time: %f/%f\n", - prune_cam_states_time, prune_cam_states_time/processing_time); - printf("Publish time: %f/%f\n", - publish_time, publish_time/processing_time); + //ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", + // processing_time, critical_time_cntr); + //printf("IMU processing time: %f/%f\n", + // imu_processing_time, imu_processing_time/processing_time); + //printf("State augmentation time: %f/%f\n", + // state_augmentation_time, state_augmentation_time/processing_time); + //printf("Add observations time: %f/%f\n", + // add_observations_time, add_observations_time/processing_time); + //printf("Remove lost features time: %f/%f\n", + // remove_lost_features_time, remove_lost_features_time/processing_time); + //printf("Remove camera states time: %f/%f\n", + // prune_cam_states_time, prune_cam_states_time/processing_time); + //printf("Publish time: %f/%f\n", + // publish_time, publish_time/processing_time); } if(STREAMPAUSE) @@ -548,25 +544,48 @@ void MsckfVio::manageMovingWindow( cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img, sensor_msgs::image_encodings::MONO8); + image_handler::undistortImage(cam0_img_ptr->image, cam0_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + + // save image information into moving window cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone(); cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone(); + cv::Mat xder; + cv::Mat yder; + cv::Mat deeper_frame; - // undistort Images + // generate derivative matrix for cam0 + cam0_img_ptr->image.convertTo(deeper_frame,CV_16S); + cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); + xder/=96.; + yder/=96.; - const cv::Matx33d K(cam0.intrinsics[0], 0.0, cam0.intrinsics[2], - 0.0, cam0.intrinsics[1], cam0.intrinsics[3], - 0.0, 0.0, 1.0); - - cv::Mat undistortedCam0; - cv::Mat undistortedCam1; + // save into moving window + cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); + cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); - image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); - image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + // generate derivative matrix for cam 1 + cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); + cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); + xder/=96.; + yder/=96.; - cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone(); - cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone(); + + /* + cv::Mat norm_abs_xderImage; + cv::convertScaleAbs(xder, norm_abs_xderImage); + cv::normalize(norm_abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); + + cv::imshow("xder", norm_abs_xderImage); + cvWaitKey(0); + */ + // save into moving window + cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); + cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) while(cam0.moving_window.size() > 100) @@ -684,7 +703,7 @@ bool MsckfVio::resetCallback( is_first_img = true; // Restart the subscribers. - imu_sub = nh.subscribe("imu", 100, + imu_sub = nh.subscribe("imu", 1000, &MsckfVio::imuCallback, this); truth_sub = nh.subscribe("ground_truth", 100, @@ -845,6 +864,76 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { truth_msg_buffer.erase(truth_msg_buffer.begin(), truth_msg_buffer.begin()+used_truth_msg_cntr); + /* + // calculate change + delta_position = state_server.imu_state.position - old_imu_state.position; + Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), old_imu_state.orientation); + delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2); + // calculate error in position + + // calculate error in change + + Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - old_true_state.position; + Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), old_true_state.orientation); + Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2); + Eigen::Vector3d error_delta_position = delta_true_position - delta_position; + Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation; + + Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position; + Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation); + Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2); + + double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/ + sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2])); + + double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + + cout << "relative pos error: " << relerr * 100 << "%" << endl; + cout << "relative ori error: " << relOerr * 100 << "%" << endl; + //cout << "absolute pos error: " << + */ + if (eval_time + 1 < ros::Time::now().toSec()) + { + + // calculate change + delta_position = state_server.imu_state.position - timed_old_imu_state.position; + Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), timed_old_imu_state.orientation); + delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2); + // calculate error in position + + // calculate error in change + + Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - timed_old_true_state.position; + Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), timed_old_true_state.orientation); + Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2); + Eigen::Vector3d error_delta_position = delta_true_position - delta_position; + Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation; + + Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position; + Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation); + Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2); + + double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/ + sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2])); + + double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + // cout << "relative pos error: " << relerr * 100 << "%" << endl; + // cout << "relative ori error: " << relOerr * 100 << "%" << endl; + + timed_old_imu_state = state_server.imu_state; + timed_old_true_state = true_state_server.imu_state; + eval_time = ros::Time::now().toSec(); + } } void MsckfVio::processTruthtoIMU(const double& time, @@ -1244,7 +1333,7 @@ void MsckfVio::addFeatureObservations( return; } -bool MsckfVio::PhotometricMeasurementJacobian( +void MsckfVio::twodotMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) @@ -1254,6 +1343,315 @@ bool MsckfVio::PhotometricMeasurementJacobian( 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 = R_c0_c1 * R_w_c0; + Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation(); + + // individual Jacobians + Matrix dh_dC0pij = Matrix::Zero(); + Matrix dh_dC1pij = Matrix::Zero(); + Matrix dh_dGpij = Matrix::Zero(); + Matrix dh_dXplj = Matrix::Zero(); + Matrix dGpj_drhoj = Matrix::Zero(); + Matrix dGpj_XpAj = Matrix::Zero(); + + Matrix dC0pij_dGpij = Matrix::Zero(); + Matrix dC1pij_dGpij = Matrix::Zero(); + Matrix dC0pij_dXplj = Matrix::Zero(); + Matrix dC1pij_dXplj = Matrix::Zero(); + + // one line of the NxN Jacobians + Eigen::Matrix H_rho; + Eigen::Matrix H_plj; + Eigen::Matrix H_pAj; + + auto frame = cam0.moving_window.find(cam_state_id)->second.image; + + auto point = feature.anchorPatch_3d[0]; + + Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + Eigen::Vector3d p_c1 = R_w_c1 * (point-t_c1_w); + // add jacobian + + //dh / d{}^Cp_{ij} + dh_dC0pij(0, 0) = 1. / p_c0(2); + dh_dC0pij(1, 1) = 1. / p_c0(2); + dh_dC0pij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); + dh_dC0pij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); + + //dh / d{}^Cp_{ij} + dh_dC1pij(2, 0) = 1. / p_c1(2); + dh_dC1pij(3, 1) = 1. / p_c1(2); + dh_dC1pij(2, 2) = -(p_c1(0))/(p_c1(2)*p_c1(2)); + dh_dC1pij(3, 2) = -(p_c1(1))/(p_c1(2)*p_c1(2)); + + dC0pij_dGpij = R_w_c0; + dC1pij_dGpij = R_c0_c1 * R_w_c0; + + dC0pij_dXplj.leftCols(3) = skewSymmetric(p_c0); + dC0pij_dXplj.rightCols(3) = -R_w_c0; + + + // 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[(N*N-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(N*N-1)/2].y/(rho*rho), 1/(rho*rho)); + + dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() + * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho), + feature.anchorPatch_ideal[(N*N-1)/2].y/(rho), + 1/(rho))); + dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); + + // Intermediate Jakobians + H_rho = dh_dC0pij * dC0pij_dGpij * dGpj_drhoj + dh_dC1pij * dC1pij_dGpij * dGpj_drhoj; // 4 x 1 + H_plj = dh_dC0pij * dC0pij_dXplj + dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6 + H_pAj = dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 + + // calculate residual + + //observation + const Vector4d& total_z = feature.observations.find(cam_state_id)->second; + + VectorXd r_i = VectorXd::Zero(4); + + //calculate residual + + r_i[0] = total_z[0] - p_c0(0)/p_c0(2); + r_i[1] = total_z[1] - p_c0(1)/p_c0(2); + r_i[2] = total_z[2] - p_c1(0)/p_c1(2); + r_i[3] = total_z[3] - p_c1(1)/p_c1(2); + + MatrixXd H_xl = MatrixXd::Zero(4, 21+state_server.cam_states.size()*7); + + // set anchor Jakobi + // 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, 4, 6) = H_pAj; + + // set frame Jakobi + //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, 4, 6) = H_plj; + + H_x = H_xl; + H_y = H_rho; + r = r_i; + + //TODO make this more fluent as well + if(PRINTIMAGES) + { + std::stringstream ss; + ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; + feature.MarkerGeneration(marker_pub, state_server.cam_states); + } + + return; +} + +void MsckfVio::twodotFeatureJacobian( + 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; + + if (feature.observations.find(cam_id) == + feature.observations.begin()) 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_xi = MatrixXd::Zero(jacobian_row_size, + 21+state_server.cam_states.size()*7); + + MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1); + + VectorXd r_i = VectorXd::Zero(jacobian_row_size); + int stack_cntr = 0; + + for (const auto& cam_id : valid_cam_state_ids) { + + MatrixXd H_xl; + MatrixXd H_yl; + + Eigen::VectorXd r_l = VectorXd::Zero(4); + + twodotMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); + 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_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl; + H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl; + + r_i.segment(stack_cntr, 4) = r_l; + stack_cntr += 4; + } + + + H_xi.conservativeResize(stack_cntr, H_xi.cols()); + H_yi.conservativeResize(stack_cntr, H_yi.cols()); + // Project the residual and Jacobians onto the nullspace + // of H_yj. + + + // get Nullspace + FullPivLU lu(H_yi.transpose()); + MatrixXd A_null_space = lu.kernel(); + + H_x = A_null_space.transpose() * H_xi; + r = A_null_space.transpose() * r_i; + + if(PRINTIMAGES) + { + + ofstream myfile; + myfile.open("/home/raphael/dev/octave/two2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: Hx\n" + << "# type: matrix\n" + << "# rows: " << H_xi.rows() << "\n" + << "# columns: " << H_xi.cols() << "\n" + << H_xi << endl; + + myfile << "# name: Hy\n" + << "# type: matrix\n" + << "# rows: " << H_yi.rows() << "\n" + << "# columns: " << H_yi.cols() << "\n" + << H_yi << endl; + + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r_i.rows() << "\n" + << "# columns: " << 1 << "\n" + << r_i << endl; + + + myfile << "# name: A\n" + << "# type: matrix\n" + << "# rows: " << A_null_space.rows() << "\n" + << "# columns: " << 1 << "\n" + << A_null_space << endl; + + myfile.close(); + std::cout << "resume playback" << std::endl; + nh.setParam("/play_bag", true); + } + return; +} + + +bool MsckfVio::PhotometricPatchPointResidual( + const StateIDType& cam_state_id, + const Feature& feature, + VectorXd& r) +{ + VectorXd r_photo = VectorXd::Zero(2*N*N); + int count = 0; + const CAMState& cam_state = state_server.cam_states[cam_state_id]; + + //estimate photometric measurement + std::vector estimate_irradiance; + IlluminationParameter estimated_illumination; + std::vector estimate_photo_z_c0, estimate_photo_z_c1; + std::vector photo_z_c0, photo_z_c1; + + // estimate irradiance based on anchor frame + + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); + for (auto& estimate_irradiance_j : estimate_irradiance) + estimate_photo_z_c0.push_back (estimate_irradiance_j);// * + //estimated_illumination.frame_gain * estimated_illumination.feature_gain + + //estimated_illumination.frame_bias + estimated_illumination.feature_bias); + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam1, estimate_irradiance, estimated_illumination); + for (auto& estimate_irradiance_j : estimate_irradiance) + estimate_photo_z_c1.push_back (estimate_irradiance_j);// * + //estimated_illumination.frame_gain * estimated_illumination.feature_gain + + //estimated_illumination.frame_bias + estimated_illumination.feature_bias); + + // irradiance measurement around feature point in c0 and c1 + std::vector true_irradiance_c0, true_irradiance_c1; + cv::Point2f p_f_c0(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1)); + cv::Point2f p_f_c1(feature.observations.find(cam_state_id)->second(2), feature.observations.find(cam_state_id)->second(3)); + + p_f_c0 = image_handler::distortPoint(p_f_c0, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs); + p_f_c1 = image_handler::distortPoint(p_f_c1, cam1.intrinsics, cam1.distortion_model, cam1.distortion_coeffs); + + cv::Mat current_image_c0 = cam0.moving_window.find(cam_state_id)->second.image; + cv::Mat current_image_c1 = cam1.moving_window.find(cam_state_id)->second.image; + for(int i = 0; i current_image_c0.cols-1 or p_in_c0.y < 0 or p_in_c0.y > current_image_c0.rows-1) + return false; + if(p_in_c1.x < 0 or p_in_c1.x > current_image_c1.cols-1 or p_in_c1.y < 0 or p_in_c1.y > current_image_c1.rows-1) + return false; + + // add observation + photo_z_c0.push_back(feature.PixelIrradiance(p_in_c0, current_image_c0)); + photo_z_c1.push_back(feature.PixelIrradiance(p_in_c1, current_image_c1)); + + // calculate photom. residual acc. to paper + //r_photo(count*2) = photo_z_c0[count] - estimate_photo_z_c0[count]; + //r_photo(count*2+1) = photo_z_c1[count] - estimate_photo_z_c1[count]; + + // calculate photom. residual alternating between frames + r_photo(count*2) = true_irradiance_c0[count] - photo_z_c0[count]; + r_photo(count*2+1) = true_irradiance_c1[count] - photo_z_c1[count]; + count++; + } + r = r_photo; + return true; +} + +// generates the jacobian of one patch point regarding rho, anchor and current frame +bool MsckfVio::PhotometricPatchPointJacobian( + const CAMState& cam_state, + const StateIDType& cam_state_id, + const Feature& feature, + Eigen::Vector3d point, + int count, + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj, + Matrix& dI_dhj) +{ + const StateIDType anchor_state_id = feature.observations.begin()->first; const CAMState anchor_state = state_server.cam_states[anchor_state_id]; @@ -1266,185 +1664,229 @@ bool MsckfVio::PhotometricMeasurementJacobian( 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(); - - //photometric observation - std::vector photo_z; - VectorXd r_photo = VectorXd::Zero(N*N); + // individual Jacobians - Matrix dI_dhj = Matrix::Zero(); - Matrix dh_dCpij = Matrix::Zero(); - Matrix dh_dGpij = Matrix::Zero(); - Matrix dh_dXplj = Matrix::Zero(); + /*Matrix */dI_dhj = Matrix::Zero(); + + Matrix dh_dC0pij = Matrix::Zero(); + Matrix dh_dC1pij = Matrix::Zero(); + Matrix dh_dGpij = Matrix::Zero(); + Matrix dh_dXplj = Matrix::Zero(); Matrix dGpj_drhoj = Matrix::Zero(); Matrix dGpj_XpAj = Matrix::Zero(); - Matrix dCpij_dGpij = Matrix::Zero(); - Matrix dCpij_dCGtheta = Matrix::Zero(); - Matrix dCpij_dGpC = Matrix::Zero(); + Matrix dC0pij_dGpij = Matrix::Zero(); + Matrix dC1pij_dGpij = Matrix::Zero(); + Matrix dC0pij_dXplj = Matrix::Zero(); + Matrix dC1pij_dXplj = Matrix::Zero(); + + // irradiance jacobian + double dx_c0, dy_c0, dx_c1, dy_c1; + + Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + Eigen::Vector3d p_c1 = R_w_c1 * (point-t_c1_w); + + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + cv::Point2f p_in_c1 = feature.projectPositionToCamera(cam_state, cam_state_id, cam1, point); + + cv::Point2f p_in_anchor = feature.projectPositionToCamera(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_c0 = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x"); + dy_c0 = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y"); + + dx_c1 = feature.CompleteCvKernel(p_in_c1, cam_state_id, cam1, "Sobel_x"); + dy_c1 = feature.CompleteCvKernel(p_in_c1, cam_state_id, cam1, "Sobel_y"); + + dI_dhj(0, 0) = dx_c0 * cam0.intrinsics[0]; + dI_dhj(0, 1) = dy_c0 * cam0.intrinsics[1]; + dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0]; + dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1]; + + //cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; + + // add jacobian + + //dh / d{}^Cp_{ij} + dh_dC0pij(0, 0) = 1. / p_c0(2); + dh_dC0pij(1, 1) = 1. / p_c0(2); + dh_dC0pij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2)); + dh_dC0pij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2)); + + //dh / d{}^Cp_{ij} + dh_dC1pij(2, 0) = 1. / p_c1(2); + dh_dC1pij(3, 1) = 1. / p_c1(2); + dh_dC1pij(2, 2) = -(p_c1(0))/(p_c1(2)*p_c1(2)); + dh_dC1pij(3, 2) = -(p_c1(1))/(p_c1(2)*p_c1(2)); + + dC0pij_dGpij = R_w_c0; + dC1pij_dGpij = R_c0_c1 * R_w_c0; + + dC0pij_dXplj.leftCols(3) = skewSymmetric(p_c0); + dC0pij_dXplj.rightCols(3) = -R_w_c0; + + + // 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[(N*N-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(N*N-1)/2].y/(rho*rho), 1/(rho*rho)); + + dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() + * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho), + feature.anchorPatch_ideal[(N*N-1)/2].y/(rho), + 1/(rho))); + dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); + + // Intermediate Jakobians + H_rhoj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_drhoj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_drhoj; // 4 x 1 + H_plj = dI_dhj * dh_dC0pij * dC0pij_dXplj + dI_dhj * dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6 + H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 + + // check if any direction not large enough for eval + /*if((dI_dhj(0, 0) < 0.1 or dI_dhj(0, 1) < 0.1) and (dI_dhj(1, 2) < 0.1 or dI_dhj(1, 3) < 0.1)) + return false; + */ + // check if point nullspaceable + if (H_rhoj(0, 0) != 0) + return true; + + return false; +} + +bool MsckfVio::PhotometricMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) +{ + // Prepare all the required data. + const CAMState& cam_state = state_server.cam_states[cam_state_id]; + const Feature& feature = map_server[feature_id]; + + //photometric observation + VectorXd r_photo; // one line of the NxN Jacobians - Eigen::Matrix H_rhoj; - Eigen::Matrix H_plj; - Eigen::Matrix H_pAj; + Eigen::Matrix H_rhoj; + Eigen::Matrix H_plj; + Eigen::Matrix H_pAj; + + Eigen::MatrixXd dI_dh(2* N*N, 4); // combined Jacobians - Eigen::MatrixXd H_rho(N*N, 1); - Eigen::MatrixXd H_pl(N*N, 6); - Eigen::MatrixXd H_pA(N*N, 6); + Eigen::MatrixXd H_rho(2 * N*N, 1); + Eigen::MatrixXd H_pl(2 * N*N, 6); + Eigen::MatrixXd H_pA(2 * 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; - - //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); + // calcualte residual of patch + if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) + return false; + //cout << "r\n" << r_photo << endl; + // calculate jacobian for patch int count = 0; - double dx, dy; - - // gradient visualization parameters - cv::Point2f gradientVector(0,0); - - // residual change visualization - cv::Point2f residualVector(0,0); - double res_sum = 0; - + bool valid = false; + Matrix dI_dhj;// = Matrix::Zero(); + int valid_count = 0; 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); - - if( p_in_c0.x < 0 || p_in_c0.x >= cam0.resolution(0) || p_in_c0.y < 0 || p_in_c0.y >= cam0.resolution(1) ) + // get jacobi of single point in patch + if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj)) { - cout << "skip" << endl; - return false; + valid_count++; + valid = true; } - //add observation - photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - //calculate photom. residual - r_photo(count) = photo_z[count] - estimate_photo_z[count]; - //cout << "residual: " << photo_r.back() << endl; - // calculate derivation for anchor frame, use position for derivation calculation - // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.cvKernel(p_in_anchor, "Sobel_x"); - dy = feature.cvKernel(p_in_anchor, "Sobel_y"); - - // 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 * cam0.intrinsics[0]; - dI_dhj(0, 1) = dy * cam0.intrinsics[1]; + // stack point into entire jacobi + H_rho.block<2, 1>(count*2, 0) = H_rhoj; + H_pl.block<2, 6>(count*2, 0) = H_plj; + H_pA.block<2, 6>(count*2, 0) = H_pAj; - - //dh / d{}^Cp_{ij} - dh_dCpij(0, 0) = 1 / p_c0(2); - dh_dCpij(1, 1) = 1 / p_c0(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)); - - dCpij_dGpij = quaternionToRotation(cam_state.orientation); - - //orientation takes camera frame to world frame, we wa - dh_dGpij = dh_dCpij * dCpij_dGpij; - - //dh / d X_{pl} - dCpij_dCGtheta = skewSymmetric(p_c0); - dCpij_dGpC = -quaternionToRotation(cam_state.orientation); - 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 - 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)); - - dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() - * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), - feature.anchorPatch_ideal[count].y/(rho), - 1/(rho))); - dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); - - // 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_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; + dI_dh.block<2, 4>(count*2, 0) = dI_dhj; count++; } + // cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; + // Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; + // cout << "h photo: \n" << h_photo << endl; - - MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - //MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); + // construct the jacobian structure needed for nullspacing + MatrixXd H_xl; + MatrixXd H_yl; - MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1); + ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); - // set anchor Jakobi - // 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 - //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; - - // 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; - */ - H_yl.block(0, 0, N*N, N*N) = Eigen::MatrixXd::Identity(N*N, N*N); - H_yl.block(0, N*N, N*N, 1) = -H_rho; - + // set to return values H_x = H_xl; H_y = H_yl; - - //TODO make this more fluent as well r = r_photo; - std::stringstream ss; - ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) - { + { + // pregenerating information for visualization + std::stringstream ss; + 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); + //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); + + ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; + + // visualizing functions feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); + feature.VisualizePatch(cam_state, cam_state_id, cam1, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } - return true; + if(valid) + return true; + else + return false; } -void MsckfVio::PhotometricFeatureJacobian( +// uses the calcualted jacobians to construct the final Hx Hy jacobians used for nullspacing +bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, + Eigen::MatrixXd& H_pl, + Eigen::MatrixXd& H_pA, + const Feature& feature, + const StateIDType& cam_state_id, + Eigen::MatrixXd& H_xl, + Eigen::MatrixXd& H_yl) +{ + H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7); + H_yl = MatrixXd::Zero(2*N*N, 1); + + // 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); + // set anchor Jakobi + H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*N*N, 6) = H_pA; + + //get position of current frame in cam states + auto cam_state_iter = state_server.cam_states.find(cam_state_id); + // set frame Jakobi + 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, 2*N*N, 6) = H_pl; + + // 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); + + + /*for(int i = 0; i< N*N; i++) + H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; + */ + + H_yl.block(0, 0, H_rho.rows(), H_rho.cols()) = H_rho; +} + + +bool MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) @@ -1465,53 +1907,67 @@ void MsckfVio::PhotometricFeatureJacobian( } int jacobian_row_size = 0; - jacobian_row_size = N * N * valid_cam_state_ids.size(); + + // stacked feature jacobians + MatrixXd H_xi; + MatrixXd H_yi; + VectorXd r_i; - MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, - 21+state_server.cam_states.size()*7); - - // MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1); - MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+1); - - VectorXd r_i = VectorXd::Zero(jacobian_row_size); + // temporary measurement jacobians + MatrixXd H_xl; + MatrixXd H_yl; + Eigen::VectorXd r_l; int stack_cntr = 0; + bool first = true; + // go through every valid state the feature was observed in for (const auto& cam_id : valid_cam_state_ids) { - MatrixXd H_xl; - MatrixXd H_yl; - Eigen::VectorXd r_l = VectorXd::Zero(N*N); - + // skip observation if measurement is not valid if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) - continue; + continue; + // set size of stacking jacobians, once the returned jacobians are known + if(first) + { + first = not first; + jacobian_row_size = r_l.size() * valid_cam_state_ids.size(); + H_xi = MatrixXd::Zero(jacobian_row_size, H_xl.cols()); + H_yi = MatrixXd::Zero(jacobian_row_size, H_yl.cols()); // CHANGED N*N+1 to 1 + r_i = VectorXd::Zero(jacobian_row_size); + } 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_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl; H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl; - r_i.segment(stack_cntr, N*N) = r_l; - stack_cntr += N*N; - - + r_i.segment(stack_cntr, r_l.size()) = r_l; + stack_cntr += r_l.size(); } + // if not enough to propper nullspace (in paper implementation) + if(stack_cntr < r_l.size()) + return false; - H_xi.conservativeResize(stack_cntr, H_xi.cols()); - H_yi.conservativeResize(stack_cntr, H_yi.cols()); // Project the residual and Jacobians onto the nullspace // of H_yj. // get Nullspace + + bool valid = false; + for(int i = 0; i < H_yi.rows(); i++) + if(H_yi(i,0) != 0) + valid = true; + + FullPivLU lu(H_yi.transpose()); MatrixXd A_null_space = lu.kernel(); H_x = A_null_space.transpose() * H_xi; r = A_null_space.transpose() * r_i; - - Eigen::MatrixXd xres = H_x.colPivHouseholderQr().solve(r); + //cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; if(PRINTIMAGES) { @@ -1551,26 +2007,32 @@ void MsckfVio::PhotometricFeatureJacobian( << "# columns: " << H_yi.cols() << "\n" << H_yi << endl; - myfile << "# name: r\n" << "# type: matrix\n" << "# rows: " << r_i.rows() << "\n" << "# columns: " << 1 << "\n" << r_i << endl; - - myfile << "# name: xres\n" + myfile << "# name: A\n" << "# type: matrix\n" - << "# rows: " << xres.rows() << "\n" - << "# columns: " << xres.cols() << "\n" - << xres << endl; + << "# rows: " << A_null_space.rows() << "\n" + << "# columns: " << A_null_space.cols() << "\n" + << A_null_space << endl; + + myfile << "# name: P\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.rows() << "\n" + << state_server.state_cov << endl; + myfile.close(); cout << "---------- LOGGED -------- " << endl; - cvWaitKey(0); } - return; + if(valid) + return true; + return false; } void MsckfVio::measurementJacobian( @@ -1635,6 +2097,7 @@ void MsckfVio::measurementJacobian( 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)); + // cout << "h reg: \n" << r(0) << "\n" << r(1) << endl; return; } @@ -1739,7 +2202,41 @@ void MsckfVio::featureJacobian( << xres << endl; myfile.close(); - cout << "---------- LOGGED ORG -------- " << endl; + + myfile.open("/home/raphael/dev/octave/org2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: Hx\n" + << "# type: matrix\n" + << "# rows: " << H_xj.rows() << "\n" + << "# columns: " << H_xj.cols() << "\n" + << H_xj << endl; + + myfile << "# name: Hy\n" + << "# type: matrix\n" + << "# rows: " << H_fj.rows() << "\n" + << "# columns: " << H_fj.cols() << "\n" + << H_fj << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r_j.rows() << "\n" + << "# columns: " << 1 << "\n" + << r_j << endl; + + myfile << "# name: A\n" + << "# type: matrix\n" + << "# rows: " << A.rows() << "\n" + << "# columns: " << A.cols() << "\n" + << A << endl; + + myfile << "# name: P\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.rows() << "\n" + << state_server.state_cov << endl; + + + myfile.close(); } return; @@ -1780,19 +2277,47 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; + MatrixXd S = H_thin*P*H_thin.transpose() + Feature::observation_noise*MatrixXd::Identity( H_thin.rows(), H_thin.rows()); + + + //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); MatrixXd K_transpose = S.ldlt().solve(H_thin*P); MatrixXd K = K_transpose.transpose(); // Compute the error of the state. VectorXd delta_x = K * r; - // cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - // cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; - // Update the IMU state. - if(PHOTOMETRIC) return; + + // cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; + // cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + + if(FILTER != 0) return; + + if(PRINTIMAGES) + { + //octave + ofstream myfile; + + myfile.open("/home/raphael/dev/octave/measurement2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: K\n" + << "# type: matrix\n" + << "# rows: " << K.rows() << "\n" + << "# columns: " << K.cols() << "\n" + << K << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << r.cols() << "\n" + << r << endl; + + myfile.close(); + } + // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1848,8 +2373,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { return; } +void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { -void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { if (H.rows() == 0 || r.rows() == 0) { @@ -1859,8 +2384,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; - - // QR decomposition to make stuff faster + if (H.rows() > H.cols()) { // Convert H to a sparse matrix. SparseMatrix H_sparse = H.sparseView(); @@ -1878,12 +2402,19 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r H_thin = H_temp.topRows(21+state_server.cam_states.size()*7); r_thin = r_temp.head(21+state_server.cam_states.size()*7); + //HouseholderQR qr_helper(H); + //MatrixXd Q = qr_helper.householderQ(); + //MatrixXd Q1 = Q.leftCols(21+state_server.cam_states.size()*6); + + //H_thin = Q1.transpose() * H; + //r_thin = Q1.transpose() * r; } else { H_thin = H; r_thin = r; } + // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; MatrixXd S = H_thin*P*H_thin.transpose() + @@ -1896,12 +2427,12 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r VectorXd delta_x = K * r; // Update the IMU state. + if (FILTER != 2) return; - - if (not PHOTOMETRIC) return; - - cout << "msc veloci: " << delta_x[6] << ", " << delta_x[7] << ", " << delta_x[8] << endl; - cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + //cout << "two: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + + delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); + delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1957,26 +2488,179 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r } -bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - return 1; - MatrixXd P1 = H * state_server.state_cov * H.transpose(); +void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { + if (H.rows() == 0 || r.rows() == 0) + return; + // Decompose the final Jacobian matrix to reduce computational + // complexity as in Equation (28), (29). + MatrixXd H_thin; + VectorXd r_thin; + + // QR decomposition to make stuff faster + if (H.rows() > H.cols()) { + // Convert H to a sparse matrix. + SparseMatrix H_sparse = H.sparseView(); + + // Perform QR decompostion on H_sparse. + SPQR > spqr_helper; + spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); + + spqr_helper.compute(H_sparse); + + MatrixXd H_temp; + VectorXd r_temp; + + (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp); + (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp); + + H_thin = H_temp.topRows(21+state_server.cam_states.size()*7); + r_thin = r_temp.head(21+state_server.cam_states.size()*7); + + } else { + H_thin = H; + r_thin = r; + } + + // Compute the Kalman gain. + const MatrixXd& P = state_server.state_cov; + MatrixXd S = H_thin*P*H_thin.transpose() + + Feature::observation_noise*MatrixXd::Identity( + H_thin.rows(), H_thin.rows()); + //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); + + MatrixXd K_transpose = S.ldlt().solve(H_thin*P); + MatrixXd K = K_transpose.transpose(); + // Compute the error of the state. + + VectorXd delta_x = K * r; + // Update the IMU state. + cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + + + if (FILTER != 1) return; + + if(PRINTIMAGES) + { + //octave + ofstream myfile; + + myfile.open("/home/raphael/dev/octave/measurement2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: K\n" + << "# type: matrix\n" + << "# rows: " << K.rows() << "\n" + << "# columns: " << K.cols() << "\n" + << K << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << r.cols() << "\n" + << r << endl; + + myfile.close(); + } + const VectorXd& delta_x_imu = delta_x.head<21>(); + + if (//delta_x_imu.segment<3>(0).norm() > 0.15 || + //delta_x_imu.segment<3>(3).norm() > 0.15 || + delta_x_imu.segment<3>(6).norm() > 0.5 || + //delta_x_imu.segment<3>(9).norm() > 0.5 || + delta_x_imu.segment<3>(12).norm() > 1.0) { + printf("delta velocity: %f\n", delta_x_imu.segment<3>(6).norm()); + printf("delta position: %f\n", delta_x_imu.segment<3>(12).norm()); + ROS_WARN("Update change is too large."); + //return; + } + + const Vector4d dq_imu = + smallAngleQuaternion(delta_x_imu.head<3>()); + state_server.imu_state.orientation = quaternionMultiplication( + dq_imu, state_server.imu_state.orientation); + state_server.imu_state.gyro_bias += delta_x_imu.segment<3>(3); + state_server.imu_state.velocity += delta_x_imu.segment<3>(6); + state_server.imu_state.acc_bias += delta_x_imu.segment<3>(9); + state_server.imu_state.position += delta_x_imu.segment<3>(12); + + const Vector4d dq_extrinsic = + smallAngleQuaternion(delta_x_imu.segment<3>(15)); + state_server.imu_state.R_imu_cam0 = quaternionToRotation( + dq_extrinsic) * state_server.imu_state.R_imu_cam0; + state_server.imu_state.t_cam0_imu += delta_x_imu.segment<3>(18); + + // Update the camera states. + auto cam_state_iter = state_server.cam_states.begin(); + for (int i = 0; i < state_server.cam_states.size(); + ++i, ++cam_state_iter) { + const VectorXd& delta_x_cam = delta_x.segment(21+i*7, 6); + const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>()); + cam_state_iter->second.orientation = quaternionMultiplication( + dq_cam, cam_state_iter->second.orientation); + cam_state_iter->second.position += delta_x_cam.tail<3>(); + cam_state_iter->second.illumination.frame_bias += delta_x(21+i*7+6); + } + + // Update state covariance. + MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin; + //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() + + // K*K.transpose()*Feature::observation_noise; + state_server.state_cov = I_KH*state_server.state_cov; + + // Fix the covariance to be symmetric + MatrixXd state_cov_fixed = (state_server.state_cov + + state_server.state_cov.transpose()) / 2.0; + state_server.state_cov = state_cov_fixed; + + return; +} + + +bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, int filter) { + + MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * MatrixXd::Identity(H.rows(), H.rows()); double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - cout << "gate: " << dof << " " << gamma << " " << - chi_squared_test_table[dof] << endl; + if(gamma > 1000000) + { + cout << " logging " << endl; + ofstream myfile; + myfile.open("/home/raphael/dev/octave/log2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: H\n" + << "# type: matrix\n" + << "# rows: " << H.rows() << "\n" + << "# columns: " << H.cols() << "\n" + << H << endl; - if (chi_squared_test_table[dof] == 0) + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << 1 << "\n" + << r << endl; + + myfile << "# name: C\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.cols() << "\n" + << state_server.state_cov << endl; + myfile.close(); + } + + if (chi_squared_test_table[dof] == 0) return false; if (gamma < chi_squared_test_table[dof]) { - //cout << "passed" << endl; + // cout << "passed" << endl; + if(filter == 1) + cout << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; return true; } else { - //cout << "failed" << endl; + // cout << "failed" << endl; return false; } } @@ -1986,6 +2670,7 @@ void MsckfVio::removeLostFeatures() { // BTW, find the size the final Jacobian matrix and residual vector. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int twojacobian_row_size = 0; vector invalid_feature_ids(0); vector processed_feature_ids(0); @@ -2025,8 +2710,10 @@ void MsckfVio::removeLostFeatures() { } } - pjacobian_row_size += N*N*feature.observations.size(); + pjacobian_row_size += 2*N*N*feature.observations.size(); + twojacobian_row_size += 4*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; + processed_feature_ids.push_back(feature.id); } @@ -2052,6 +2739,12 @@ void MsckfVio::removeLostFeatures() { VectorXd pr = VectorXd::Zero(pjacobian_row_size); int pstack_cntr = 0; + + MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, + 21+7*state_server.cam_states.size()); + VectorXd twor = VectorXd::Zero(twojacobian_row_size); + int twostack_cntr = 0; + // Process the features which lose track. for (const auto& feature_id : processed_feature_ids) { auto& feature = map_server[feature_id]; @@ -2064,35 +2757,54 @@ void MsckfVio::removeLostFeatures() { VectorXd r_j; MatrixXd pH_xj; VectorXd pr_j; - - PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); + MatrixXd twoH_xj; + VectorXd twor_j; + + if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) + { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { + pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; + pr.segment(pstack_cntr, pr_j.rows()) = pr_j; + pstack_cntr += pH_xj.rows(); + } + } + featureJacobian(feature.id, cam_state_ids, H_xj, r_j); + twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); + 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(); } - - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { - pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; - pr.segment(pstack_cntr, pr_j.rows()) = pr_j; - pstack_cntr += pH_xj.rows(); - } + if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + twor.segment(twostack_cntr, twor_j.rows()) = twor_j; + twostack_cntr += twoH_xj.rows(); + } + // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. //if (stack_cntr > 1500) break; } + if(pstack_cntr) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + photometricMeasurementUpdate(pH_x, pr); + } H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); + + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); // Perform the measurement update step. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); - + twoMeasurementUpdate(twoH_x, twor); + // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) map_server.erase(feature_id); @@ -2155,6 +2867,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Set the size of the Jacobian matrix. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int twojacobian_row_size = 0; //initialize all the features which are going to be removed @@ -2191,8 +2904,9 @@ void MsckfVio::pruneLastCamStateBuffer() } } - pjacobian_row_size += N*N*feature.observations.size(); + pjacobian_row_size += 2*N*N*feature.observations.size(); jacobian_row_size += 4*feature.observations.size() - 3; + twojacobian_row_size += 4*feature.observations.size(); } @@ -2204,9 +2918,14 @@ void MsckfVio::pruneLastCamStateBuffer() VectorXd pr_j; MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size()); VectorXd pr = VectorXd::Zero(pjacobian_row_size); + MatrixXd twoH_xj; + VectorXd twor_j; + MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); + VectorXd twor = VectorXd::Zero(twojacobian_row_size); int stack_cntr = 0; int pruned_cntr = 0; int pstack_cntr = 0; + int twostack_cntr = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2221,8 +2940,19 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) + { + + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { + pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; + pr.segment(pstack_cntr, pr_j.rows()) = pr_j; + pstack_cntr += pH_xj.rows(); + } + } + + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); 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; @@ -2230,25 +2960,34 @@ void MsckfVio::pruneLastCamStateBuffer() stack_cntr += H_xj.rows(); pruned_cntr++; } - if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { - pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; - pr.segment(pstack_cntr, pr_j.rows()) = pr_j; - pstack_cntr += pH_xj.rows(); + + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + twor.segment(twostack_cntr, twor_j.rows()) = twor_j; + twostack_cntr += twoH_xj.rows(); } + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } + if(pstack_cntr) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + photometricMeasurementUpdate(pH_x, pr); + } + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - - pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); - // Perform measurement update. - + + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); + + // Perform the measurement update step. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); - + twoMeasurementUpdate(twoH_x, twor); + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -2300,6 +3039,7 @@ void MsckfVio::pruneCamStateBuffer() { // Find the size of the Jacobian matrix. int jacobian_row_size = 0; int pjacobian_row_size = 0; + int twojacobian_row_size = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2344,7 +3084,8 @@ void MsckfVio::pruneCamStateBuffer() { } } - pjacobian_row_size += N*N*involved_cam_state_ids.size(); + twojacobian_row_size += 4*involved_cam_state_ids.size(); + pjacobian_row_size += 2*N*N*involved_cam_state_ids.size(); jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -2361,6 +3102,11 @@ void MsckfVio::pruneCamStateBuffer() { MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size()); VectorXd pr = VectorXd::Zero(pjacobian_row_size); int pstack_cntr = 0; + MatrixXd twoH_xj; + VectorXd twor_j; + MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size()); + VectorXd twor = VectorXd::Zero(twojacobian_row_size); + int twostack_cntr = 0; for (auto& item : map_server) { auto& feature = item.second; // Check how many camera states to be removed are associated @@ -2373,36 +3119,53 @@ void MsckfVio::pruneCamStateBuffer() { } if (involved_cam_state_ids.size() == 0) continue; - - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) + { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// involved_cam_state_ids.size())) { + pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; + pr.segment(pstack_cntr, pr_j.rows()) = pr_j; + pstack_cntr += pH_xj.rows(); + } + } featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - + twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); + if (gatingTest(H_xj, r_j, involved_cam_state_ids.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(); } - - if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { - pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; - pr.segment(pstack_cntr, pr_j.rows()) = pr_j; - pstack_cntr += pH_xj.rows(); + + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + twor.segment(twostack_cntr, twor_j.rows()) = twor_j; + twostack_cntr += twoH_xj.rows(); } + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); - } + } + + if(pstack_cntr) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + photometricMeasurementUpdate(pH_x, pr); + } H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); - - // Perform measurement update. + + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); + + // Perform the measurement update step. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); - + twoMeasurementUpdate(twoH_x, twor); + //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), @@ -2548,6 +3311,23 @@ void MsckfVio::publish(const ros::Time& time) { T_i_c_tf, time, child_frame_id, "camera")); } + // Publish true odometry + // Ground truth odometry. + nav_msgs::Odometry truth_odom_msg; + truth_odom_msg.header.stamp = time; + truth_odom_msg.header.frame_id = fixed_frame_id; + truth_odom_msg.child_frame_id = child_frame_id+"_true"; + + Eigen::Isometry3d true_T_i_w = Eigen::Isometry3d::Identity(); + true_T_i_w.linear() = quaternionToRotation( + true_state_server.imu_state.orientation).transpose(); + true_T_i_w.translation() = true_state_server.imu_state.position; + tf::poseEigenToMsg(true_T_i_w, truth_odom_msg.pose.pose); + Eigen::Isometry3d true_T_b_w = IMUState::T_imu_body * true_T_i_w * + IMUState::T_imu_body.inverse(); + + tf::poseEigenToMsg(true_T_b_w, truth_odom_msg.pose.pose); + truth_odom_pub.publish(truth_odom_msg); // Publish the odometry nav_msgs::Odometry odom_msg; odom_msg.header.stamp = time;