bool KeyframeMapper::publishKeyframesSrvCallback(
PublishKeyframes::Request& request,
PublishKeyframes::Response& response)
{
bool found_match = false;
// regex matching - try match the request string against each
// keyframe index
boost::regex expression(request.re);
for (unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx)
{
std::stringstream ss;
ss << kf_idx;
std::string kf_idx_string = ss.str();
boost::smatch match;
if(boost::regex_match(kf_idx_string, match, expression))
{
found_match = true;
ROS_INFO("Publishing keyframe %d", kf_idx);
publishKeyframeData(kf_idx);
publishKeyframePose(kf_idx);
usleep(25000);
}
}
publishPath();
return found_match;
}
bool KeyframeMapper::solveGraphSrvCallback(
SolveGraph::Request& request,
SolveGraph::Response& response)
{
ros::WallTime start = ros::WallTime::now();
// Graph solving: keyframe positions only, path is interpolated
graph_solver_.solve(keyframes_, associations_);
updatePathFromKeyframePoses();
// Graph solving: keyframe positions and VO path
/*
AffineTransformVector path;
pathROSToEigenAffine(path_msg_, path);
graph_solver_.solve(keyframes_, associations_, path);
pathEigenAffineToROS(path, path_msg_);
*/
double dur = getMsDuration(start);
ROS_INFO("Solving took %.1f ms", dur);
publishPath();
publishKeyframePoses();
publishKeyframeAssociations();
return true;
}
void KeyframeMapper::RGBDCallback(
const ImageMsg::ConstPtr& rgb_msg,
const ImageMsg::ConstPtr& depth_msg,
const CameraInfoMsg::ConstPtr& info_msg)
{
tf::StampedTransform transform;
const ros::Time& time = rgb_msg->header.stamp;
try{
tf_listener_.waitForTransform(
fixed_frame_, rgb_msg->header.frame_id, time, ros::Duration(0.1));
tf_listener_.lookupTransform(
fixed_frame_, rgb_msg->header.frame_id, time, transform);
}
catch(...)
{
return;
}
// create a new frame and increment the counter
rgbdtools::RGBDFrame frame;
createRGBDFrameFromROSMessages(rgb_msg, depth_msg, info_msg, frame);
frame.index = rgbd_frame_index_;
rgbd_frame_index_++;
bool result = processFrame(frame, eigenAffineFromTf(transform));
if (result) publishKeyframeData(keyframes_.size() - 1);
publishPath();
}
// Set and publish full state.
void Publisher::publishFullStateAsCallback(
const okvis::Time & t, const okvis::kinematics::Transformation & T_WS,
const Eigen::Matrix<double, 9, 1> & speedAndBiases,
const Eigen::Matrix<double, 3, 1> & omega_S)
{
setTime(t);
setOdometry(T_WS, speedAndBiases, omega_S); // TODO: provide setters for this hack
setPath(T_WS);
publishOdometry();
publishTransform();
publishPath();
}
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);
}
