void VisualOdometry::RGBDCallback(
const ImageMsg::ConstPtr& rgb_msg,
const ImageMsg::ConstPtr& depth_msg,
const CameraInfoMsg::ConstPtr& info_msg)
{
ros::WallTime start = ros::WallTime::now();
// **** initialize ***************************************************
if (!initialized_)
{
initialized_ = getBaseToCameraTf(rgb_msg->header);
init_time_ = rgb_msg->header.stamp;
if (!initialized_) return;
motion_estimation_.setBaseToCameraTf(eigenAffineFromTf(b2c_));
}
// **** create frame *************************************************
ros::WallTime start_frame = ros::WallTime::now();
rgbdtools::RGBDFrame frame;
createRGBDFrameFromROSMessages(rgb_msg, depth_msg, info_msg, frame);
ros::WallTime end_frame = ros::WallTime::now();
// **** find features ************************************************
ros::WallTime start_features = ros::WallTime::now();
feature_detector_->findFeatures(frame);
ros::WallTime end_features = ros::WallTime::now();
// **** registration *************************************************
ros::WallTime start_reg = ros::WallTime::now();
AffineTransform m = motion_estimation_.getMotionEstimation(frame);
tf::Transform motion = tfFromEigenAffine(m);
f2b_ = motion * f2b_;
ros::WallTime end_reg = ros::WallTime::now();
// **** publish outputs **********************************************
if (publish_tf_) publishTf(rgb_msg->header);
if (publish_odom_) publishOdom(rgb_msg->header);
if (publish_path_) publishPath(rgb_msg->header);
if (publish_pose_) publishPoseStamped(rgb_msg->header);
if (publish_feature_cloud_) publishFeatureCloud(frame);
if (publish_feature_cov_) publishFeatureCovariances(frame);
if (publish_model_cloud_) publishModelCloud();
if (publish_model_cov_) publishModelCovariances();
// **** print diagnostics *******************************************
ros::WallTime end = ros::WallTime::now();
frame_count_++;
int n_features = frame.keypoints.size();
int n_valid_features = frame.n_valid_keypoints;
int n_model_pts = motion_estimation_.getModelSize();
double d_frame = 1000.0 * (end_frame - start_frame ).toSec();
double d_features = 1000.0 * (end_features - start_features).toSec();
double d_reg = 1000.0 * (end_reg - start_reg ).toSec();
double d_total = 1000.0 * (end - start ).toSec();
diagnostics(n_features, n_valid_features, n_model_pts,
d_frame, d_features, d_reg, d_total);
}
void SparseTrackerAM::pointCloudCallback(const PointCloudT::ConstPtr& cloud_in_ptr)
{
boost::mutex::scoped_lock(mutex_);
struct timeval start_callback, end_callback;
struct timeval start_features, end_features;
struct timeval start_model, end_model;
struct timeval start_icp, end_icp;
gettimeofday(&start_callback, NULL);
// **** initialize ***********************************************************
if (!initialized_)
{
initialized_ = getBaseToCameraTf(cloud_in_ptr);
if (!initialized_) return;
}
// **** extract features *****************************************************
gettimeofday(&start_features, NULL);
// **** ab prediction
ros::Time cur_time = cloud_in_ptr->header.stamp;
double dt = (cur_time - prev_time_).toSec();
prev_time_ = cur_time;
tf::Transform predicted_f2b;
double pr_x, pr_y, pr_z, pr_roll, pr_pitch, pr_yaw;
if (use_alpha_beta_)
{
double cx, cy, cz, croll, cpitch, cyaw;
getXYZRPY(f2b_, cx, cy, cz, croll, cpitch, cyaw);
pr_x = cx + v_x_ * dt;
pr_y = cy + v_y_ * dt;
pr_z = cz + v_z_ * dt;
pr_roll = croll + v_roll_ * dt;
pr_pitch = cpitch + v_pitch_ * dt;
pr_yaw = cyaw + v_yaw_ * dt;
btVector3 pr_pos(pr_x, pr_y, pr_z);
btQuaternion pr_q;
pr_q.setRPY(pr_roll, pr_pitch, pr_yaw);
predicted_f2b.setOrigin(pr_pos);
predicted_f2b.setRotation(pr_q);
}
else
{
predicted_f2b = f2b_;
}
PointCloudOrb::Ptr orb_features_ptr = boost::make_shared<PointCloudOrb>();
bool orb_extract_result = extractOrbFeatures(cloud_in_ptr, orb_features_ptr);
pcl::transformPointCloud (*orb_features_ptr , *orb_features_ptr, eigenFromTf(predicted_f2b * b2c_));
orb_features_ptr->header.frame_id = fixed_frame_;
// FIXME - removed canny
PointCloudCanny::Ptr canny_features_ptr = boost::make_shared<PointCloudCanny>();
bool canny_extract_result;// = extractCannyFeatures(cloud_in_ptr, canny_features_ptr);
pcl::transformPointCloud (*canny_features_ptr , *canny_features_ptr, eigenFromTf(predicted_f2b * b2c_));
canny_features_ptr->header.frame_id = fixed_frame_;
gettimeofday(&end_features, NULL);
// **** ICP ******************************
gettimeofday(&start_icp, NULL);
bool orb_icp_result = false;
bool canny_icp_result = false;
if (orb_extract_result)
{
//printf("~ORB~\n");
orb_icp_result = OrbICP(orb_features_ptr);
double eps_roll_ = 0.3;
double eps_pitch_ = 0.3;
double eps_yaw_ = 0.3;
double eps_x_ = 0.3;
double eps_y_ = 0.3;
double eps_z_ = 0.3;
if (orb_icp_result)
{
tf::Transform corr = tfFromEigen(orb_reg_.getFinalTransformation());
// particles add error to motion
double c_x, c_y, c_z, c_roll, c_pitch, c_yaw;
double e_x, e_y, e_z, e_roll, e_pitch, e_yaw;
getXYZRPY(corr, c_x, c_y, c_z, c_roll, c_pitch, c_yaw);
for (int i = 0; i < 20; i++)
{
e_roll = getUrand() * eps_roll_ * c_roll;
e_pitch = getUrand() * eps_pitch_ * c_pitch;
e_yaw = getUrand() * eps_yaw_ * c_yaw;
e_x = getUrand() * eps_x_ * c_x;
e_y = getUrand() * eps_y_ * c_y;
e_z = getUrand() * eps_z_ * c_z;
btVector3 e_pos(c_x + e_x, c_y + e_y, c_z + e_z);
btQuaternion e_q;
e_q.setRPY(c_roll + e_roll, c_pitch + e_pitch, c_yaw + e_yaw);
tf::Transform e_corr;
e_corr.setOrigin(e_pos);
e_corr.setRotation(e_q);
pf2b_[i] = e_corr * pf2b_[i];
}
// end particles
tf::Transform measured_f2b = corr * predicted_f2b;
// **** ab estmation
if (use_alpha_beta_)
{
double m_x, m_y, m_z, m_roll, m_pitch, m_yaw;
getXYZRPY(measured_f2b, m_x, m_y, m_z, m_roll, m_pitch, m_yaw);
// residuals
double r_x = m_x - pr_x;
double r_y = m_y - pr_y;
double r_z = m_z - pr_z;
double r_roll = m_roll - pr_roll;
double r_pitch = m_pitch - pr_pitch;
double r_yaw = m_yaw - pr_yaw;
fixAngleD(r_roll);
fixAngleD(r_pitch);
fixAngleD(r_yaw);
// final position
double f_x = pr_x + alpha_ * r_x;
double f_y = pr_y + alpha_ * r_y;
double f_z = pr_z + alpha_ * r_z;
double f_roll = pr_roll + alpha_ * r_roll;
double f_pitch = pr_pitch + alpha_ * r_pitch;
double f_yaw = pr_yaw + alpha_ * r_yaw;
btVector3 f_pos(f_x, f_y, f_z);
btQuaternion f_q;
f_q.setRPY(f_roll, f_pitch, f_yaw);
f2b_.setOrigin(f_pos);
f2b_.setRotation(f_q);
// final velocity
v_x_ = v_x_ + beta_ * r_x / dt;
v_y_ = v_y_ + beta_ * r_y / dt;
v_z_ = v_z_ + beta_ * r_z / dt;
v_roll_ = v_roll_ + beta_ * r_roll / dt;
v_pitch_ = v_pitch_ + beta_ * r_pitch / dt;
v_yaw_ = v_yaw_ + beta_ * r_yaw / dt;
//printf("VEL: %f, %f, %f\n", v_x_, v_y_, v_z_);
}
else
{
f2b_ = measured_f2b;
}
}
}
if (!orb_icp_result)
printf("ERROR\n");
gettimeofday(&end_icp, NULL);
// **** ADD features to model
gettimeofday(&start_model, NULL);
if (model_->points.size() == 0)
{
*model_ += *orb_features_ptr;
}
else
{
pcl::KdTreeFLANN<PointOrb> tree_model;
tree_model.setInputCloud(model_);
std::vector<int> indices;
std::vector<float> distances;
indices.resize(1);
distances.resize(1);
for (int i = 0; i < orb_features_ptr->points.size(); ++i)
{
PointOrb& p = orb_features_ptr->points[i];
int n_found = tree_model.nearestKSearch(p, 1, indices, distances);
if (n_found == 0)
{
model_->points.push_back(p);
model_->width++;
}
else
{
if ( distances[0] > 0.01)
{
//found far away, insert new one
model_->points.push_back(p);
model_->width++;
}
else
{
// found near, modify old one
//PointOrb& q = model_->points[indices[0]];
//q.x = 0.5*(p.x + q.x);
//q.y = 0.5*(p.y + q.y);
//q.z = 0.5*(p.z + q.z);
}
}
}
}
gettimeofday(&end_model, NULL);
// *** broadcast tf **********************************************************
broadcastTF(cloud_in_ptr);
// *** counter **************************************************************
frame_count_++;
// **** print diagnostics ****************************************************
gettimeofday(&end_callback, NULL);
double features_dur = msDuration(start_features, end_features);
double model_dur = msDuration(start_model, end_model);
double icp_dur = msDuration(start_icp, end_icp);
double callback_dur = msDuration(start_callback, end_callback);
//int model_frames = orb_history_.getSize();
int icp_iterations = orb_reg_.getFinalIterations();
int orb_features_count = orb_features_ptr->points.size();
int canny_features_count =canny_features_ptr->points.size();
int model_size = model_->points.size();
printf("F[%d][%d] %.1f \t M[%d] %.1f \t ICP[%d] %.1f \t TOTAL %.1f\n",
orb_features_count, canny_features_count, features_dur,
model_size, model_dur,
icp_iterations,
icp_dur, callback_dur);
}
