CaptureServer() :
nh_private("~") {
ROS_INFO("Creating localization");
tf_prefix_ = tf::getPrefixParam(nh_private);
odom_frame = tf::resolve(tf_prefix_, "odom_combined");
map_frame = tf::resolve(tf_prefix_, "map");
map_to_odom.setIdentity();
queue_size_ = 1;
rgb_sub.subscribe(nh_, "rgb/image_raw", queue_size_);
depth_sub.subscribe(nh_, "depth/image_raw", queue_size_);
info_sub.subscribe(nh_, "rgb/camera_info", queue_size_);
// Synchronize inputs.
sync.reset(
new Synchronizer(SyncPolicy(queue_size_), rgb_sub, depth_sub,
info_sub));
sync->registerCallback(
boost::bind(&CaptureServer::RGBDCallback, this, _1, _2, _3));
tracked_points.reset(new std::vector<cv::Vec2f>);
}
void PoseTransform()
{
tf::Vector3 point_vec(ARTag_pose.position.x, ARTag_pose.position.y, ARTag_pose.position.z);
tf::Quaternion quat_vec(ARTag_pose.orientation.x, ARTag_pose.orientation.y, ARTag_pose.orientation.z, ARTag_pose.orientation.w);
artag_tf.setIdentity();
artag_tf.setOrigin(point_vec);
artag_tf.setRotation(quat_vec);
tf::Vector3 point_vec_1(ARTag_pose_1.position.x, ARTag_pose_1.position.y, ARTag_pose_1.position.z);
tf::Quaternion quat_vec_1(ARTag_pose_1.orientation.x, ARTag_pose_1.orientation.y, ARTag_pose_1.orientation.z, ARTag_pose_1.orientation.w);
artag_tf_1.setIdentity();
artag_tf_1.setOrigin(point_vec_1);
artag_tf_1.setRotation(quat_vec_1);
}