raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

changed visualization

Browse Source
This commit is contained in:
Raphael Maenle 2019-07-12 14:01:11 +02:00
parent 1380ec426f
commit 02156bd89a
4 changed files with 25 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
config/camchain-imucam-tum-scaled.yaml
View File

@ -7,7 +7,7 @@ cam0:
camera_model: pinhole camera_model: pinhole
distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202,
0.00020293673591811182] 0.00020293673591811182]
distortion_model: equidistant distortion_model: pre-equidistant
intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]
resolution: [512, 512] resolution: [512, 512]
rostopic: /cam0/image_raw rostopic: /cam0/image_raw
@ -25,7 +25,7 @@ cam1:
camera_model: pinhole camera_model: pinhole
distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606,
0.0003299517423931039] 0.0003299517423931039]
distortion_model: equidistant distortion_model: pre-equidistant
intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983]
resolution: [512, 512] resolution: [512, 512]
rostopic: /cam1/image_raw rostopic: /cam1/image_raw

34
include/msckf_vio/feature.hpp
View File

@ -224,7 +224,7 @@ bool VisualizeKernel(
bool VisualizePatch( bool VisualizePatch(
const CAMState& cam_state, const CAMState& cam_state,
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
CameraCalibration& cam0, CameraCalibration& cam,
const Eigen::VectorXd& photo_r, const Eigen::VectorXd& photo_r,
std::stringstream& ss) const; std::stringstream& ss) const;
/* /*
@ -740,7 +740,7 @@ bool Feature::VisualizeKernel(
bool Feature::VisualizePatch( bool Feature::VisualizePatch(
const CAMState& cam_state, const CAMState& cam_state,
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
CameraCalibration& cam0, CameraCalibration& cam,
const Eigen::VectorXd& photo_r, const Eigen::VectorXd& photo_r,
std::stringstream& ss) const std::stringstream& ss) const
{ {
@ -749,7 +749,7 @@ bool Feature::VisualizePatch(
//visu - anchor //visu - anchor
auto anchor = observations.begin(); 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::Mat dottedFrame(anchorImage.size(), CV_8UC3);
cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB); cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
@ -761,10 +761,10 @@ bool Feature::VisualizePatch(
cv::Point ys(point.x, point.y); cv::Point ys(point.x, point.y);
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255)); cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255));
} }
cam0.featureVisu = dottedFrame.clone(); cam.featureVisu = dottedFrame.clone();
// visu - feature // 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); cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
// set position in frame // set position in frame
@ -772,7 +772,7 @@ bool Feature::VisualizePatch(
std::vector<double> projectionPatch; std::vector<double> projectionPatch;
for(auto point : anchorPatch_3d) 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)); projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image));
// visu - feature // visu - feature
cv::Point xs(p_in_c0.x, p_in_c0.y); 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::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 // 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)); cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
// move to real pixels // 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; i<N; i++) for(int i = 0; i<N; i++)
{ {
@ -853,17 +853,17 @@ bool Feature::VisualizePatch(
for(int i = 0; i<N; i++) for(int i = 0; i<N; i++)
for(int j = 0; j<N; j++) for(int j = 0; j<N; j++)
if(photo_r(i*N+j)>0) if(photo_r(2*(i*N+j))>0)
cv::rectangle(irradianceFrame, cv::rectangle(irradianceFrame,
cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), 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::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); CV_FILLED);
else else
cv::rectangle(irradianceFrame, cv::rectangle(irradianceFrame,
cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), 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::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); CV_FILLED);
// gradient arrow // gradient arrow
@ -884,7 +884,7 @@ bool Feature::VisualizePatch(
3); 3);
*/ */
cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); cv::hconcat(cam.featureVisu, irradianceFrame, cam.featureVisu);
/* /*
// visualize position of used observations and resulting feature position // 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 // 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 // write feature position
std::stringstream pos_s; std::stringstream pos_s;
pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1); 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); cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA);
// create line? // create line?
@ -932,9 +932,9 @@ bool Feature::VisualizePatch(
std::stringstream loc; std::stringstream loc;
// loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; // 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); cvWaitKey(1);
} }

10
launch/image_processor_tinytum.launch
View File

@ -14,8 +14,8 @@
<rosparam command="load" file="$(arg calibration_file)"/> <rosparam command="load" file="$(arg calibration_file)"/>
<param name="grid_row" value="4"/> <param name="grid_row" value="4"/>
<param name="grid_col" value="4"/> <param name="grid_col" value="4"/>
<param name="grid_min_feature_num" value="5"/> <param name="grid_min_feature_num" value="3"/>
<param name="grid_max_feature_num" value="10"/> <param name="grid_max_feature_num" value="4"/>
<param name="pyramid_levels" value="3"/> <param name="pyramid_levels" value="3"/>
<param name="patch_size" value="15"/> <param name="patch_size" value="15"/>
<param name="fast_threshold" value="10"/> <param name="fast_threshold" value="10"/>
@ -24,9 +24,9 @@
<param name="ransac_threshold" value="3"/> <param name="ransac_threshold" value="3"/>
<param name="stereo_threshold" value="5"/> <param name="stereo_threshold" value="5"/>
<remap from="~imu" to="/imu0"/> <remap from="~imu" to="/imu02"/>
<remap from="~cam0_image" to="/cam0/image_raw"/> <remap from="~cam0_image" to="/cam0/image_raw2"/>
<remap from="~cam1_image" to="/cam1/image_raw"/> <remap from="~cam1_image" to="/cam1/image_raw2"/>
</node> </node>
</group> </group>

2
src/msckf_vio.cpp
View File

@ -1828,7 +1828,7 @@ bool MsckfVio::PhotometricMeasurementJacobian(
// visualizing functions // visualizing functions
feature.MarkerGeneration(marker_pub, state_server.cam_states); 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); //feature.VisualizeKernel(cam_state, cam_state_id, cam0);
} }