Subscriber.cpp

void Subscriber::imageCallback(const sensor_msgs::ImageConstPtr& msg,/*
 const sensor_msgs::CameraInfoConstPtr& info,*/
                               unsigned int cameraIndex)
{
  //==============================================================================
  // Copy the ros image message to cv::Mat.
  // Note we force reading it in as a MONO image (we don't handle color images)
  cv_bridge::CvImageConstPtr cv_ptr;
  try {
    cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8);
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

  // Finally do our normal blur if specified
  cv::Mat filtered;
  if (vioParameters_.optimization.useMedianFilter) {
    cv::medianBlur(cv_ptr->image, filtered, 3);
  } else {
    filtered = cv_ptr->image.clone();
  }
  //==============================================================================

  // adapt timestamp
  okvis::Time t(msg->header.stamp.sec, msg->header.stamp.nsec);
  t -= okvis::Duration(vioParameters_.sensors_information.imageDelay);

  if (!vioInterface_->addImage(t, cameraIndex, filtered))
    LOG(WARNING) << "Frame delayed at time "<<t;

  // TODO: pass the keypoints...
}

Make sure to include

#include <cv_bridge/cv_bridge.h>

This will allow OKVIZ to correctly handle color images.