void StereoProcessor::processDisparity(const cv::Mat& left_rect, const cv::Mat& right_rect,
const image_geometry::StereoCameraModel& model,
stereo_msgs::DisparityImage& disparity) const
{
// Fixed-point disparity is 16 times the true value: d = d_fp / 16.0 = x_l - x_r.
static const int DPP = 16; // disparities per pixel
static const double inv_dpp = 1.0 / DPP;
// Block matcher produces 16-bit signed (fixed point) disparity image
block_matcher_(left_rect, right_rect, disparity16_);
// Fill in DisparityImage image data, converting to 32-bit float
sensor_msgs::Image& dimage = disparity.image;
dimage.height = disparity16_.rows;
dimage.width = disparity16_.cols;
dimage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
dimage.step = dimage.width * sizeof(float);
dimage.data.resize(dimage.step * dimage.height);
cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
// We convert from fixed-point to float disparity and also adjust for any x-offset between
// the principal points: d = d_fp*inv_dpp - (cx_l - cx_r)
disparity16_.convertTo(dmat, dmat.type(), inv_dpp, -(model.left().cx() - model.right().cx()));
ROS_ASSERT(dmat.data == &dimage.data[0]);
/// @todo is_bigendian? :)
// Stereo parameters
disparity.f = model.right().fx();
disparity.T = model.baseline();
/// @todo Window of (potentially) valid disparities
// Disparity search range
disparity.min_disparity = getMinDisparity();
disparity.max_disparity = getMinDisparity() + getDisparityRange() - 1;
disparity.delta_d = inv_dpp;
}
void DisparityWideNodelet::imageCb(const ImageConstPtr& l_image_msg,
const CameraInfoConstPtr& l_info_msg,
const ImageConstPtr& r_image_msg,
const CameraInfoConstPtr& r_info_msg)
{
/// @todo Convert (share) with new cv_bridge
assert(l_image_msg->encoding == sensor_msgs::image_encodings::MONO8);
assert(r_image_msg->encoding == sensor_msgs::image_encodings::MONO8);
// Update the camera model
model_.fromCameraInfo(l_info_msg, r_info_msg);
// Allocate new disparity image message
DisparityImagePtr disp_msg = boost::make_shared<DisparityImage>();
disp_msg->header = l_info_msg->header;
disp_msg->image.header = l_info_msg->header;
disp_msg->image.height = l_image_msg->height;
disp_msg->image.width = l_image_msg->width;
disp_msg->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
disp_msg->image.step = disp_msg->image.width * sizeof(float);
disp_msg->image.data.resize(disp_msg->image.height * disp_msg->image.step);
// Stereo parameters
disp_msg->f = model_.right().fx();
disp_msg->T = model_.baseline();
// Compute window of (potentially) valid disparities
cv::Ptr<CvStereoBMState> params = block_matcher_.state;
int border = params->SADWindowSize / 2;
int left = params->numberOfDisparities + params->minDisparity + border - 1;
int wtf = (params->minDisparity >= 0) ? border + params->minDisparity : std::max(border, -params->minDisparity);
int right = disp_msg->image.width - 1 - wtf;
int top = border;
int bottom = disp_msg->image.height - 1 - border;
disp_msg->valid_window.x_offset = left;
disp_msg->valid_window.y_offset = top;
disp_msg->valid_window.width = right - left;
disp_msg->valid_window.height = bottom - top;
// Disparity search range
disp_msg->min_disparity = params->minDisparity;
disp_msg->max_disparity = params->minDisparity + params->numberOfDisparities - 1;
disp_msg->delta_d = 1.0 / 16; // OpenCV uses 16 disparities per pixel
// Create cv::Mat views onto all buffers
const cv::Mat_<uint8_t> l_image(l_image_msg->height, l_image_msg->width,
const_cast<uint8_t*>(&l_image_msg->data[0]),
l_image_msg->step);
const cv::Mat_<uint8_t> r_image(r_image_msg->height, r_image_msg->width,
const_cast<uint8_t*>(&r_image_msg->data[0]),
r_image_msg->step);
cv::Mat_<float> disp_image(disp_msg->image.height, disp_msg->image.width,
reinterpret_cast<float*>(&disp_msg->image.data[0]),
disp_msg->image.step);
// Perform block matching to find the disparities
block_matcher_(l_image, r_image, disp_image, CV_32F);
// Adjust for any x-offset between the principal points: d' = d - (cx_l - cx_r)
double cx_l = model_.left().cx();
double cx_r = model_.right().cx();
if (cx_l != cx_r)
cv::subtract(disp_image, cv::Scalar(cx_l - cx_r), disp_image);
pub_disparity_.publish(disp_msg);
}
void imageCb(const sensor_msgs::ImageConstPtr& l_image,
const sensor_msgs::CameraInfoConstPtr& l_cam_info,
const sensor_msgs::ImageConstPtr& r_image,
const sensor_msgs::CameraInfoConstPtr& r_cam_info)
{
ROS_INFO("In callback, seq = %u", l_cam_info->header.seq);
// Convert ROS messages for use with OpenCV
cv::Mat left, right;
try {
left = l_bridge_.imgMsgToCv(l_image, "mono8");
right = r_bridge_.imgMsgToCv(r_image, "mono8");
}
catch (sensor_msgs::CvBridgeException& e) {
ROS_ERROR("Conversion error: %s", e.what());
return;
}
cam_model_.fromCameraInfo(l_cam_info, r_cam_info);
frame_common::CamParams cam_params;
cam_params.fx = cam_model_.left().fx();
cam_params.fy = cam_model_.left().fy();
cam_params.cx = cam_model_.left().cx();
cam_params.cy = cam_model_.left().cy();
cam_params.tx = cam_model_.baseline();
if (vslam_system_.addFrame(cam_params, left, right)) {
/// @todo Not rely on broken encapsulation of VslamSystem here
int size = vslam_system_.sba_.nodes.size();
if (size % 2 == 0) {
// publish markers
sba::drawGraph(vslam_system_.sba_, cam_marker_pub_, point_marker_pub_);
}
// Publish VO tracks
if (vo_tracks_pub_.getNumSubscribers() > 0) {
frame_common::drawVOtracks(left, vslam_system_.vo_.frames, vo_display_);
IplImage ipl = vo_display_;
sensor_msgs::ImagePtr msg = sensor_msgs::CvBridge::cvToImgMsg(&ipl);
msg->header = l_cam_info->header;
vo_tracks_pub_.publish(msg, l_cam_info);
}
// Refine large-scale SBA.
const int LARGE_SBA_INTERVAL = 10;
if (size > 4 && size % LARGE_SBA_INTERVAL == 0) {
ROS_INFO("Running large SBA on %d nodes", size);
vslam_system_.refine();
}
if (pointcloud_pub_.getNumSubscribers() > 0 && size % 2 == 0)
publishRegisteredPointclouds(vslam_system_.sba_, vslam_system_.frames_, pointcloud_pub_);
// Publish odometry data to tf.
if (0) // TODO: Change this to parameter.
{
ros::Time stamp = l_cam_info->header.stamp;
std::string image_frame = l_cam_info->header.frame_id;
Eigen::Vector4d trans = -vslam_system_.sba_.nodes.back().trans;
Eigen::Quaterniond rot = vslam_system_.sba_.nodes.back().qrot.conjugate();
trans.head<3>() = rot.toRotationMatrix()*trans.head<3>();
tf_transform_.setOrigin(tf::Vector3(trans(0), trans(1), trans(2)));
tf_transform_.setRotation(tf::Quaternion(rot.x(), rot.y(), rot.z(), rot.w()) );
tf::Transform simple_transform;
simple_transform.setOrigin(tf::Vector3(0, 0, 0));
simple_transform.setRotation(tf::Quaternion(.5, -.5, .5, .5));
tf_broadcast_.sendTransform(tf::StampedTransform(tf_transform_, stamp, image_frame, "visual_odom"));
tf_broadcast_.sendTransform(tf::StampedTransform(simple_transform, stamp, "visual_odom", "pgraph"));
// Publish odometry data on topic.
if (odom_pub_.getNumSubscribers() > 0)
{
tf::StampedTransform base_to_image;
tf::Transform base_to_visodom;
try
{
tf_listener_.lookupTransform(image_frame, "/base_footprint",
stamp, base_to_image);
}
catch (tf::TransformException ex)
{
ROS_WARN("%s",ex.what());
return;
}
base_to_visodom = tf_transform_.inverse() * base_to_image;
geometry_msgs::PoseStamped pose;
nav_msgs::Odometry odom;
pose.header.frame_id = "/visual_odom";
pose.pose.position.x = base_to_visodom.getOrigin().x();
pose.pose.position.y = base_to_visodom.getOrigin().y();
pose.pose.position.z = base_to_visodom.getOrigin().z();
pose.pose.orientation.x = base_to_visodom.getRotation().x();
pose.pose.orientation.y = base_to_visodom.getRotation().y();
pose.pose.orientation.z = base_to_visodom.getRotation().z();
pose.pose.orientation.w = base_to_visodom.getRotation().w();
odom.header.stamp = stamp;
odom.header.frame_id = "/visual_odom";
odom.child_frame_id = "/base_footprint";
odom.pose.pose = pose.pose;
/* odom.pose.covariance[0] = 1;
odom.pose.covariance[7] = 1;
odom.pose.covariance[14] = 1;
odom.pose.covariance[21] = 1;
odom.pose.covariance[28] = 1;
odom.pose.covariance[35] = 1; */
odom_pub_.publish(odom);
}
}
}
}
