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

added tum launch files, removed anchor procedure being called multiple times through a flag

Browse Source
This commit is contained in:
Raphael Maenle 2019-04-18 11:06:45 +02:00
parent cfecefe29f
commit d91ff7ca9d
6 changed files with 170 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
config/camchain-imucam-tum.yaml Normal file
View File

@ -0,0 +1,36 @@
cam0:
T_cam_imu:
[-0.9995378259923383, 0.02917807204183088, -0.008530798463872679, 0.047094247958417004,
0.007526588843243184, -0.03435493139706542, -0.9993813532126198, -0.04788273017221637,
-0.029453096117288798, -0.9989836729399656, 0.034119442089241274, -0.0697294754693238,
0.0, 0.0, 0.0, 1.0]
camera_model: pinhole
distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756,
-0.001662284667857643]
distortion_model: equidistant
intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506]
resolution: [1024, 1024]
rostopic: /cam0/image_raw
cam1:
T_cam_imu:
[-0.9995240747493029, 0.02986739485347808, -0.007717688852024281, -0.05374086123613335,
0.008095979457928231, 0.01256553460985914, -0.9998882749870535, -0.04648588412432889,
-0.02976708103202316, -0.9994748851595197, -0.0128013601698453, -0.07333210787623645,
0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
[0.9999994317488622, -0.0008361847221513937, -0.0006612844045898121, -0.10092123225528335,
0.0008042457277382264, 0.9988989443471681, -0.04690684567228134, -0.001964540595211977,
0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572,
0.0, 0.0, 0.0, 1.0]
camera_model: pinhole
distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326,
-0.002347858896562788]
distortion_model: equidistant
intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407]
resolution: [1024, 1024]
rostopic: /cam1/image_raw
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]

23
include/msckf_vio/feature.hpp
View File

@ -59,11 +59,11 @@ struct Feature {
// Constructors for the struct.
Feature(): id(0), position(Eigen::Vector3d::Zero()),
is_initialized(false) {}
is_initialized(false), is_anchored(false) {}
Feature(const FeatureIDType& new_id): id(new_id),
position(Eigen::Vector3d::Zero()),
is_initialized(false) {}
is_initialized(false), is_anchored(false) {}
/*
* @brief cost Compute the cost of the camera observations
@ -217,7 +217,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
// A indicator to show if the 3d postion of the feature
// has been initialized or not.
bool is_initialized;
bool is_anchored;
// Noise for a normalized feature measurement.
static double observation_noise;
@ -380,6 +380,8 @@ bool Feature::estimate_FrameIrradiance(
double b_A = 0;
double a_l =frameExposureTime_ms;
double b_l = 0;
printf("frames: %lld, %lld\n", anchor->first, cam_state_id);
printf("exposure: %f, %f\n", a_A, a_l);
for (double anchorPixel : anchorPatch)
{
float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l;
@ -396,6 +398,8 @@ bool Feature::FrameIrradiance(
std::vector<float>& anchorPatch_measurement) const
{
//project every point in anchorPatch_3d.
if(!is_anchored)
printf("not anchored!\n");
for (auto point : anchorPatch_3d)
{
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
@ -471,12 +475,19 @@ bool Feature::initializeAnchor(
cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second.image;
auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2];
auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3];
printf("initializing anchor\n");
if(u - n < 0 || u + n >= cam.resolution(0) || v - n < 0 || v + n >= cam.resolution(1))
{
printf("no good: \n");
printf("%f, %f\n", u, v);
return false;
}
//for NxN patch pixels around feature
for(double u_run = u - n; u_run <= u + n; u_run = u_run + 1)
{
for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1)
{
printf("ADDING\n");
// add irradiance information
anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)u_run,(int)v_run));
@ -489,8 +500,8 @@ bool Feature::initializeAnchor(
anchorPatch_3d.push_back(Npose);
}
}
//TODO test if NxN patch can be selected
printf("set to true!!!\n");
is_anchored = true;
return true;
}

34
launch/image_processor_tum.launch Normal file
View File

@ -0,0 +1,34 @@
<launch>
<arg name="robot" default="firefly_sbx"/>
<arg name="calibration_file"
default="$(find msckf_vio)/config/camchain-imucam-tum.yaml"/>
<!-- Image Processor Nodelet -->
<group ns="$(arg robot)">
<node pkg="nodelet" type="nodelet" name="image_processor"
args="standalone msckf_vio/ImageProcessorNodelet"
output="screen"
>
<rosparam command="load" file="$(arg calibration_file)"/>
<param name="grid_row" value="4"/>
<param name="grid_col" value="4"/>
<param name="grid_min_feature_num" value="3"/>
<param name="grid_max_feature_num" value="4"/>
<param name="pyramid_levels" value="3"/>
<param name="patch_size" value="15"/>
<param name="fast_threshold" value="10"/>
<param name="max_iteration" value="30"/>
<param name="track_precision" value="0.01"/>
<param name="ransac_threshold" value="3"/>
<param name="stereo_threshold" value="5"/>
<remap from="~imu" to="/imu0"/>
<remap from="~cam0_image" to="/cam0/image_raw"/>
<remap from="~cam1_image" to="/cam1/image_raw"/>
</node>
</group>
</launch>

64
launch/msckf_vio_tum.launch Normal file
View File

@ -0,0 +1,64 @@
<launch>
<arg name="robot" default="firefly_sbx"/>
<arg name="fixed_frame_id" default="world"/>
<arg name="calibration_file"
default="$(find msckf_vio)/config/camchain-imucam-tum.yaml"/>
<!-- Image Processor Nodelet -->
<include file="$(find msckf_vio)/launch/image_processor_tum.launch">
<arg name="robot" value="$(arg robot)"/>
<arg name="calibration_file" value="$(arg calibration_file)"/>
</include>
<!-- Msckf Vio Nodelet -->
<group ns="$(arg robot)">
<node pkg="nodelet" type="nodelet" name="vio"
args='standalone msckf_vio/MsckfVioNodelet'
output="screen">
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>
<param name="publish_tf" value="true"/>
<param name="frame_rate" value="20"/>
<param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
<param name="child_frame_id" value="odom"/>
<param name="max_cam_state_size" value="20"/>
<param name="position_std_threshold" value="8.0"/>
<param name="rotation_threshold" value="0.2618"/>
<param name="translation_threshold" value="0.4"/>
<param name="tracking_rate_threshold" value="0.5"/>
<!-- Feature optimization config -->
<param name="feature/config/translation_threshold" value="-1.0"/>
<!-- These values should be standard deviation -->
<param name="noise/gyro" value="0.005"/>
<param name="noise/acc" value="0.05"/>
<param name="noise/gyro_bias" value="0.001"/>
<param name="noise/acc_bias" value="0.01"/>
<param name="noise/feature" value="0.035"/>
<param name="initial_state/velocity/x" value="0.0"/>
<param name="initial_state/velocity/y" value="0.0"/>
<param name="initial_state/velocity/z" value="0.0"/>
<!-- These values should be covariance -->
<param name="initial_covariance/velocity" value="0.25"/>
<param name="initial_covariance/gyro_bias" value="0.01"/>
<param name="initial_covariance/acc_bias" value="0.01"/>
<param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
<param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
<remap from="~imu" to="/imu0"/>
<remap from="~cam0_image" to="/cam0/image_raw"/>
<remap from="~cam1_image" to="/cam1/image_raw"/>
<remap from="~features" to="image_processor/features"/>
</node>
</group>
</launch>

79
src/image_processor.cpp
View File

@ -390,7 +390,6 @@ void ImageProcessor::predictFeatureTracking(
const cv::Matx33f& R_p_c,
const cv::Vec4d& intrinsics,
vector<cv::Point2f>& compensated_pts) {
// Return directly if there are no input features.
if (input_pts.size() == 0) {
compensated_pts.clear();
@ -421,7 +420,6 @@ void ImageProcessor::trackFeatures() {
cam0_curr_img_ptr->image.rows / processor_config.grid_row;
static int grid_width =
cam0_curr_img_ptr->image.cols / processor_config.grid_col;
// Compute a rough relative rotation which takes a vector
// from the previous frame to the current frame.
Matx33f cam0_R_p_c;
@ -611,7 +609,6 @@ void ImageProcessor::stereoMatch(
const vector<cv::Point2f>& cam0_points,
vector<cv::Point2f>& cam1_points,
vector<unsigned char>& inlier_markers) {
if (cam0_points.size() == 0) return;
if(cam1_points.size() == 0) {
@ -700,8 +697,8 @@ void ImageProcessor::addNewFeatures() {
cam0_curr_img_ptr->image.rows / processor_config.grid_row;
static int grid_width =
cam0_curr_img_ptr->image.cols / processor_config.grid_col;
// Create a mask to avoid redetecting existing features.
Mat mask(curr_img.rows, curr_img.cols, CV_8U, Scalar(1));
for (const auto& features : *curr_features_ptr) {
@ -721,7 +718,6 @@ void ImageProcessor::addNewFeatures() {
mask(row_range, col_range) = 0;
}
}
// Detect new features.
vector<KeyPoint> new_features(0);
detector_ptr->detect(curr_img, new_features, mask);
@ -736,7 +732,6 @@ void ImageProcessor::addNewFeatures() {
new_feature_sieve[
row*processor_config.grid_col+col].push_back(feature);
}
new_features.clear();
for (auto& item : new_feature_sieve) {
if (item.size() > processor_config.grid_max_feature_num) {
@ -749,7 +744,6 @@ void ImageProcessor::addNewFeatures() {
}
int detected_new_features = new_features.size();
// Find the stereo matched points for the newly
// detected features.
vector<cv::Point2f> cam0_points(new_features.size());
@ -777,7 +771,6 @@ void ImageProcessor::addNewFeatures() {
static_cast<double>(detected_new_features) < 0.1)
ROS_WARN("Images at [%f] seems unsynced...",
cam0_curr_img_ptr->header.stamp.toSec());
// Group the features into grids
GridFeatures grid_new_features;
for (int code = 0; code <
@ -799,7 +792,6 @@ void ImageProcessor::addNewFeatures() {
new_feature.cam1_point = cam1_point;
grid_new_features[code].push_back(new_feature);
}
// Sort the new features in each grid based on its response.
for (auto& item : grid_new_features)
std::sort(item.second.begin(), item.second.end(),
@ -849,73 +841,6 @@ void ImageProcessor::pruneGridFeatures() {
return;
}
void ImageProcessor::undistortPoints(
const vector<cv::Point2f>& pts_in,
const cv::Vec4d& intrinsics,
const string& distortion_model,
const cv::Vec4d& distortion_coeffs,
vector<cv::Point2f>& pts_out,
const cv::Matx33d &rectification_matrix,
const cv::Vec4d &new_intrinsics) {
if (pts_in.size() == 0) return;
const cv::Matx33d K(
intrinsics[0], 0.0, intrinsics[2],
0.0, intrinsics[1], intrinsics[3],
0.0, 0.0, 1.0);
const cv::Matx33d K_new(
new_intrinsics[0], 0.0, new_intrinsics[2],
0.0, new_intrinsics[1], new_intrinsics[3],
0.0, 0.0, 1.0);
if (distortion_model == "radtan") {
cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
rectification_matrix, K_new);
} else if (distortion_model == "equidistant") {
cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
rectification_matrix, K_new);
} else {
ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
distortion_model.c_str());
cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
rectification_matrix, K_new);
}
return;
}
vector<cv::Point2f> ImageProcessor::distortPoints(
const vector<cv::Point2f>& pts_in,
const cv::Vec4d& intrinsics,
const string& distortion_model,
const cv::Vec4d& distortion_coeffs) {
const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2],
0.0, intrinsics[1], intrinsics[3],
0.0, 0.0, 1.0);
vector<cv::Point2f> pts_out;
if (distortion_model == "radtan") {
vector<cv::Point3f> homogenous_pts;
cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
distortion_coeffs, pts_out);
} else if (distortion_model == "equidistant") {
cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
} else {
ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
distortion_model.c_str());
vector<cv::Point3f> homogenous_pts;
cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
distortion_coeffs, pts_out);
}
return pts_out;
}
void ImageProcessor::integrateImuData(
Matx33f& cam0_R_p_c, Matx33f& cam1_R_p_c) {
// Find the start and the end limit within the imu msg buffer.
@ -967,7 +892,6 @@ void ImageProcessor::integrateImuData(
void ImageProcessor::rescalePoints(
vector<Point2f>& pts1, vector<Point2f>& pts2,
float& scaling_factor) {
scaling_factor = 0.0f;
for (int i = 0; i < pts1.size(); ++i) {
@ -1232,7 +1156,6 @@ void ImageProcessor::twoPointRansac(
}
void ImageProcessor::publish() {
// Publish features.
CameraMeasurementPtr feature_msg_ptr(new CameraMeasurement);
feature_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;

34
src/msckf_vio.cpp
View File

@ -306,7 +306,6 @@ 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;
@ -344,7 +343,7 @@ void MsckfVio::imageCallback(
// Add new images to moving window
start_time = ros::Time::now();
//manageMovingWindow(cam0_img, cam1_img, feature_msg);
manageMovingWindow(cam0_img, cam1_img, feature_msg);
double manage_moving_window_time = (
ros::Time::now()-start_time).toSec();
@ -398,16 +397,16 @@ void MsckfVio::manageMovingWindow(
const CameraMeasurementConstPtr& feature_msg) {
//save exposure Time into moving window
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000;
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000;
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000000;
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000000;
if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms < 1)
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 1;
if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms < 1)
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 1;
if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 500)
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 500;
if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 500)
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 500;
if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 100)
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 100;
if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 100)
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 100;
// Get the current image.
cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img,
@ -981,6 +980,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
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));
printf("-----\n");
//estimate photometric measurement
std::vector<float> estimate_photo_z;
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z);
@ -990,9 +991,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
for(int i = 0; i < photo_z.size(); i++)
photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
// printf("-----\n");
// for(int i = 0; i < photo_z.size(); i++)
// printf("%.4f - %.4f\n", photo_z[i], estimate_photo_z[i]);
for(int i = 0; i < photo_z.size(); i++)
printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
photo_z.clear();
return;
@ -1341,6 +1342,9 @@ void MsckfVio::removeLostFeatures() {
continue;
}
}
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
invalid_feature_ids.push_back(feature.id);
@ -1477,7 +1481,6 @@ void MsckfVio::pruneCamStateBuffer() {
feature.observations.erase(involved_cam_state_ids[0]);
continue;
}
if (!feature.is_initialized) {
// Check if the feature can be initialize.
if (!feature.checkMotion(state_server.cam_states)) {
@ -1494,6 +1497,9 @@ void MsckfVio::pruneCamStateBuffer() {
continue;
}
}
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
for (const auto& cam_id : involved_cam_state_ids)
@ -1501,7 +1507,6 @@ void MsckfVio::pruneCamStateBuffer() {
continue;
}
}
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
}
@ -1512,7 +1517,6 @@ void MsckfVio::pruneCamStateBuffer() {
21+6*state_server.cam_states.size());
VectorXd r = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
ros::Time start_time = ros::Time::now();
for (auto& item : map_server) {
auto& feature = item.second;
// Check how many camera states to be removed are associated
@ -1539,8 +1543,6 @@ void MsckfVio::pruneCamStateBuffer() {
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
}
double anchorPrune_processing_time = (ros::Time::now()-start_time).toSec();
printf("FeatureJacobian Time: %f\n", anchorPrune_processing_time);
H_x.conservativeResize(stack_cntr, H_x.cols());
r.conservativeResize(stack_cntr);