void KeyframeMapper::publishKeyframeAssociations()
{
visualization_msgs::Marker marker;
marker.header.stamp = ros::Time::now();
marker.header.frame_id = fixed_frame_;
marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_LIST;
marker.action = visualization_msgs::Marker::ADD;
marker.points.resize(associations_.size() * 2);
marker.color.a = 1.0;
for (unsigned int as_idx = 0; as_idx < associations_.size(); ++as_idx)
{
// set up shortcut references
const rgbdtools::KeyframeAssociation& association = associations_[as_idx];
int kf_idx_a = association.kf_idx_a;
int kf_idx_b = association.kf_idx_b;
rgbdtools::RGBDKeyframe& keyframe_a = keyframes_[kf_idx_a];
rgbdtools::RGBDKeyframe& keyframe_b = keyframes_[kf_idx_b];
int idx_start = as_idx*2;
int idx_end = as_idx*2 + 1;
tf::Transform keyframe_a_pose = tfFromEigenAffine(keyframe_a.pose);
tf::Transform keyframe_b_pose = tfFromEigenAffine(keyframe_b.pose);
// start point for the edge
marker.points[idx_start].x = keyframe_a_pose.getOrigin().getX();
marker.points[idx_start].y = keyframe_a_pose.getOrigin().getY();
marker.points[idx_start].z = keyframe_a_pose.getOrigin().getZ();
// end point for the edge
marker.points[idx_end].x = keyframe_b_pose.getOrigin().getX();
marker.points[idx_end].y = keyframe_b_pose.getOrigin().getY();
marker.points[idx_end].z = keyframe_b_pose.getOrigin().getZ();
if (association.type == rgbdtools::KeyframeAssociation::VO)
{
marker.ns = "VO";
marker.scale.x = 0.002;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
}
else if (association.type == rgbdtools::KeyframeAssociation::RANSAC)
{
marker.ns = "RANSAC";
marker.scale.x = 0.002;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
}
kf_assoc_pub_.publish(marker);
}
}
void KeyframeMapper::buildOctomap(octomap::OcTree& tree)
{
ROS_INFO("Building Octomap...");
octomap::point3d sensor_origin(0.0, 0.0, 0.0);
for (unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx)
{
ROS_INFO("Processing keyframe %u", kf_idx);
const rgbdtools::RGBDKeyframe& keyframe = keyframes_[kf_idx];
PointCloudT cloud;
keyframe.constructDensePointCloud(cloud, max_range_, max_stdev_);
octomap::pose6d frame_origin = poseTfToOctomap(tfFromEigenAffine(keyframe.pose));
// build octomap cloud from pcl cloud
octomap::Pointcloud octomap_cloud;
for (unsigned int pt_idx = 0; pt_idx < cloud.points.size(); ++pt_idx)
{
const PointT& p = cloud.points[pt_idx];
if (!std::isnan(p.z))
octomap_cloud.push_back(p.x, p.y, p.z);
}
tree.insertScan(octomap_cloud, sensor_origin, frame_origin);
}
}
void KeyframeMapper::buildColorOctomap(octomap::ColorOcTree& tree)
{
ROS_INFO("Building Octomap with color...");
octomap::point3d sensor_origin(0.0, 0.0, 0.0);
for (unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx)
{
ROS_INFO("Processing keyframe %u", kf_idx);
const rgbdtools::RGBDKeyframe& keyframe = keyframes_[kf_idx];
// construct the cloud
PointCloudT::Ptr cloud_unf(new PointCloudT());
keyframe.constructDensePointCloud(*cloud_unf, max_range_, max_stdev_);
// perform filtering for max z
pcl::transformPointCloud(*cloud_unf, *cloud_unf, keyframe.pose);
PointCloudT cloud;
pcl::PassThrough<PointT> pass;
pass.setInputCloud (cloud_unf);
pass.setFilterFieldName ("z");
pass.setFilterLimits (-std::numeric_limits<double>::infinity(), max_map_z_);
pass.filter(cloud);
pcl::transformPointCloud(cloud, cloud, keyframe.pose.inverse());
octomap::pose6d frame_origin = poseTfToOctomap(tfFromEigenAffine(keyframe.pose));
// build octomap cloud from pcl cloud
octomap::Pointcloud octomap_cloud;
for (unsigned int pt_idx = 0; pt_idx < cloud.points.size(); ++pt_idx)
{
const PointT& p = cloud.points[pt_idx];
if (!std::isnan(p.z))
octomap_cloud.push_back(p.x, p.y, p.z);
}
// insert scan (only xyz considered, no colors)
tree.insertScan(octomap_cloud, sensor_origin, frame_origin);
// insert colors
PointCloudT cloud_tf;
pcl::transformPointCloud(cloud, cloud_tf, keyframe.pose);
for (unsigned int pt_idx = 0; pt_idx < cloud_tf.points.size(); ++pt_idx)
{
const PointT& p = cloud_tf.points[pt_idx];
if (!std::isnan(p.z))
{
octomap::point3d endpoint(p.x, p.y, p.z);
octomap::ColorOcTreeNode* n = tree.search(endpoint);
if (n) n->setColor(p.r, p.g, p.b);
}
}
tree.updateInnerOccupancy();
}
}
bool KeyframeMapper::processFrame(
const rgbdtools::RGBDFrame& frame,
const AffineTransform& pose)
{
// add the frame pose to the path vector
geometry_msgs::PoseStamped frame_pose;
tf::Transform frame_tf = tfFromEigenAffine(pose);
tf::poseTFToMsg(frame_tf, frame_pose.pose);
// update the header of the pose for the path
frame_pose.header.frame_id = fixed_frame_;
frame_pose.header.seq = frame.header.seq;
frame_pose.header.stamp.sec = frame.header.stamp.sec;
frame_pose.header.stamp.nsec = frame.header.stamp.nsec;
path_msg_.poses.push_back(frame_pose);
// determine if a new keyframe is needed
bool result;
if(keyframes_.empty() || manual_add_)
{
result = true;
}
else
{
double dist, angle;
getTfDifference(tfFromEigenAffine(pose),
tfFromEigenAffine(keyframes_.back().pose),
dist, angle);
if (dist > kf_dist_eps_ || angle > kf_angle_eps_)
result = true;
else
result = false;
}
if (result)
{
addKeyframe(frame, pose);
}
return result;
}
void pathEigenAffineToROS(
const AffineTransformVector& path,
PathMsg& path_msg)
{
assert(path.size() == path_msg.poses.size());
for (int idx = 0; idx < path.size(); ++idx)
{
tf::Transform pose_tf = tfFromEigenAffine(path[idx]);
tf::poseTFToMsg(pose_tf, path_msg.poses[idx].pose);
}
}
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);
}
/** In the event that the keyframe poses change (from pose-graph solving)
* this function will propagete teh changes in the path message
*/
void KeyframeMapper::updatePathFromKeyframePoses()
{
int kf_size = keyframes_.size();
int f_size = path_msg_.poses.size();
// temporary store the new path
AffineTransformVector path_new;
path_new.resize(f_size);
if (kf_size < 2) return;
for (int kf_idx = 0; kf_idx < kf_size - 1; ++kf_idx)
{
// the indices of the current and next keyframes (a and b)
const rgbdtools::RGBDKeyframe& keyframe_a = keyframes_[kf_idx];
const rgbdtools::RGBDKeyframe& keyframe_b = keyframes_[kf_idx + 1];
// the corresponding frame indices
int f_idx_a = keyframe_a.index;
int f_idx_b = keyframe_b.index;
// the new poses of keyframes a and b (after graph solving)
tf::Transform kf_pose_a = tfFromEigenAffine(keyframe_a.pose);
tf::Transform kf_pose_b = tfFromEigenAffine(keyframe_b.pose);
// the previous pose of keyframe a and b (before graph solving)
tf::Transform kf_pose_a_prev, kf_pose_b_prev;
tf::poseMsgToTF(path_msg_.poses[f_idx_a].pose, kf_pose_a_prev);
tf::poseMsgToTF(path_msg_.poses[f_idx_b].pose, kf_pose_b_prev);
// the motion, in the camera frame (after and before graph solving)
tf::Transform kf_motion = kf_pose_a.inverse() * kf_pose_b;
tf::Transform kf_motion_prev = kf_pose_a_prev.inverse() * kf_pose_b_prev;
// the correction from the graph solving
tf::Transform correction = kf_motion_prev.inverse() * kf_motion;
// update the poses in-between keyframes
for (int f_idx = f_idx_a; f_idx < f_idx_b; ++f_idx)
{
// calculate interpolation scale
double interp_scale = (double)(f_idx - f_idx_a) / (double)(f_idx_b - f_idx_a);
// create interpolated correction translation and rotation
tf::Vector3 v_interp = correction.getOrigin() * interp_scale;
tf::Quaternion q_interp = tf::Quaternion::getIdentity();
q_interp.slerp(correction.getRotation(), interp_scale);
// create interpolated correction
tf::Transform interpolated_correction;
interpolated_correction.setOrigin(v_interp);
interpolated_correction.setRotation(q_interp);
// the previous frame pose
tf::Transform frame_pose_prev;
tf::poseMsgToTF(path_msg_.poses[f_idx].pose, frame_pose_prev);
// the pevious frame motion
tf::Transform frame_motion_prev = kf_pose_a_prev.inverse() * frame_pose_prev;
// the interpolated motion
tf::Transform interpolated_motion = frame_motion_prev * interpolated_correction;
// calculate the interpolated pose
path_new[f_idx] = keyframe_a.pose * eigenAffineFromTf(interpolated_motion);
}
}
// update the last pose
const rgbdtools::RGBDKeyframe& last_kf = keyframes_[kf_size - 1];
tf::Transform last_kf_pose_prev;
tf::poseMsgToTF(path_msg_.poses[last_kf.index].pose, last_kf_pose_prev);
// update the poses in-between last keyframe and end of vo path
for (int f_idx = last_kf.index; f_idx < f_size; ++f_idx)
{
// the previous frame pose
tf::Transform frame_pose_prev;
tf::poseMsgToTF(path_msg_.poses[f_idx].pose, frame_pose_prev);
// the pevious frame motion
tf::Transform frame_motion_prev = last_kf_pose_prev.inverse() * frame_pose_prev;
// calculate the new pose
path_new[f_idx] = last_kf.pose * eigenAffineFromTf(frame_motion_prev);
}
// copy over the interpolated path
pathEigenAffineToROS(path_new, path_msg_);
}
void KeyframeMapper::publishKeyframePose(int i)
{
rgbdtools::RGBDKeyframe& keyframe = keyframes_[i];
// **** publish camera pose
visualization_msgs::Marker marker;
marker.header.stamp = ros::Time::now();
marker.header.frame_id = fixed_frame_;
marker.ns = "keyframe_poses";
marker.id = i;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.points.resize(2);
// start point for the arrow
tf::Transform keyframe_pose = tfFromEigenAffine(keyframe.pose);
marker.points[0].x = keyframe_pose.getOrigin().getX();
marker.points[0].y = keyframe_pose.getOrigin().getY();
marker.points[0].z = keyframe_pose.getOrigin().getZ();
// end point for the arrow
tf::Transform ep;
ep.setIdentity();
ep.setOrigin(tf::Vector3(0.00, 0.00, 0.12)); // z = arrow length
ep = keyframe_pose * ep;
marker.points[1].x = ep.getOrigin().getX();
marker.points[1].y = ep.getOrigin().getY();
marker.points[1].z = ep.getOrigin().getZ();
marker.scale.x = 0.02; // shaft radius
marker.scale.y = 0.05; // head radius
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
poses_pub_.publish(marker);
// **** publish frame index text
visualization_msgs::Marker marker_text;
marker_text.header.stamp = ros::Time::now();
marker_text.header.frame_id = fixed_frame_;
marker_text.ns = "keyframe_indexes";
marker_text.id = i;
marker_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker_text.action = visualization_msgs::Marker::ADD;
tf::poseTFToMsg(keyframe_pose, marker_text.pose);
marker_text.pose.position.z -= 0.05;
char label[6];
sprintf(label, "%d", i);
marker_text.text = label;
marker_text.color.a = 1.0;
marker_text.color.r = 1.0;
marker_text.color.g = 1.0;
marker_text.color.b = 0.0;
marker_text.scale.z = 0.05; // shaft radius
poses_pub_.publish(marker_text);
}