void initOdometer(
const sensor_msgs::CameraInfoConstPtr& l_info_msg,
const sensor_msgs::CameraInfoConstPtr& r_info_msg)
{
// read calibration info from camera info message
// to fill remaining parameters
/*image_geometry::StereoCameraModel model;
model.fromCameraInfo(*l_info_msg, *r_info_msg);
visual_odometer_params_.base = model.baseline();
visual_odometer_params_.calib.f = model.left().fx();
visual_odometer_params_.calib.cu = model.left().cx();
visual_odometer_params_.calib.cv = model.left().cy();
*/
visual_odometer_params_.base = -r_info_msg->P[3] / r_info_msg->P[0];
visual_odometer_params_.calib.f = l_info_msg->P[0];
visual_odometer_params_.calib.cu = l_info_msg->P[2];
visual_odometer_params_.calib.cv = l_info_msg->P[5];
/*printf("%lf %lf %lf\n",l_info_msg->P[0],l_info_msg->P[2],l_info_msg->P[5] );
printf("%lf %lf %lf\n",r_info_msg->P[0],r_info_msg->P[2],r_info_msg->P[5] );*/
visual_odometer_.reset(new VisualOdometryStereo(visual_odometer_params_));
if (l_info_msg->header.frame_id != "") setSensorFrameId(l_info_msg->header.frame_id);
ROS_INFO_STREAM("Initialized libviso2 stereo odometry "
"with the following parameters:" << std::endl <<
visual_odometer_params_ <<
" motion_threshold = " << motion_threshold_);
}
void imageCallback(
const sensor_msgs::ImageConstPtr& image_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg)
{
ros::WallTime start_time = ros::WallTime::now();
bool first_run = false;
// create odometer if not exists
if (!visual_odometer_)
{
first_run = true;
// read calibration info from camera info message
// to fill remaining odometer parameters
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(info_msg);
visual_odometer_params_.calib.f = model.fx();
visual_odometer_params_.calib.cu = model.cx();
visual_odometer_params_.calib.cv = model.cy();
visual_odometer_.reset(new VisualOdometryMono(visual_odometer_params_));
if (image_msg->header.frame_id != "") setSensorFrameId(image_msg->header.frame_id);
ROS_INFO_STREAM("Initialized libviso2 mono odometry "
"with the following parameters:" << std::endl <<
visual_odometer_params_);
}
// convert image if necessary
uint8_t *image_data;
int step;
cv_bridge::CvImageConstPtr cv_ptr;
if (image_msg->encoding == sensor_msgs::image_encodings::MONO8)
{
image_data = const_cast<uint8_t*>(&(image_msg->data[0]));
step = image_msg->step;
}
else
{
cv_ptr = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8);
image_data = cv_ptr->image.data;
step = cv_ptr->image.step[0];
}
// run the odometer
int32_t dims[] = {image_msg->width, image_msg->height, step};
// on first run, only feed the odometer with first image pair without
// retrieving data
if (first_run)
{
visual_odometer_->process(image_data, dims);
tf::Transform delta_transform;
delta_transform.setIdentity();
integrateAndPublish(delta_transform, image_msg->header.stamp);
}
else
{
bool success = visual_odometer_->process(image_data, dims);
if(success)
{
replace_ = false;
Matrix camera_motion = Matrix::inv(visual_odometer_->getMotion());
ROS_DEBUG("Found %i matches with %i inliers.",
visual_odometer_->getNumberOfMatches(),
visual_odometer_->getNumberOfInliers());
ROS_DEBUG_STREAM("libviso2 returned the following motion:\n" << camera_motion);
tf::Matrix3x3 rot_mat(
camera_motion.val[0][0], camera_motion.val[0][1], camera_motion.val[0][2],
camera_motion.val[1][0], camera_motion.val[1][1], camera_motion.val[1][2],
camera_motion.val[2][0], camera_motion.val[2][1], camera_motion.val[2][2]);
tf::Vector3 t(camera_motion.val[0][3], camera_motion.val[1][3], camera_motion.val[2][3]);
tf::Transform delta_transform(rot_mat, t);
integrateAndPublish(delta_transform, image_msg->header.stamp);
}
else
{
ROS_DEBUG("Call to VisualOdometryMono::process() failed. Assuming motion too small.");
replace_ = true;
tf::Transform delta_transform;
delta_transform.setIdentity();
integrateAndPublish(delta_transform, image_msg->header.stamp);
}
// create and publish viso2 info msg
VisoInfo info_msg;
info_msg.header.stamp = image_msg->header.stamp;
info_msg.got_lost = !success;
info_msg.change_reference_frame = false;
info_msg.num_matches = visual_odometer_->getNumberOfMatches();
info_msg.num_inliers = visual_odometer_->getNumberOfInliers();
ros::WallDuration time_elapsed = ros::WallTime::now() - start_time;
info_msg.runtime = time_elapsed.toSec();
info_pub_.publish(info_msg);
}
}