bool KeyframeMapper::solveGraphSrvCallback(
SolveGraph::Request& request,
SolveGraph::Response& response)
{
ros::WallTime start = ros::WallTime::now();
// Graph solving: keyframe positions only, path is interpolated
graph_solver_.solve(keyframes_, associations_);
updatePathFromKeyframePoses();
// Graph solving: keyframe positions and VO path
/*
AffineTransformVector path;
pathROSToEigenAffine(path_msg_, path);
graph_solver_.solve(keyframes_, associations_, path);
pathEigenAffineToROS(path, path_msg_);
*/
double dur = getMsDuration(start);
ROS_INFO("Solving took %.1f ms", dur);
publishPath();
publishKeyframePoses();
publishKeyframeAssociations();
return true;
}
void RGBDImageProc::RGBDCallback(
const ImageMsg::ConstPtr& rgb_msg,
const ImageMsg::ConstPtr& depth_msg,
const CameraInfoMsg::ConstPtr& rgb_info_msg,
const CameraInfoMsg::ConstPtr& depth_info_msg)
{
boost::mutex::scoped_lock(mutex_);
// for profiling
double dur_unwarp, dur_rectify, dur_reproject, dur_cloud, dur_allocate;
// **** initialize if needed
if (!initialized_)
{
initMaps(rgb_info_msg, depth_info_msg);
initialized_ = true;
}
// **** convert ros images to opencv Mat
cv_bridge::CvImageConstPtr rgb_ptr = cv_bridge::toCvShare(rgb_msg);
cv_bridge::CvImageConstPtr depth_ptr = cv_bridge::toCvShare(depth_msg);
const cv::Mat& rgb_img = rgb_ptr->image;
const cv::Mat& depth_img = depth_ptr->image;
//cv::imshow("RGB", rgb_img);
//cv::imshow("Depth", depth_img);
//cv::waitKey(1);
// **** rectify
ros::WallTime start_rectify = ros::WallTime::now();
cv::Mat rgb_img_rect, depth_img_rect;
cv::remap(rgb_img, rgb_img_rect, map_rgb_1_, map_rgb_2_, cv::INTER_LINEAR);
cv::remap(depth_img, depth_img_rect, map_depth_1_, map_depth_2_, cv::INTER_NEAREST);
dur_rectify = getMsDuration(start_rectify);
//cv::imshow("RGB Rect", rgb_img_rect);
//cv::imshow("Depth Rect", depth_img_rect);
//cv::waitKey(1);
// **** unwarp
if (unwarp_)
{
ros::WallTime start_unwarp = ros::WallTime::now();
unwarpDepthImage(depth_img_rect, coeff_0_rect_, coeff_1_rect_, coeff_2_rect_, fit_mode_);
dur_unwarp = getMsDuration(start_unwarp);
}
else dur_unwarp = 0.0;
// **** reproject
ros::WallTime start_reproject = ros::WallTime::now();
cv::Mat depth_img_rect_reg;
buildRegisteredDepthImage(intr_rect_depth_, intr_rect_rgb_, ir2rgb_,
depth_img_rect, depth_img_rect_reg);
dur_reproject = getMsDuration(start_reproject);
// **** point cloud
if (publish_cloud_)
{
ros::WallTime start_cloud = ros::WallTime::now();
PointCloudT::Ptr cloud_ptr;
cloud_ptr.reset(new PointCloudT());
buildPointCloud(depth_img_rect_reg, rgb_img_rect, intr_rect_rgb_, *cloud_ptr);
cloud_ptr->header = rgb_info_msg->header;
cloud_publisher_.publish(cloud_ptr);
dur_cloud = getMsDuration(start_cloud);
}
else dur_cloud = 0.0;
// **** allocate registered rgb image
ros::WallTime start_allocate = ros::WallTime::now();
cv_bridge::CvImage cv_img_rgb(rgb_msg->header, rgb_msg->encoding, rgb_img_rect);
ImageMsg::Ptr rgb_out_msg = cv_img_rgb.toImageMsg();
// **** allocate registered depth image
cv_bridge::CvImage cv_img_depth(depth_msg->header, depth_msg->encoding, depth_img_rect_reg);
ImageMsg::Ptr depth_out_msg = cv_img_depth.toImageMsg();
// **** update camera info (single, since both images are in rgb frame)
rgb_rect_info_msg_.header = rgb_info_msg->header;
dur_allocate = getMsDuration(start_allocate);
ROS_INFO("Rect: %.1f Reproj: %.1f Unwarp: %.1f Cloud %.1f Alloc: %.1f ms",
dur_rectify, dur_reproject, dur_unwarp, dur_cloud, dur_allocate);
// **** publish
rgb_publisher_.publish(rgb_out_msg);
depth_publisher_.publish(depth_out_msg);
info_publisher_.publish(rgb_rect_info_msg_);
}
void RGBDImageProc::RGBDCallback(
const ImageMsg::ConstPtr& rgb_msg,
const ImageMsg::ConstPtr& depth_msg,
const CameraInfoMsg::ConstPtr& rgb_info_msg,
const CameraInfoMsg::ConstPtr& depth_info_msg)
{
boost::mutex::scoped_lock(mutex_);
// for profiling
double dur_unwarp, dur_rectify, dur_reproject, dur_cloud, dur_allocate;
// **** images need to be the same size
if (rgb_msg->height != depth_msg->height ||
rgb_msg->width != depth_msg->width)
{
ROS_WARN("RGB and depth images have different sizes, skipping");
return;
}
// **** initialize if needed
if (size_in_.height != (int)rgb_msg->height ||
size_in_.width != (int)rgb_msg->width)
{
ROS_INFO("Initializing");
size_in_.height = (int)rgb_msg->height;
size_in_.width = (int)rgb_msg->width;
initMaps(rgb_info_msg, depth_info_msg);
}
// **** convert ros images to opencv Mat
cv_bridge::CvImageConstPtr rgb_ptr = cv_bridge::toCvShare(rgb_msg);
cv_bridge::CvImageConstPtr depth_ptr = cv_bridge::toCvShare(depth_msg);
const cv::Mat& rgb_img = rgb_ptr->image;
const cv::Mat& depth_img = depth_ptr->image;
//cv::imshow("RGB", rgb_img);
//cv::imshow("Depth", depth_img);
//cv::waitKey(1);
// **** rectify
ros::WallTime start_rectify = ros::WallTime::now();
cv::Mat rgb_img_rect, depth_img_rect;
cv::remap(rgb_img, rgb_img_rect, map_rgb_1_, map_rgb_2_, cv::INTER_LINEAR);
cv::remap(depth_img, depth_img_rect, map_depth_1_, map_depth_2_, cv::INTER_NEAREST);
dur_rectify = getMsDuration(start_rectify);
//cv::imshow("RGB Rect", rgb_img_rect);
//cv::imshow("Depth Rect", depth_img_rect);
//cv::waitKey(1);
// **** unwarp
if (unwarp_)
{
ros::WallTime start_unwarp = ros::WallTime::now();
rgbdtools::unwarpDepthImage(
depth_img_rect, coeff_0_rect_, coeff_1_rect_, coeff_2_rect_, fit_mode_);
dur_unwarp = getMsDuration(start_unwarp);
}
else dur_unwarp = 0.0;
// **** reproject
ros::WallTime start_reproject = ros::WallTime::now();
cv::Mat depth_img_rect_reg;
rgbdtools::buildRegisteredDepthImage(
intr_rect_depth_, intr_rect_rgb_, ir2rgb_, depth_img_rect, depth_img_rect_reg);
dur_reproject = getMsDuration(start_reproject);
// **** point cloud
if (publish_cloud_)
{
ros::WallTime start_cloud = ros::WallTime::now();
PointCloudT::Ptr cloud_ptr;
cloud_ptr.reset(new PointCloudT());
rgbdtools::buildPointCloud(
depth_img_rect_reg, rgb_img_rect, intr_rect_rgb_, *cloud_ptr);
cloud_ptr->header = rgb_info_msg->header;
cloud_publisher_.publish(cloud_ptr);
dur_cloud = getMsDuration(start_cloud);
}
else dur_cloud = 0.0;
// **** allocate registered rgb image
ros::WallTime start_allocate = ros::WallTime::now();
cv_bridge::CvImage cv_img_rgb(rgb_msg->header, rgb_msg->encoding, rgb_img_rect);
ImageMsg::Ptr rgb_out_msg = cv_img_rgb.toImageMsg();
// **** allocate registered depth image
cv_bridge::CvImage cv_img_depth(depth_msg->header, depth_msg->encoding, depth_img_rect_reg);
ImageMsg::Ptr depth_out_msg = cv_img_depth.toImageMsg();
// **** update camera info (single, since both images are in rgb frame)
rgb_rect_info_msg_.header = rgb_info_msg->header;
dur_allocate = getMsDuration(start_allocate);
// **** print diagnostics
double dur_total = dur_rectify + dur_reproject + dur_unwarp + dur_cloud + dur_allocate;
if(verbose_)
{
ROS_INFO("Rect %.1f Reproj %.1f Unwarp %.1f Cloud %.1f Alloc %.1f Total %.1f ms",
dur_rectify, dur_reproject, dur_unwarp, dur_cloud, dur_allocate,
dur_total);
}
// **** publish
rgb_publisher_.publish(rgb_out_msg);
depth_publisher_.publish(depth_out_msg);
info_publisher_.publish(rgb_rect_info_msg_);
}
