/// \brief Main callback which takes care of republishing.
///
/// This callback is called by the Synchronizer filter when
/// messages for both left/right images and cameras information
/// have been collected.
///
/// The callback the left image timestamps in all the other messages.
///
/// \param pub_l left image publisher (binded)
/// \param pub_cam_l left camera information publisher (binded)
/// \param pub_r right image publisher (binded)
/// \param pub_cam_r right camera information publisher (binded)
/// \param left left image
/// \param left_camera left camera information
/// \param right right image
/// \param right_camera right camera information
void imageCallback(image_transport::Publisher& pub_l,
ros::Publisher& pub_cam_l,
image_transport::Publisher& pub_r,
ros::Publisher& pub_cam_r,
const sensor_msgs::ImageConstPtr& left,
const sensor_msgs::CameraInfoConstPtr& left_camera,
const sensor_msgs::ImageConstPtr& right,
const sensor_msgs::CameraInfoConstPtr& right_camera
)
{
sensor_msgs::Image left_(*left);
sensor_msgs::Image right_(*right);
sensor_msgs::CameraInfo left_camera_(*left_camera);
sensor_msgs::CameraInfo right_camera_(*right_camera);
// Fix timestamps.
// FIXME: a warning should be issued if the differences between
// timestamps are too important.
left_camera_.header.stamp = left_.header.stamp;
right_camera_.header.stamp = left_.header.stamp;
right_.header.stamp = left_.header.stamp;
pub_l.publish(left_);
pub_cam_l.publish(left_camera_);
pub_r.publish(right_);
pub_cam_r.publish(right_camera_);
}
VertexType operator () (ScalarType u, ScalarType v) const {
VertexType a = interpolate(u, left_(v), right_(v)),
b = interpolate(v, bottom_(u), top_(u)),
c = get_patch_value(u, v);
return interpolate(.5,
interpolate(-1, a, c),
interpolate(-1, b, c)
);
}