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

not working, added stop and go to image processing, added undistorted image to image processing

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-26 19:23:50 +02:00
parent 273bbf8a94
commit ebc73c0c5e
11 changed files with 260 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
src/image_handler.cpp
View File

@ -14,6 +14,16 @@ namespace msckf_vio {
namespace image_handler {
cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics)
{
return cv::Point2f(p_in.x * intrinsics[0] + intrinsics[2], p_in.y * intrinsics[1] + intrinsics[3]);
}
cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics)
{
return cv::Point2f((p_in.x - intrinsics[2])/intrinsics[0], (p_in.y - intrinsics[3])/intrinsics[1]);
}
void undistortPoint(
const cv::Point2f& pt_in,
const cv::Vec4d& intrinsics,
@ -42,10 +52,13 @@ void undistortPoint(
if (distortion_model == "radtan") {
cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
rectification_matrix, K_new);
} else if (distortion_model == "equidistant") {
}
// equidistant
else if (distortion_model == "equidistant") {
cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
rectification_matrix, K_new);
}
// fov
else if (distortion_model == "fov") {
for(int i = 0; i < pts_in.size(); i++)
{
@ -59,7 +72,15 @@ void undistortPoint(
pts_out.push_back(newPoint);
}
}
}
else if (distortion_model == "pre-equidistant")
{
std::vector<cv::Point2f> temp_pts_out;
for(int i = 0; i < pts_in.size(); i++)
temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics));
pts_out = temp_pts_out;
}
else {
ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
distortion_model.c_str());
@ -112,7 +133,16 @@ void undistortPoints(
pts_out.push_back(newPoint);
}
}
}
else if (distortion_model == "pre-equidistant")
{
std::vector<cv::Point2f> temp_pts_out;
for(int i = 0; i < pts_in.size(); i++)
temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics));
pts_out = temp_pts_out;
}
else {
ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
distortion_model.c_str());
@ -156,7 +186,15 @@ std::vector<cv::Point2f> distortPoints(
pts_out.push_back(newPoint);
}
}
}
else if (distortion_model == "pre-equidistant")
{
std::vector<cv::Point2f> temp_pts_out;
for(int i = 0; i < pts_in.size(); i++)
temp_pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics));
pts_out = temp_pts_out;
}
else {
ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
distortion_model.c_str());
@ -205,7 +243,15 @@ cv::Point2f distortPoint(
pts_out.push_back(newPoint);
}
}
}
else if (distortion_model == "pre-equidistant")
{
std::vector<cv::Point2f> temp_pts_out;
for(int i = 0; i < pts_in.size(); i++)
pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics));
pts_out = temp_pts_out;
}
else {
ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
distortion_model.c_str());

31
src/image_processor.cpp
View File

@ -42,6 +42,9 @@ ImageProcessor::~ImageProcessor() {
}
bool ImageProcessor::loadParameters() {
// debug parameters
nh.param<bool>("StreamPause", STREAMPAUSE, false);
// Camera calibration parameters
nh.param<string>("cam0/distortion_model",
cam0.distortion_model, string("radtan"));
@ -211,7 +214,9 @@ void ImageProcessor::stereoCallback(
const sensor_msgs::ImageConstPtr& cam0_img,
const sensor_msgs::ImageConstPtr& cam1_img) {
//cout << "==================================" << endl;
// stop playing bagfile if printing images
if(STREAMPAUSE)
nh.setParam("/play_bag_image", false);
// Get the current image.
cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
@ -219,6 +224,18 @@ void ImageProcessor::stereoCallback(
cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
sensor_msgs::image_encodings::MONO8);
const cv::Matx33d K0(cam0.intrinsics[0], 0.0, cam0.intrinsics[2],
0.0, cam0.intrinsics[1], cam0.intrinsics[3],
0.0, 0.0, 1.0);
const cv::Matx33d K1(cam1.intrinsics[0], 0.0, cam1.intrinsics[2],
0.0, cam1.intrinsics[1], cam1.intrinsics[3],
0.0, 0.0, 1.0);
cv::fisheye::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, K0, cam0.distortion_coeffs, K0);
cv::fisheye::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, K1, cam1.distortion_coeffs, K1);
// Build the image pyramids once since they're used at multiple places
createImagePyramids();
@ -245,6 +262,7 @@ void ImageProcessor::stereoCallback(
// Add new features into the current image.
start_time = ros::Time::now();
addNewFeatures();
//ROS_INFO("Addition time: %f",
// (ros::Time::now()-start_time).toSec());
@ -267,16 +285,19 @@ void ImageProcessor::stereoCallback(
// (ros::Time::now()-start_time).toSec());
// Publish features in the current image.
ros::Time start_time = ros::Time::now();
publish();
//ROS_INFO("Publishing: %f",
// (ros::Time::now()-start_time).toSec());
// Update the previous image and previous features.
cam0_prev_img_ptr = cam0_curr_img_ptr;
prev_features_ptr = curr_features_ptr;
std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);
// Initialize the current features to empty vectors.
curr_features_ptr.reset(new GridFeatures());
for (int code = 0; code <
@ -284,6 +305,10 @@ void ImageProcessor::stereoCallback(
(*curr_features_ptr)[code] = vector<FeatureMetaData>(0);
}
// stop playing bagfile if printing images
if(STREAMPAUSE)
nh.setParam("/play_bag_image", true);
return;
}
@ -580,6 +605,7 @@ void ImageProcessor::trackFeatures() {
++after_ransac;
}
// Compute the tracking rate.
int prev_feature_num = 0;
for (const auto& item : *prev_features_ptr)
@ -659,6 +685,8 @@ void ImageProcessor::stereoMatch(
// Further remove outliers based on the known
// essential matrix.
vector<cv::Point2f> cam0_points_undistorted(0);
vector<cv::Point2f> cam1_points_undistorted(0);
image_handler::undistortPoints(
@ -668,6 +696,7 @@ void ImageProcessor::stereoMatch(
cam1_points, cam1.intrinsics, cam1.distortion_model,
cam1.distortion_coeffs, cam1_points_undistorted);
double norm_pixel_unit = 4.0 / (
cam0.intrinsics[0]+cam0.intrinsics[1]+
cam1.intrinsics[0]+cam1.intrinsics[1]);

58
src/msckf_vio.cpp
View File

@ -450,7 +450,7 @@ void MsckfVio::imageCallback(
double imu_processing_time = (
ros::Time::now()-start_time).toSec();
// cout << "1" << endl;
//cout << "1" << endl;
// Augment the state vector.
start_time = ros::Time::now();
//truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
@ -459,7 +459,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 +468,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 +476,20 @@ 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;
//cout << "6" << endl;
// Publish the odometry.
start_time = ros::Time::now();
publish(feature_msg->header.stamp);
@ -552,6 +552,20 @@ void MsckfVio::manageMovingWindow(
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();
// undistort Images
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::fisheye::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, K, cam0.distortion_coeffs, K);
//cv::imshow("anchor", undistortedCam0);
//cvWaitKey(0);
cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone();
cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone();
//TODO handle any massive overflow correctly (should be pruned, before this ever triggers)
while(cam0.moving_window.size() > 100)
{
@ -1228,7 +1242,7 @@ void MsckfVio::addFeatureObservations(
return;
}
void MsckfVio::PhotometricMeasurementJacobian(
bool MsckfVio::PhotometricMeasurementJacobian(
const StateIDType& cam_state_id,
const FeatureIDType& feature_id,
MatrixXd& H_x, MatrixXd& H_y, VectorXd& r)
@ -1305,21 +1319,25 @@ void MsckfVio::PhotometricMeasurementJacobian(
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);
cv::Point2f p_in_c0 = feature.projectPositionToCameraUndistorted(cam_state, cam_state_id, cam0, point);
cv::Point2f p_in_anchor = feature.projectPositionToCameraUndistorted(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) )
{
cout << "skip" << endl;
return false;
}
//add observation
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
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");
@ -1329,6 +1347,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
dI_dhj(0, 0) = dx * cam0.intrinsics[0];
dI_dhj(0, 1) = dy * cam0.intrinsics[1];
//dh / d{}^Cp_{ij}
dh_dCpij(0, 0) = 1 / p_c0(2);
dh_dCpij(1, 1) = 1 / p_c0(2);
@ -1369,6 +1388,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
count++;
}
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);
@ -1419,7 +1439,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
//feature.VisualizeKernel(cam_state, cam_state_id, cam0);
}
return;
return true;
}
void MsckfVio::PhotometricFeatureJacobian(
@ -1460,7 +1480,8 @@ void MsckfVio::PhotometricFeatureJacobian(
MatrixXd H_yl;
Eigen::VectorXd r_l = VectorXd::Zero(N*N);
PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l))
continue;
auto cam_state_iter = state_server.cam_states.find(cam_id);
int cam_state_cntr = std::distance(
@ -1475,6 +1496,9 @@ void MsckfVio::PhotometricFeatureJacobian(
}
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.
@ -1978,7 +2002,7 @@ void MsckfVio::removeLostFeatures() {
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0, N))
if(!feature.initializeAnchorUndistort(cam0, N))
{
invalid_feature_ids.push_back(feature.id);
continue;
@ -2144,7 +2168,7 @@ void MsckfVio::pruneLastCamStateBuffer()
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0, N))
if(!feature.initializeAnchorUndistort(cam0, N))
{
feature.observations.erase(rm_cam_state_id);
continue;
@ -2296,7 +2320,7 @@ void MsckfVio::pruneCamStateBuffer() {
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0, N))
if(!feature.initializeAnchorUndistort(cam0, N))
{
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);