diff --git a/config/camchain-imucam-tum-scaled.yaml b/config/camchain-imucam-tum-scaled.yaml index 4facacc..f5ea7e8 100644 --- a/config/camchain-imucam-tum-scaled.yaml +++ b/config/camchain-imucam-tum-scaled.yaml @@ -7,7 +7,7 @@ cam0: camera_model: pinhole distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] resolution: [512, 512] rostopic: /cam0/image_raw @@ -25,7 +25,7 @@ cam1: camera_model: pinhole distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] - distortion_model: equidistant + distortion_model: pre-equidistant intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] resolution: [512, 512] rostopic: /cam1/image_raw diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 6c55861..8d5c9c1 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -224,7 +224,7 @@ 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) const; /* @@ -740,7 +740,7 @@ bool Feature::VisualizeKernel( bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, - CameraCalibration& cam0, + CameraCalibration& cam, const Eigen::VectorXd& photo_r, std::stringstream& ss) const { @@ -749,7 +749,7 @@ bool Feature::VisualizePatch( //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); @@ -761,10 +761,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 @@ -772,7 +772,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); @@ -780,7 +780,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 @@ -829,7 +829,7 @@ bool Feature::VisualizePatch( cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1)); // move to real pixels - 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 @@ -884,7 +884,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 @@ -916,15 +916,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? @@ -932,9 +932,9 @@ 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); + cv::imshow("patch", cam.featureVisu); cvWaitKey(1); } diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index 0e5985c..f862755 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -14,8 +14,8 @@ - - + + @@ -24,9 +24,9 @@ - - - + + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 172fda2..00b2c99 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1828,7 +1828,7 @@ bool MsckfVio::PhotometricMeasurementJacobian( // visualizing functions feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); + feature.VisualizePatch(cam_state, cam_state_id, cam1, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); }