CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

Refactor aruco_pose, split up to aruco_detect and aruco_map notelets

Browse Source
This commit is contained in:
Oleg Kalachev
2019-02-10 00:33:09 +03:00
parent 38f89fd68f
commit adc485c75a
14 changed files with 794 additions and 550 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
aruco_pose/CMakeLists.txt
View File

@@ -16,9 +16,11 @@ find_package(catkin REQUIRED COMPONENTS
image_transport
cv_bridge
tf
#tf2
#tf2_ros
#aruco_msgs
tf2
tf2_ros
tf2_geometry_msgs
sensor_msgs
message_generation
)
find_package(OpenCV 3 REQUIRED)
@@ -57,11 +59,12 @@ find_package(OpenCV 3 REQUIRED)
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
#add_message_files(
# FILES
# Marker.msg
# MarkerArray.msg
#)
add_message_files(
FILES
Point2D.msg
Marker.msg
MarkerArray.msg
)
## Generate services in the 'srv' folder
# add_service_files(
@@ -78,10 +81,11 @@ find_package(OpenCV 3 REQUIRED)
# )
## Generate added messages and services with any dependencies listed here
#generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
#)
generate_messages(
DEPENDENCIES
std_msgs
geometry_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
@@ -113,9 +117,9 @@ find_package(OpenCV 3 REQUIRED)
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
INCLUDE_DIRS DEPENDS OpenCV
LIBRARIES aruco_pose
# CATKIN_DEPENDS other_catkin_pkg
CATKIN_DEPENDS message_runtime
# DEPENDS system_lib
)
@@ -128,17 +132,16 @@ catkin_package(
include_directories(
# include
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(${PROJECT_NAME}
src/aruco_pose.cpp
add_library(aruco_pose
src/aruco_detect.cpp
src/aruco_map.cpp
)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME} aruco_pose_generate_messages_cpp)
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
@@ -156,9 +159,7 @@ add_library(${PROJECT_NAME}
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
link_directories(/opt/ros/kinetic/lib)
target_link_libraries(${PROJECT_NAME}
target_link_libraries(aruco_pose
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
)

4
aruco_pose/map/map.txt Normal file
View File

@@ -0,0 +1,4 @@
1 0.33 0 0 0 0 0 0
2 0.33 1 0 0 0 0 0
3 0.33 0 1 0 0 0 0
4 0.33 1 1 0 0 0 0

6
aruco_pose/msg/Marker.msg Normal file
View File

@@ -0,0 +1,6 @@
uint32 id
geometry_msgs/PoseWithCovariance pose
Point2D c1
Point2D c2
Point2D c3
Point2D c4

2
aruco_pose/msg/MarkerArray.msg Normal file
View File

@@ -0,0 +1,2 @@
Header header
Marker[] markers

2
aruco_pose/msg/Point2D.msg Normal file
View File

@@ -0,0 +1,2 @@
float32 x
float32 y

5
aruco_pose/nodelet_plugins.xml
View File

@@ -1,5 +1,8 @@
<library path="lib/libaruco_pose">
<class name="aruco_pose/aruco_pose" type="ArucoPose" base_class_type="nodelet::Nodelet">
<class name="aruco_pose/aruco_detect" type="ArucoDetect" base_class_type="nodelet::Nodelet">
<description/>
</class>
<class name="aruco_pose/aruco_map" type="ArucoMap" base_class_type="nodelet::Nodelet">
<description/>
</class>
</library>

30
aruco_pose/package.xml
View File

@@ -2,30 +2,38 @@
<package>
<name>aruco_pose</name>
<version>0.0.0</version>
<description>ArUco maps precise pose estimation nodelet</description>
<description>Positioning with ArUco markers</description>
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
<license>MIT</license>
<!--url type="website">http://wiki.ros.org/aruco_pose</url-->
<author email="okalachev@gmail.com">Oleg Kalachev</author>
<author email="urpylka@gmail.com">Artem Smirnov</author>
<!-- Use build_depend for packages you need at compile time: -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>tf</build_depend>
<build_depend>tf2</build_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>message_runtime</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>tf</build_depend>
<!-- Use buildtool_depend for build tool packages: -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Use run_depend for packages you need at runtime: -->
<build_depend>std_msgs</build_depend>
<run_depend>nodelet</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>cv_bridge</run_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<run_depend>message_runtime</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>tf2</run_depend>
<run_depend>tf2_ros</run_depend>
<run_depend>tf2_geometry_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>

294
aruco_pose/src/aruco_detect.cpp Normal file
View File

@@ -0,0 +1,294 @@
/*
* Detecting and pose estimation of ArUco markers
* Copyright (C) 2018 Copter Express Technologies
*
* Author: Oleg Kalachev <okalachev@gmail.com>
*
* Distributed under MIT License (available at https://opensource.org/licenses/MIT).
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*/
/*
* Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
* under the BSD license.
*/
#include <math.h>
#include <vector>
#include <string>
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <tf/transform_datatypes.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <aruco_pose/Marker.h>
#include <aruco_pose/MarkerArray.h>
#include "utils.h"
using std::vector;
using cv::Mat;
class ArucoDetect : public nodelet::Nodelet {
private:
ros::NodeHandle nh_, nh_priv_;
tf2_ros::TransformBroadcaster br_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_{tf_buffer_};
cv::Ptr<cv::aruco::Dictionary> dictionary_;
cv::Ptr<cv::aruco::DetectorParameters> parameters_;
image_transport::Publisher debug_pub_;
image_transport::CameraSubscriber img_sub_;
ros::Publisher markers_pub_, vis_markers_pub_;
bool estimate_poses_, send_tf_;
double length_;
std::string frame_id_prefix_, snap_orientation_;
Mat camera_matrix_, dist_coeffs_;
aruco_pose::MarkerArray array_;
visualization_msgs::MarkerArray vis_array_;
public:
virtual void onInit()
{
nh_ = getNodeHandle();
nh_priv_ = getPrivateNodeHandle();
int dictionary;
nh_priv_.param("dictionary", dictionary, 2);
nh_priv_.param("estimate_poses", estimate_poses_, true);
nh_priv_.param("send_tf", send_tf_, true);
if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
ROS_FATAL("aruco_detect: can't estimate marker's poses as ~length parameter is not defined");
ros::shutdown();
}
nh_priv_.param<std::string>("snap_orientation", snap_orientation_, "");
nh_priv_.param<std::string>("frame_id_prefix", frame_id_prefix_, "aruco_");
camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
dictionary_ = cv::aruco::getPredefinedDictionary(static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(dictionary));
parameters_ = cv::aruco::DetectorParameters::create();
parameters_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
image_transport::ImageTransport it(nh_);
image_transport::ImageTransport it_priv(nh_priv_);
debug_pub_ = it_priv.advertise("debug", 1);
markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);
ROS_INFO("aruco_detect: ready");
}
private:
void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
{
Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
vector<int> ids;
vector<vector<cv::Point2f>> corners, rejected;
vector<cv::Vec3d> rvecs, tvecs;
vector<cv::Point3f> obj_points;
geometry_msgs::TransformStamped snap_to;
// Detect markers
cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
array_.header.stamp = msg->header.stamp;
array_.header.frame_id = msg->header.frame_id;
array_.markers.clear();
if (ids.size() != 0) {
parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
// Estimate individual markers' poses
if (estimate_poses_) {
cv::aruco::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_,
rvecs, tvecs);
if (!snap_orientation_.empty()) {
try {
snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, snap_orientation_,
msg->header.stamp, ros::Duration(0.02));
} catch (const tf2::TransformException& e) {
ROS_WARN_THROTTLE(5, "aruco_detect: can't snap: %s", e.what());
}
}
}
array_.markers.reserve(ids.size());
aruco_pose::Marker marker;
geometry_msgs::TransformStamped transform;
transform.header.stamp = msg->header.stamp;
transform.header.frame_id = msg->header.frame_id;
for (unsigned int i = 0; i < ids.size(); i++) {
marker.id = ids[i];
fillCorners(marker, corners[i]);
if (estimate_poses_) {
fillPose(marker.pose.pose, rvecs[i], tvecs[i]);
// snap orientation (if enabled and snap frame avaiable)
if (!snap_orientation_.empty() && !snap_to.header.frame_id.empty()) {
snapOrientation(marker.pose.pose, snap_to.transform.rotation);
}
// TODO: check IDs are unique
if (send_tf_) {
transform.child_frame_id = getChildFrameId(ids[i]);
transform.transform.rotation = marker.pose.pose.orientation;
fillTranslation(transform.transform.translation, tvecs[i]);
br_.sendTransform(transform);
}
}
array_.markers.push_back(marker);
}
}
markers_pub_.publish(array_);
// Publish visualization markers
if (estimate_poses_ && vis_markers_pub_.getNumSubscribers() != 0) {
// Delete all markers
visualization_msgs::Marker vis_marker;
vis_marker.action = visualization_msgs::Marker::DELETEALL;
vis_array_.markers.clear();
vis_array_.markers.reserve(ids.size() + 1);
vis_array_.markers.push_back(vis_marker);
for (unsigned int i = 0; i < ids.size(); i++)
pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose.pose,
length_, ids[i], i);
vis_markers_pub_.publish(vis_array_);
}
// Publish debug image
if (debug_pub_.getNumSubscribers() != 0) {
Mat debug = image.clone();
cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
if (estimate_poses_)
for (unsigned int i = 0; i < ids.size(); i++)
cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], length_);
cv_bridge::CvImage out_msg;
out_msg.header.frame_id = msg->header.frame_id;
out_msg.header.stamp = msg->header.stamp;
out_msg.encoding = sensor_msgs::image_encodings::BGR8;
out_msg.image = debug;
debug_pub_.publish(out_msg.toImageMsg());
}
}
inline void fillCorners(aruco_pose::Marker& marker, const vector<cv::Point2f>& corners) const
{
marker.c1.x = corners[0].x;
marker.c2.x = corners[1].x;
marker.c3.x = corners[2].x;
marker.c4.x = corners[3].x;
marker.c1.y = corners[0].y;
marker.c2.y = corners[1].y;
marker.c3.y = corners[2].y;
marker.c4.y = corners[3].y;
}
inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec) const
{
pose.position.x = tvec[0];
pose.position.y = tvec[1];
pose.position.z = tvec[2];
double angle = norm(rvec);
cv::Vec3d axis = rvec / angle;
tf2::Quaternion q;
q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
pose.orientation.w = q.w();
pose.orientation.x = q.x();
pose.orientation.y = q.y();
pose.orientation.z = q.z();
}
inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec) const
{
translation.x = tvec[0];
translation.y = tvec[1];
translation.z = tvec[2];
}
void pushVisMarkers(const std::string& frame_id, const ros::Time& stamp,
const geometry_msgs::Pose &pose, double length, int id, int index)
{
visualization_msgs::Marker marker;
marker.header.frame_id = frame_id;
marker.header.stamp = stamp;
marker.action = visualization_msgs::Marker::ADD;
marker.id = index;
// Marker
marker.ns = "aruco_marker";
marker.type = visualization_msgs::Marker::CUBE;
marker.scale.x = length;
marker.scale.y = length;
marker.scale.z = 0.001;
marker.color.r = 1;
marker.color.g = 1;
marker.color.b = 1;
marker.color.a = 0.9;
marker.pose = pose;
vis_array_.markers.push_back(marker);
// Label
marker.ns = "aruco_marker_label";
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.scale.z = length * 0.6;
marker.color.r = 0;
marker.color.g = 0;
marker.color.b = 0;
marker.color.a = 1;
marker.text = std::to_string(id);
marker.pose = pose;
vis_array_.markers.push_back(marker);
}
void snapOrientation(geometry_msgs::Pose& pose, const geometry_msgs::Quaternion orientation)
{
tf::Quaternion q;
q.setRPY(0, 0, -tf::getYaw(pose.orientation) + tf::getYaw(orientation) + M_PI / 2);
tf::Quaternion pq;
tf::quaternionMsgToTF(orientation, pq);
pq = pq * q;
tf::quaternionTFToMsg(pq, pose.orientation);
}
inline std::string getChildFrameId(int id) const
{
return frame_id_prefix_ + std::to_string(id);
}
};
PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet)

308
aruco_pose/src/aruco_map.cpp Normal file
View File

@@ -0,0 +1,308 @@
/*
* Detecting and pose estimation of ArUco markers maps
* Copyright (C) 2018 Copter Express Technologies
*
* Author: Oleg Kalachev <okalachev@gmail.com>
*
* Distributed under MIT License (available at https://opensource.org/licenses/MIT).
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*/
/*
* Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
* under the BSD license.
*/
#include <math.h>
#include <string>
#include <vector>
#include <fstream>
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/Image.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <aruco_pose/MarkerArray.h>
#include <aruco_pose/Marker.h>
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include "utils.h"
#include "gridboard.h"
using std::vector;
using cv::Mat;
class ArucoMap : public nodelet::Nodelet {
private:
ros::NodeHandle nh_, nh_priv_;
ros::Publisher img_pub_, pose_pub_;
ros::Subscriber markers_sub_, cinfo_sub;
cv::Ptr<cv::aruco::Board> board_;
Mat camera_matrix_, dist_coeffs_;
geometry_msgs::TransformStamped transform_;
geometry_msgs::PoseWithCovarianceStamped pose_;
tf2_ros::TransformBroadcaster br_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_{tf_buffer_};
visualization_msgs::MarkerArray vis_markers;
std::string snap_orientation_;
bool has_camera_info_ = false;
public:
virtual void onInit()
{
nh_ = getNodeHandle();
nh_priv_ = getPrivateNodeHandle();
image_transport::ImageTransport it_priv(nh_priv_);
// TODO: why image_transport doesn't work here?
img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
board_ = cv::makePtr<cv::aruco::Board>();
board_->dictionary = cv::aruco::getPredefinedDictionary(
static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
std::string type, map, map_name;
nh_priv_.param<std::string>("type", type, "map");
if (type == "map") {
param(nh_priv_, "map", map);
loadMap(map);
} else if (type == "gridboard") {
createGridBoard();
} else {
ROS_FATAL("aruco_map: unknown type: %s", type.c_str());
ros::shutdown();
}
nh_priv_.param<std::string>("name", map_name, "map");
nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
nh_priv_.param<std::string>("snap_orientation", snap_orientation_, "");
pose_pub_ = nh_priv_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 1);
// TODO: use synchronised subscriber
markers_sub_ = nh_.subscribe("markers", 1, &ArucoMap::markersCallback, this);
cinfo_sub = nh_.subscribe("camera_info", 1, &ArucoMap::cinfoCallback, this);
publishMapImage();
ROS_INFO("aruco_map: ready");
}
void markersCallback(const aruco_pose::MarkerArray& markers)
{
if (!has_camera_info_) return;
if (markers.markers.empty()) return;
int count = markers.markers.size();
std::vector<int> ids;
std::vector<std::vector<cv::Point2f>> corners;
ids.reserve(count);
corners.reserve(count);
for(auto const &marker : markers.markers) {
ids.push_back(marker.id);
std::vector<cv::Point2f> marker_corners = {
cv::Point2f(marker.c1.x, marker.c1.y),
cv::Point2f(marker.c2.x, marker.c2.y),
cv::Point2f(marker.c3.x, marker.c3.y),
cv::Point2f(marker.c4.x, marker.c4.y)
};
corners.push_back(marker_corners);
}
Mat obj_points, img_points;
cv::Vec3d rvec, tvec;
if (snap_orientation_.empty()) {
// simple estimation
int valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
rvec, tvec, false);
if (!valid) return;
transform_.header.stamp = markers.header.stamp;
transform_.header.frame_id = markers.header.frame_id;
pose_.header = transform_.header;
fillPose(pose_.pose.pose, rvec, tvec);
fillTransform(transform_.transform, rvec, tvec);
} else {
// estimation with "snapping"
cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
if (obj_points.empty()) return;
double center_x = 0, center_y = 0;
alignObjPointsToCenter(obj_points, center_x, center_y);
int res = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
if (!res) return;
fillTransform(transform_.transform, rvec, tvec);
try {
geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers.header.frame_id,
snap_orientation_, markers.header.stamp, ros::Duration(0.02));
snapOrientation(transform_.transform.rotation, snap_to.transform.rotation);
} catch (const tf2::TransformException& e) {
ROS_WARN_THROTTLE(1, "aruco_map: can't snap: %s", e.what());
}
geometry_msgs::TransformStamped shift;
shift.transform.translation.x = -center_x;
shift.transform.translation.y = -center_y;
shift.transform.rotation.w = 1;
tf2::doTransform(shift, transform_, transform_);
transform_.header.stamp = markers.header.stamp;
transform_.header.frame_id = markers.header.frame_id;
pose_.header = transform_.header;
transformToPose(transform_.transform, pose_.pose.pose);
}
if (!transform_.child_frame_id.empty()) {
br_.sendTransform(transform_);
}
pose_pub_.publish(pose_);
}
void cinfoCallback(const sensor_msgs::CameraInfoConstPtr& cinfo)
{
parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
has_camera_info_ = true;
}
void alignObjPointsToCenter(Mat &obj_points, double &center_x, double &center_y) const
{
// Align object points to the center of mass
double sum_x = 0;
double sum_y = 0;
for (int i = 0; i < obj_points.rows; i++) {
sum_x += obj_points.at<float>(i, 0);
sum_y += obj_points.at<float>(i, 1);
}
center_x = sum_x / obj_points.rows;
center_y = sum_y / obj_points.rows;
for (int i = 0; i < obj_points.rows; i++) {
obj_points.at<float>(i, 0) -= center_x;
obj_points.at<float>(i, 1) -= center_y;
}
}
void loadMap(std::string filename)
{
std::ifstream f(filename);
std::string line;
if (!f.good()) {
ROS_FATAL("aruco_map: %s - %s", strerror(errno), filename.c_str());
ros::shutdown();
}
while (std::getline(f, line)) {
int id;
double length, x, y, z, yaw, pitch, roll;
std::istringstream s(line);
if (!(s >> id >> length >> x >> y >> z >> yaw >> pitch >> roll)) {
ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str());
continue;
}
addMarker(id, length, x, y, z, yaw, pitch, roll);
}
ROS_INFO("aruco_map: loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
}
void createGridBoard()
{
ROS_INFO("aruco_map: generate gridboard");
ROS_WARN("aruco_map: gridboard maps are deprecated");
int markers_x, markers_y, first_marker;
double markers_side, markers_sep_x, markers_sep_y;
std::vector<int> marker_ids;
nh_priv_.param<int>("markers_x", markers_x, 10);
nh_priv_.param<int>("markers_y", markers_y, 10);
nh_priv_.param<int>("first_marker", first_marker, 0);
param(nh_priv_, "markers_side", markers_side);
param(nh_priv_, "markers_sep_x", markers_sep_x);
param(nh_priv_, "markers_sep_y", markers_sep_y);
if (nh_priv_.getParam("marker_ids", marker_ids)) {
if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
ros::shutdown();
}
} else {
// Fill marker_ids automatically
marker_ids.resize(markers_x * markers_y);
for (int i = 0; i < markers_x * markers_y; i++)
{
marker_ids.at(i) = first_marker++;
}
}
createCustomGridBoard(board_, markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, marker_ids);
}
void addMarker(int id, double length, double x, double y, double z,
double yaw, double pitch, double roll)
{
// Create transform
geometry_msgs::TransformStamped t;
t.transform.translation.x = x;
t.transform.translation.y = y;
t.transform.translation.z = z;
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
tf::quaternionTFToMsg(q, t.transform.rotation);
// TODO: process roll pitch yaw
vector<cv::Point3f> obj_points(4);
setMarkerObjectPoints(x, y, z, yaw, length, obj_points);
board_->ids.push_back(id);
board_->objPoints.push_back(obj_points);
}
void setMarkerObjectPoints(float x, float y, float z, float yaw, float length,
vector<cv::Point3f>& obj_points)
{
obj_points[0] = cv::Point3f(x - length / 2, y + length / 2, 0);
obj_points[1] = obj_points[0] + cv::Point3f(length, 0, 0);
obj_points[2] = obj_points[0] + cv::Point3f(length, -length, 0);
obj_points[3] = obj_points[0] + cv::Point3f(0, -length, 0);
}
void publishMapImage()
{
cv::Mat image;
cv_bridge::CvImage msg;
cv::aruco::drawPlanarBoard(board_, cv::Size(2000, 2000), image, 50, 1);
cv::cvtColor(image, image, CV_GRAY2BGR);
msg.encoding = sensor_msgs::image_encodings::BGR8;
msg.image = image;
img_pub_.publish(msg.toImageMsg());
}
};
PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet)

350
aruco_pose/src/aruco_pose.cpp
View File

@@ -1,350 +0,0 @@
#include <algorithm>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <pluginlib/class_list_macros.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_datatypes.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <stdio.h>
#include <tf/transform_broadcaster.h>
#include "util.h"
using std::vector;
using std::string;
namespace aruco_pose {
class ArucoPose : public nodelet::Nodelet {
tf::TransformBroadcaster br;
cv::Ptr<cv::aruco::Dictionary> dictionary;
cv::Ptr<cv::aruco::DetectorParameters> parameters;
cv::Ptr<cv::aruco::Board> board;
std::string frame_id_;
image_transport::CameraSubscriber img_sub;
image_transport::Publisher img_pub;
ros::Publisher marker_pub;
ros::Publisher pose_pub;
ros::NodeHandle nh_, nh_priv_;
virtual void onInit();
void createBoard();
cv::Point3f getObjPointsCenter(cv::Mat objPoints);
void detect(const sensor_msgs::ImageConstPtr&, const sensor_msgs::CameraInfoConstPtr&);
void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr&, cv::Mat&, cv::Mat&);
tf::Transform aruco2tf(cv::Mat rvec, cv::Mat tvec);
};
void ArucoPose::onInit() {
ROS_INFO("Initializing aruco_pose");
nh_ = getNodeHandle();
nh_priv_ = getPrivateNodeHandle();
nh_priv_.param("frame_id", frame_id_, std::string("aruco_map"));
dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_1000);
parameters = cv::aruco::DetectorParameters::create();
try
{
createBoard();
}
catch (const std::exception &exc)
{
std::cerr << exc.what();
exit(0);
}
image_transport::ImageTransport it(nh_);
img_sub = it.subscribeCamera("image", 1, &ArucoPose::detect, this);
image_transport::ImageTransport it_priv(nh_priv_);
img_pub = it_priv.advertise("debug", 1);
pose_pub = nh_priv_.advertise<geometry_msgs::PoseStamped>("pose", 1);
ROS_INFO("aruco_pose nodelet inited");
}
cv::Ptr<cv::aruco::Board> createCustomGridBoard(int markersX, int markersY, float markerLength, float markerSeparationX, float markerSeparationY,
const cv::Ptr<cv::aruco::Dictionary> &dictionary, std::vector<int> ids) {
CV_Assert(markersX > 0 && markersY > 0 && markerLength > 0 && markerSeparationX > 0 && markerSeparationY > 0);
cv::Ptr<cv::aruco::Board> res = cv::makePtr<cv::aruco::Board>();
res->dictionary = dictionary;
size_t totalMarkers = (size_t) markersX * markersY;
res->ids = ids;
res->objPoints.reserve(totalMarkers);
// calculate Board objPoints
float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparationY;
for(int y = 0; y < markersY; y++) {
for(int x = 0; x < markersX; x++) {
std::vector< cv::Point3f > corners;
corners.resize(4);
corners[0] = cv::Point3f(x * (markerLength + markerSeparationX),
maxY - y * (markerLength + markerSeparationY), 0);
corners[1] = corners[0] + cv::Point3f(markerLength, 0, 0);
corners[2] = corners[0] + cv::Point3f(markerLength, -markerLength, 0);
corners[3] = corners[0] + cv::Point3f(0, -markerLength, 0);
res->objPoints.push_back(corners);
}
}
return res;
}
cv::Ptr<cv::aruco::Board> createCustomBoard(std::map<string, string> markers, const cv::Ptr<cv::aruco::Dictionary> &dictionary) {
cv::Ptr<cv::aruco::Board> res = cv::makePtr<cv::aruco::Board>();
res->dictionary = dictionary;
size_t total_markers = markers.size();
res->ids.reserve(total_markers);
res->objPoints.reserve(total_markers);
// Generate ids and objPoints
for(auto const &marker : markers) {
res->ids.push_back(std::stoi(marker.first));
vector<string> parts;
parts = strSplit(marker.second, " ");
float size = std::stof(parts.at(0));
float x = std::stof(parts.at(1));
float y = std::stof(parts.at(2));
float z = std::stof(parts.at(3));
float yaw = std::stof(parts.at(4));
float pitch = std::stof(parts.at(5));
float roll = std::stof(parts.at(6));
vector<cv::Point3f> corners;
corners.resize(4);
corners[0] = cv::Point3f(x - size / 2, y + size / 2, 0);
corners[1] = corners[0] + cv::Point3f(size, 0, 0);
corners[2] = corners[0] + cv::Point3f(size, -size, 0);
corners[3] = corners[0] + cv::Point3f(0, -size, 0);
// TODO: process yaw, pitch, roll
res->objPoints.push_back(corners);
}
return res;
}
#include "fix.cpp"
void ArucoPose::createBoard()
{
static auto map_image_pub = nh_priv_.advertise<sensor_msgs::Image>("map_image", 1, true);
cv_bridge::CvImage map_image_msg;
cv::Mat map_image;
std::string type;
nh_priv_.param<std::string>("type", type, "gridboard");
if (type == "gridboard")
{
ROS_INFO("Initialize gridboard");
int markers_x, markers_y, first_marker;
float markers_side, markers_sep_x, markers_sep_y;
std::vector<int> marker_ids;
nh_priv_.param<int>("markers_x", markers_x, 10);
nh_priv_.param<int>("markers_y", markers_y, 10);
nh_priv_.param<int>("first_marker", first_marker, 0);
if (!nh_priv_.getParam("markers_side", markers_side))
{
ROS_ERROR("gridboard: required parameter ~markers_side is not set.");
exit(1);
}
if (!nh_priv_.getParam("markers_sep_x", markers_sep_x))
{
if (!nh_priv_.getParam("markers_sep", markers_sep_x))
{
ROS_ERROR("gridboard: ~markers_sep_x or ~markers_sep parameters are required");
exit(1);
}
}
if (!nh_priv_.getParam("markers_sep_y", markers_sep_y))
{
if (!nh_priv_.getParam("markers_sep", markers_sep_y))
{
ROS_ERROR("gridboard: ~markers_sep_y or ~markers_sep parameters are required");
exit(1);
}
}
if (nh_priv_.getParam("marker_ids", marker_ids))
{
if (markers_x * markers_y != marker_ids.size())
{
ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
exit(1);
}
}
else
{
// Fill marker_ids automatically
marker_ids.resize(markers_x * markers_y);
for(int i = 0; i < markers_x * markers_y; i++)
{
marker_ids.at(i) = first_marker++;
}
}
// Create grid board
board = createCustomGridBoard(markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, dictionary, marker_ids);
// Publish map image for debugging
_drawPlanarBoard(board, cv::Size(2000, 2000), map_image, 50, 1);
cv::cvtColor(map_image, map_image, CV_GRAY2BGR);
map_image_msg.encoding = sensor_msgs::image_encodings::BGR8;
map_image_msg.image = map_image;
map_image_pub.publish(map_image_msg.toImageMsg());
}
else if (type == "custom")
{
ROS_INFO("Initialize a custom board");
std::map<string, string> markers;
nh_priv_.getParam("markers", markers);
board = createCustomBoard(markers, dictionary);
ROS_INFO("Draw a custom board");
// Publish map image for debugging
_drawPlanarBoard(board, cv::Size(2000, 2000), map_image, 50, 1);
cv::cvtColor(map_image, map_image, CV_GRAY2BGR);
map_image_msg.encoding = sensor_msgs::image_encodings::BGR8;
map_image_msg.image = map_image;
map_image_pub.publish(map_image_msg.toImageMsg());
}
else
{
ROS_ERROR("Incorrect map type '%s'", type.c_str());
}
}
cv::Point3f ArucoPose::getObjPointsCenter(cv::Mat objPoints) {
float min_x = std::numeric_limits<float>::max();
float max_x = std::numeric_limits<float>::min();
float min_y = min_x, max_y = max_x;
for (int i = 0; i < objPoints.rows; i++) {
max_x = std::max(max_x, objPoints.at<float>(i, 0));
max_y = std::max(max_y, objPoints.at<float>(i, 1));
min_x = std::min(min_x, objPoints.at<float>(i, 0));
min_y = std::min(min_y, objPoints.at<float>(i, 1));
}
cv::Point3f res((min_x + max_x) / 2, (min_y + max_y) / 2, 0);
return res;
}
void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo) {
cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
std::vector<int> markerIds;
std::vector<std::vector<cv::Point2f>> markerCorners;
std::vector<std::vector<cv::Point2f>> rejectedCandidates;
cv::aruco::detectMarkers(image, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);
cv::Mat cameraMatrix(3, 3, CV_64F);
cv::Mat distCoeffs(8, 1, CV_64F);
parseCameraInfo(cinfo, cameraMatrix, distCoeffs);
int valid = 0;
cv::Mat rvec, tvec, objPoints;
if (markerIds.size() > 0) {
valid = _estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs,
rvec, tvec, false, objPoints);
if (valid) {
// Send map transform
tf::StampedTransform transform(aruco2tf(rvec, tvec), msg->header.stamp, cinfo->header.frame_id, frame_id_);
br.sendTransform(transform);
// Publish map pose
static geometry_msgs::PoseStamped ps;
ps.header.frame_id = frame_id_;
ps.header.stamp = msg->header.stamp;
ps.pose.orientation.w = 1;
pose_pub.publish(ps);
// Send reference point
cv::Point3f ref = getObjPointsCenter(objPoints);
tf::Vector3 ref_vector3 = tf::Vector3(ref.x, ref.y, ref.z);
tf::Quaternion q(0, 0, 0);
static tf::StampedTransform ref_transform;
ref_transform.stamp_ = msg->header.stamp;
ref_transform.frame_id_ = frame_id_;
ref_transform.child_frame_id_ = "aruco_map_reference";
ref_transform.setOrigin(ref_vector3);
ref_transform.setRotation(q);
br.sendTransform(ref_transform);
}
}
if (img_pub.getNumSubscribers() > 0)
{
cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds); // draw markers
if (valid)
{
cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.3); // draw board axis
}
cv_bridge::CvImage out_msg;
out_msg.header.frame_id = msg->header.frame_id;
out_msg.header.stamp = msg->header.stamp;
out_msg.encoding = sensor_msgs::image_encodings::BGR8;
out_msg.image = image;
img_pub.publish(out_msg.toImageMsg());
}
}
void ArucoPose::parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo, cv::Mat &cameraMat, cv::Mat &distCoeffs) {
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
cameraMat.at<double>(i, j) = cinfo->K[3 * i + j];
}
}
for (int k = 0; k < cinfo->D.size(); k++) {
distCoeffs.at<double>(k) = cinfo->D[k];
}
}
tf::Transform ArucoPose::aruco2tf(cv::Mat rvec, cv::Mat tvec) {
cv::Mat rot;
cv::Rodrigues(rvec, rot);
tf::Matrix3x3 tf_rot(rot.at<double>(0,0), rot.at<double>(0,1), rot.at<double>(0,2),
rot.at<double>(1,0), rot.at<double>(1,1), rot.at<double>(1,2),
rot.at<double>(2,0), rot.at<double>(2,1), rot.at<double>(2,2));
tf::Vector3 tf_orig(tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0));
return tf::Transform(tf_rot, tf_orig);
}
PLUGINLIB_EXPORT_CLASS(ArucoPose, nodelet::Nodelet)
}

145
aruco_pose/src/fix.cpp
View File

@@ -1,145 +0,0 @@
using namespace cv;
using namespace cv::aruco;
// Temporal fix!
// TODO: remove
// fix strange bug in our OpenCV version
void _getBoardObjectAndImagePoints(const Ptr<aruco::Board> &board, InputArrayOfArrays detectedCorners,
InputArray detectedIds, OutputArray objPoints, OutputArray imgPoints) {
CV_Assert(board->ids.size() == board->objPoints.size());
CV_Assert(detectedIds.total() == detectedCorners.total());
size_t nDetectedMarkers = detectedIds.total();
std::vector< Point3f > objPnts;
objPnts.reserve(nDetectedMarkers);
std::vector< Point2f > imgPnts;
imgPnts.reserve(nDetectedMarkers);
// look for detected markers that belong to the board and get their information
for(unsigned int i = 0; i < nDetectedMarkers; i++) {
int currentId = detectedIds.getMat().ptr< int >(0)[i];
for(unsigned int j = 0; j < board->ids.size(); j++) {
if(currentId == board->ids[j]) {
for(int p = 0; p < 4; p++) {
objPnts.push_back(board->objPoints[j][p]);
imgPnts.push_back(detectedCorners.getMat(i).ptr< Point2f >(0)[p]);
}
}
}
}
// create output
Mat(objPnts).copyTo(objPoints);
Mat(imgPnts).copyTo(imgPoints);
}
int _estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr<aruco::Board> &board,
InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec,
OutputArray _tvec, bool useExtrinsicGuess, Mat &objPoints) {
CV_Assert(_corners.total() == _ids.total());
// get object and image points for the solvePnP function
Mat /*objPoints, */imgPoints;
_getBoardObjectAndImagePoints(board, _corners, _ids, objPoints, imgPoints);
CV_Assert(imgPoints.total() == objPoints.total());
if(objPoints.total() == 0) // 0 of the detected markers in board
return 0;
// std::cout << "objPoints: " << objPoints << std::endl;
// std::cout << "imgPoints: " << imgPoints << std::endl;
solvePnP(objPoints, imgPoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess);
// divide by four since all the four corners are concatenated in the array for each marker
return (int)objPoints.total() / 4;
}
void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
int borderBits) {
CV_Assert(outSize.area() > 0);
CV_Assert(marginSize >= 0);
_img.create(outSize, CV_8UC1);
Mat out = _img.getMat();
out.setTo(Scalar::all(255));
out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
// calculate max and min values in XY plane
CV_Assert(_board->objPoints.size() > 0);
float minX, maxX, minY, maxY;
minX = maxX = _board->objPoints[0][0].x;
minY = maxY = _board->objPoints[0][0].y;
for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
for(int j = 0; j < 4; j++) {
minX = min(minX, _board->objPoints[i][j].x);
maxX = max(maxX, _board->objPoints[i][j].x);
minY = min(minY, _board->objPoints[i][j].y);
maxY = max(maxY, _board->objPoints[i][j].y);
}
}
float sizeX = maxX - minX;
float sizeY = maxY - minY;
// proportion transformations
float xReduction = sizeX / float(out.cols);
float yReduction = sizeY / float(out.rows);
// determine the zone where the markers are placed
if(xReduction > yReduction) {
int nRows = int(sizeY / xReduction);
int rowsMargins = (out.rows - nRows) / 2;
out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
} else {
int nCols = int(sizeX / yReduction);
int colsMargins = (out.cols - nCols) / 2;
out.adjustROI(0, 0, -colsMargins, -colsMargins);
}
// now paint each marker
Dictionary &dictionary = *(_board->dictionary);
Mat marker;
Point2f outCorners[3];
Point2f inCorners[3];
for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
// transform corners to markerZone coordinates
for(int j = 0; j < 3; j++) {
Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
// move top left to 0, 0
pf -= Point2f(minX, minY);
pf.x = pf.x / sizeX * float(out.cols);
pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
outCorners[j] = pf;
}
// get marker
Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
dictionary.drawMarker(_board->ids[m], dst_sz.width, marker, borderBits);
if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) {
// marker is aligned to image axes
marker.copyTo(out(Rect(outCorners[0], dst_sz)));
continue;
}
// interpolate tiny marker to marker position in markerZone
inCorners[0] = Point2f(-0.5f, -0.5f);
inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
// remove perspective
Mat transformation = getAffineTransform(inCorners, outCorners);
warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
BORDER_TRANSPARENT);
}
}

22
aruco_pose/src/gridboard.h Normal file
View File

@@ -0,0 +1,22 @@
void createCustomGridBoard(cv::Ptr<cv::aruco::Board>& board, int markersX, int markersY, float markerLength,
float markerSeparationX, float markerSeparationY, std::vector<int> ids)
{
size_t totalMarkers = (size_t) markersX * markersY;
board->ids = ids;
board->objPoints.reserve(totalMarkers);
// calculate Board objPoints
float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparationY;
for(int y = 0; y < markersY; y++) {
for(int x = 0; x < markersX; x++) {
std::vector< cv::Point3f > corners;
corners.resize(4);
corners[0] = cv::Point3f(x * (markerLength + markerSeparationX),
maxY - y * (markerLength + markerSeparationY), 0);
corners[1] = corners[0] + cv::Point3f(markerLength, 0, 0);
corners[2] = corners[0] + cv::Point3f(markerLength, -markerLength, 0);
corners[3] = corners[0] + cv::Point3f(0, -markerLength, 0);
board->objPoints.push_back(corners);
}
}
}

20
aruco_pose/src/util.h
View File

@@ -1,20 +0,0 @@
#pragma once
#include <vector>
#include <string>
std::vector<std::string> strSplit(const std::string& str, const std::string& delim)
{
std::vector<std::string> tokens;
size_t prev = 0, pos = 0;
do
{
pos = str.find(delim, prev);
if (pos == std::string::npos) pos = str.length();
std::string token = str.substr(prev, pos-prev);
if (!token.empty()) tokens.push_back(token);
prev = pos + delim.length();
}
while (pos < str.length() && prev < str.length());
return tokens;
}

109
aruco_pose/src/utils.h Normal file
View File

@@ -0,0 +1,109 @@
#pragma once
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Vector3.h>
#include <sensor_msgs/CameraInfo.h>
#include <opencv2/opencv.hpp>
// Read required param or shutdown the node
template<typename T>
static void param(ros::NodeHandle nh, const std::string& param_name, T& param_val)
{
if (!nh.getParam(param_name, param_val)) {
ROS_FATAL("Required param %s is not defined", param_name.c_str());
ros::shutdown();
}
}
static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo,
cv::Mat& matrix, cv::Mat& dist)
{
for (unsigned int i = 0; i < 3; ++i)
for (unsigned int j = 0; j < 3; ++j)
matrix.at<double>(i, j) = cinfo->K[3 * i + j];
for (unsigned int k = 0; k < cinfo->D.size(); k++)
dist.at<double>(k) = cinfo->D[k];
}
inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle)
{
float s = sin(angle);
float c = cos(angle);
// translate point back to origin:
p.x -= origin.x;
p.y -= origin.y;
// rotate point
float xnew = p.x * c - p.y * s;
float ynew = p.x * s + p.y * c;
// translate point back:
p.x = xnew + origin.x;
p.y = ynew + origin.y;
}
inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
{
pose.position.x = tvec[0];
pose.position.y = tvec[1];
pose.position.z = tvec[2];
double angle = norm(rvec);
cv::Vec3d axis = rvec / angle;
tf2::Quaternion q;
q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
pose.orientation.w = q.w();
pose.orientation.x = q.x();
pose.orientation.y = q.y();
pose.orientation.z = q.z();
}
inline void fillTransform(geometry_msgs::Transform& transform, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
{
transform.translation.x = tvec[0];
transform.translation.y = tvec[1];
transform.translation.z = tvec[2];
double angle = norm(rvec);
cv::Vec3d axis = rvec / angle;
tf2::Quaternion q;
q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
transform.rotation.w = q.w();
transform.rotation.x = q.x();
transform.rotation.y = q.y();
transform.rotation.z = q.z();
}
inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec)
{
translation.x = tvec[0];
translation.y = tvec[1];
translation.z = tvec[2];
}
inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from)
{
tf::Quaternion q;
q.setRPY(0, 0, -tf::getYaw(to) + tf::getYaw(from));
tf::Quaternion pq;
tf::quaternionMsgToTF(from, pq);
pq = pq * q;
tf::quaternionTFToMsg(pq, to);
}
inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)
{
pose.position.x = transform.translation.x;
pose.position.y = transform.translation.y;
pose.position.z = transform.translation.z;
pose.orientation = transform.rotation;
}