Skip to content

Commit

Permalink
initially working ros node
Browse files Browse the repository at this point in the history
  • Loading branch information
@k-chaney
k-chaney committed Jul 11, 2018
1 parent 1681ee7 commit cf1ee71
Show file tree
Hide file tree
Showing 7 changed files with 88 additions and 58 deletions.
4 changes: 4 additions & 0 deletions datasets/asl_msckf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,12 @@ int main(int argc, char** argv)
camera.c_v = K.at<float>(1,2);

camera.q_CI = cam0->get_q_BS();
const auto q_CI = camera.q_CI;
camera.p_C_I = cam0->get_p_BS();

ROS_INFO_STREAM("Camera\n- q_CI " << q_CI.x() << "," << q_CI.y() << "," << q_CI.z() << "," << q_CI.w() << "\n" <<
"- p_C_I " << camera.p_C_I.transpose());

float feature_cov;
nh.param<float>("feature_covariance", feature_cov, 7);

Expand Down
16 changes: 8 additions & 8 deletions datasets/asl_msckf_no_ground_truth.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,14 +178,14 @@ int main(int argc, char** argv)
auto q = imu_state.q_IG;

ROS_INFO_STREAM("\nInitial IMU State" <<
"\n---p_I_G " << imu_state.p_I_G.transpose() <<
"\n---q_IG " << q.w() << "," << q.x() << "," << q.y() << "," << q.x() <<
"\n---v_I_G " << imu_state.v_I_G.transpose() <<
"\n---b_a " << imu_state.b_a.transpose() <<
"\n---b_g " << imu_state.b_g.transpose() <<
"\n---a " << imu_data.a.transpose()<<
"\n---g " << imu_state.g.transpose()<<
"\n---world_adjusted_a " << (q.toRotationMatrix().transpose()*(imu_data.a-imu_state.b_a)).transpose());
"\n--p_I_G " << imu_state.p_I_G.transpose() <<
"\n--q_IG " << q.w() << "," << q.x() << "," << q.y() << "," << q.x() <<
"\n--v_I_G " << imu_state.v_I_G.transpose() <<
"\n--b_a " << imu_state.b_a.transpose() <<
"\n--b_g " << imu_state.b_g.transpose() <<
"\n--a " << imu_data.a.transpose()<<
"\n--g " << imu_state.g.transpose()<<
"\n--world_adjusted_a " << (q.toRotationMatrix().transpose()*(imu_data.a-imu_state.b_a)).transpose());

ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 100);
ros::Publisher map_pub = nh.advertise<sensor_msgs::PointCloud2>("map", 100);
Expand Down
1 change: 1 addition & 0 deletions include/msckf_mono/corner_detector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,6 +155,7 @@ class TrackHandler

Eigen::Vector3f gyro_accum_;
size_t n_gyro_readings_;
bool use_gyro_;

const std::string distortion_model_;
const cv::Mat distortion_coeffs_;
Expand Down
8 changes: 6 additions & 2 deletions include/msckf_mono/ros_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,8 @@ namespace msckf_mono
{
class RosInterface {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

RosInterface(ros::NodeHandle nh);

void imuCallback(const sensor_msgs::ImuConstPtr& imu);
Expand Down Expand Up @@ -49,10 +51,12 @@ namespace msckf_mono
void setup_track_handler();
std::shared_ptr<corner_detector::TrackHandler> track_handler_;

Matrix4<float> T_imu_cam_;
Matrix3<float> R_imu_cam_;
Vector3<float> p_imu_cam_;

Matrix3<float> R_cam_imu_;
Vector3<float> p_cam_imu_;

std::string camera_model_;
cv::Mat K_;
std::string distortion_model_;
Expand All @@ -71,7 +75,7 @@ namespace msckf_mono

int state_k_;
void setup_msckf();
std::shared_ptr<MSCKF<float>> msckf_;
MSCKF<float> msckf_;
Camera<float> camera_;
noiseParams<float> noise_params_;
Eigen::Matrix<float,12,1> Q_imu_vars_;
Expand Down
4 changes: 2 additions & 2 deletions launch/asl_msckf.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,8 @@
<group unless="$(arg profile)">
<node pkg="msckf_mono" name="asl_msckf" type="asl_msckf_no_ground_truth" unless="$(arg gdb_debug)" output="screen"/>
<node pkg="msckf_mono" name="asl_msckf" type="asl_msckf_no_ground_truth" launch-prefix="xterm -e gdb --args" if="$(arg gdb_debug)"/>
</group>
<group if="$(arg profile)">
</group>
<group if="$(arg profile)">
<node pkg="msckf_mono" name="asl_msckf" type="asl_msckf_no_ground_truth" launch-prefix="perf record -g --call-graph dwarf --output=perf.out.node_name.data --" if="$(arg profile)"/>
</group>

Expand Down
40 changes: 23 additions & 17 deletions src/corner_detector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -198,7 +198,7 @@ TrackHandler::TrackHandler(const cv::Mat K,
: ransac_threshold_(0.0000002), next_feature_id_(0),
gyro_accum_(Eigen::Vector3f::Zero()), n_gyro_readings_(0),
K_(K), K_inv_(K.inv()), distortion_coeffs_(distortion_coeffs),
distortion_model_(dist_model)
distortion_model_(dist_model), use_gyro_(true)
{
dR_ = cv::Mat::eye(3,3,CV_32F);
clear_tracks();
Expand All @@ -223,7 +223,8 @@ void TrackHandler::integrate_gyro() {
if(n_gyro_readings_>0){
gyro_accum_ /= static_cast<float>(n_gyro_readings_);
}else{
gyro_accum_.setZero();
use_gyro_ = false;
return;
}
gyro_accum_ *= dt;

Expand All @@ -247,25 +248,30 @@ void TrackHandler::predict_features(){
prev_feature_ids_.end(),
std::back_inserter(cur_feature_ids_));

integrate_gyro();
if (use_gyro_){
integrate_gyro();

// homography by rotation
cv::Mat H = K_ * dR_ * K_inv_;
cv::Mat pt_buf1(3,1,CV_32F);
pt_buf1.at<float>(2) = 1.0;
// homography by rotation
cv::Mat H = K_ * dR_ * K_inv_;
cv::Mat pt_buf1(3,1,CV_32F);
pt_buf1.at<float>(2) = 1.0;

cv::Mat pt_buf2(3,1,CV_32F);
cv::Mat pt_buf2(3,1,CV_32F);

for(auto& pt : prev_features_){
pt_buf1.at<float>(0) = pt.x;
pt_buf1.at<float>(1) = pt.y;
for(auto& pt : prev_features_){
pt_buf1.at<float>(0) = pt.x;
pt_buf1.at<float>(1) = pt.y;

pt_buf2 = H * pt_buf1;
pt_buf2 = H * pt_buf1;

Point2f new_point;
new_point.x = pt_buf2.at<float>(0) / pt_buf2.at<float>(2);
new_point.y = pt_buf2.at<float>(1) / pt_buf2.at<float>(2);
cur_features_.push_back(new_point);
const float x = pt_buf2.at<float>(0) / pt_buf2.at<float>(2);
const float y = pt_buf2.at<float>(1) / pt_buf2.at<float>(2);
cur_features_.emplace_back(x,y);
}
}else{
std::copy(prev_features_.begin(),
prev_features_.end(),
std::back_inserter(cur_features_));
}
}

Expand Down Expand Up @@ -357,7 +363,7 @@ void TrackHandler::tracked_features(OutFeatureVector& features, IdVector& featur
for(int j=0; j<3; j++)
dR(i,j) = dR_.at<float>(i,j);

if(cur_features.size() > 5){
if(cur_features.size() > 5 && false){
auto valid_pts = twoPointRansac(dR, prev_features, cur_features);

auto ocf_it = cur_features.begin();
Expand Down
73 changes: 44 additions & 29 deletions src/ros_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,13 @@ namespace msckf_mono
load_parameters();
setup_track_handler();

image_sub_ = it_.subscribe("/rig/left/image_mono", 1,
image_sub_ = it_.subscribe("/cam0/image_raw", 1,
&RosInterface::imageCallback, this);

track_image_pub_ = it_.advertise("/rig/left/image_mono/tracks", 1);
track_image_pub_ = it_.advertise("/cam0/image_raw/tracks", 1);

imu_sub_ = nh_.subscribe("/rig/imu", 10, &RosInterface::imuCallback, this);
odom_pub_ = nh.advertise<nav_msgs::Odometry>("/rig/msckf_mono/odom", 100);
imu_sub_ = nh_.subscribe("/imu0", 10, &RosInterface::imuCallback, this);
odom_pub_ = nh.advertise<nav_msgs::Odometry>("/msckf_mono/odom", 100);
}

void RosInterface::imuCallback(const sensor_msgs::ImuConstPtr& imu)
Expand Down Expand Up @@ -88,10 +88,10 @@ namespace msckf_mono
[](auto& x){return std::get<1>(x);});

for(auto& reading : imu_since_prev_img){
msckf_->propagate(reading);
msckf_.propagate(reading);

Vector3<float> gyro_measurement = R_imu_cam_ * reading.omega;
// track_handler_->add_gyro_reading(gyro_measurement);
Vector3<float> gyro_measurement = R_imu_cam_ * (reading.omega - init_imu_state_.b_g);
track_handler_->add_gyro_reading(gyro_measurement);
}

imu_queue_.erase(imu_queue_.begin(), frame_end);
Expand All @@ -108,20 +108,20 @@ namespace msckf_mono
corner_detector::IdVector new_ids;
track_handler_->new_features(new_features, new_ids);

msckf_->augmentState(state_k_, (float)cur_image_time);
msckf_->update(cur_features, cur_ids);
msckf_->addFeatures(new_features, new_ids);
msckf_->marginalize();
msckf_->pruneRedundantStates();
msckf_->pruneEmptyStates();
msckf_.augmentState(state_k_, (float)cur_image_time);
msckf_.update(cur_features, cur_ids);
msckf_.addFeatures(new_features, new_ids);
msckf_.marginalize();
msckf_.pruneRedundantStates();
msckf_.pruneEmptyStates();

publish_core(msg->header.stamp);
publish_extra(msg->header.stamp);
}

void RosInterface::publish_core(const ros::Time& publish_time)
{
auto imu_state = msckf_->getImuState();
auto imu_state = msckf_.getImuState();

nav_msgs::Odometry odom;
odom.header.stamp = publish_time;
Expand Down Expand Up @@ -157,7 +157,7 @@ namespace msckf_mono
bool RosInterface::can_initialize_imu()
{
if(imu_calibration_method_ == TimedStandStill){
return imu_queue_.size() > 2600;
return imu_queue_.size() > 1600;
}

return false;
Expand Down Expand Up @@ -193,12 +193,16 @@ namespace msckf_mono

init_imu_state_.p_I_G.setZero();
init_imu_state_.v_I_G.setZero();
const auto q = init_imu_state_.q_IG;

ROS_INFO_STREAM("\nInitial IMU State" <<
"\n--p_I_G " << init_imu_state_.p_I_G.transpose() <<
"\n--q_IG " << q.w() << "," << q.x() << "," << q.y() << "," << q.x() <<
"\n--v_I_G " << init_imu_state_.v_I_G.transpose() <<
"\n--b_a " << init_imu_state_.b_a.transpose() <<
"\n--b_g " << init_imu_state_.b_g.transpose() <<
"\n--g " << init_imu_state_.g.transpose());

ROS_INFO_STREAM("IMU Biases Calibrated");
ROS_INFO_STREAM("b_g - " << init_imu_state_.b_g.transpose());
ROS_INFO_STREAM("b_a - " << init_imu_state_.b_a.transpose());
const auto q_IG = init_imu_state_.q_IG;
ROS_INFO_STREAM("q_IG - " << q_IG.x() << "," << q_IG.y() << "," << q_IG.z() << "," << q_IG.w());
}

void RosInterface::setup_track_handler()
Expand All @@ -210,9 +214,8 @@ namespace msckf_mono

void RosInterface::setup_msckf()
{
msckf_.reset(new MSCKF<float>() );
state_k_ = 0;
msckf_->initialize(camera_, noise_params_, msckf_params_, init_imu_state_);
msckf_.initialize(camera_, noise_params_, msckf_params_, init_imu_state_);
}

void RosInterface::load_parameters()
Expand Down Expand Up @@ -247,18 +250,22 @@ namespace msckf_mono
XmlRpc::XmlRpcValue ros_param_list;
nh_.getParam(kalibr_camera+"/T_cam_imu", ros_param_list);
ROS_ASSERT(ros_param_list.getType() == XmlRpc::XmlRpcValue::TypeArray);


Matrix4<float> T_imu_cam;
for (int32_t i = 0; i < ros_param_list.size(); ++i)
{
ROS_ASSERT(ros_param_list[i].getType() == XmlRpc::XmlRpcValue::TypeArray);
for(int32_t j=0; j<ros_param_list[i].size(); ++j){
ROS_ASSERT(ros_param_list[i][j].getType() == XmlRpc::XmlRpcValue::TypeDouble);
T_imu_cam_(i,j) = static_cast<double>(ros_param_list[i][j]);
T_imu_cam(i,j) = static_cast<double>(ros_param_list[i][j]);
}
}

R_imu_cam_ = T_imu_cam_.block<3,3>(0,0);
p_imu_cam_ = T_imu_cam_.block<3,1>(0,3);
R_imu_cam_ = T_imu_cam.block<3,3>(0,0);
p_imu_cam_ = T_imu_cam.block<3,1>(0,3);

R_cam_imu_ = R_imu_cam_.transpose();
p_cam_imu_ = R_cam_imu_ * (-1. * p_imu_cam_);

nh_.param<int>("n_grid_rows", n_grid_rows_, 8);
nh_.param<int>("n_grid_cols", n_grid_cols_, 8);
Expand All @@ -272,8 +279,8 @@ namespace msckf_mono
camera_.f_u = intrinsics[2];
camera_.f_v = intrinsics[3];

camera_.q_CI = Quaternion<float>(R_imu_cam_);
camera_.p_C_I = p_imu_cam_;
camera_.q_CI = Quaternion<float>(R_cam_imu_).inverse(); // compensates for expected form
camera_.p_C_I = p_cam_imu_;

float feature_cov;
nh_.param<float>("feature_covariance", feature_cov, 7);
Expand Down Expand Up @@ -328,7 +335,15 @@ namespace msckf_mono
<< distortion_coeffs[2] << ", "
<< distortion_coeffs[3] );
ROS_INFO_STREAM("-Camera topic " << subscribe_topic_);
ROS_INFO_STREAM("-T_cam_imu \n" << T_imu_cam_);
ROS_INFO_STREAM("-T_imu_cam \n" << T_imu_cam);
ROS_INFO_STREAM("-R_imu_cam \n" << R_imu_cam_);
ROS_INFO_STREAM("-p_imu_cam \n" << p_imu_cam_.transpose());
ROS_INFO_STREAM("-R_cam_imu \n" << R_cam_imu_);
ROS_INFO_STREAM("-p_cam_imu \n" << p_cam_imu_.transpose());

const auto q_CI = camera_.q_CI;
ROS_INFO_STREAM("-q_CI \n" << q_CI.x() << "," << q_CI.y() << "," << q_CI.z() << "," << q_CI.w());
ROS_INFO_STREAM("-p_C_I \n" << camera_.p_C_I.transpose());
}

}

0 comments on commit cf1ee71

Please sign in to comment.