Commit 3fc8ae07 authored by julian.leichert@neura-robotics.com's avatar julian.leichert@neura-robotics.com
Browse files

Merge branch 'master' into BoardEstimation

parents 3a12eb4c 15ecf5dc
......@@ -33,7 +33,8 @@ find_package(OpenCV REQUIRED)
catkin_package(
INCLUDE_DIRS include
)
add_library(aruco_tracking_nodelet src/aruco_tracking_nodelet.cpp)
add_library(aruco_tracking_nodelet src/aruco_tracking_nodelet.cpp src/tracking_eval_nodelet.cpp)
target_link_libraries(aruco_tracking_nodelet ${catkin_LIBRARIES} ${OpenCV_LIBS} ${OpenCV_LIBRARIES})
add_dependencies(aruco_tracking_nodelet ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_dependencies(aruco_tracking_nodelet ${PROJECT_NAME}_gencfg)
......
......@@ -4,4 +4,9 @@
base_class_type="nodelet::Nodelet">
<description>pose estimate</description>
</class>
<class name="aruco_tracking/EvalNodelet"
type="aruco_tracking::EvalNodelet"
base_class_type="nodelet::Nodelet">
<description>tracking evaluation</description>
</class>
</library>
......@@ -5,12 +5,11 @@ marker_side_length: 0.0375
half_cube_side_length: &C 0.02
neg_half_cube_side_length: &CI -0.02
world:
markers:
boards:
world:
"0": [0,0,0, 0,0,0]
cube:
markers:
cube:
"1": [ 0, *CI, 0, 1.20919958, -1.20919958, 1.20919958 ]
"2": [ *C, 0, 0, 1.20919958, 1.20919958, 1.20919958 ]
"3": [ 0, *C, 0, 0, 2.22144147, 2.22144147 ]
......
......@@ -8,15 +8,14 @@ neg_half_cube_side_length: &CI -0.022
half_board_side_length: &B 0.15
neg_half_board_side_length: &BI -0.15
world:
markers:
boards:
world:
"0": [*BI, *BI, 0, 0,0,3.1415]
"6": [*BI, *B, 0, 0,0,3.1415]
"7": [*B, *B, 0, 0,0,3.1415]
"8": [*B, *BI, 0, 0,0,3.1415]
cube:
markers:
cube:
"1": [ 0, *CI, 0, 1.20919958, -1.20919958, 1.20919958 ]
"2": [ *C, 0, 0, 1.20919958, 1.20919958, 1.20919958 ]
"3": [ 0, *C, 0, 0, 2.22144147, 2.22144147 ]
......
tracker_names: ["camhs4", "camhs6", "cube"]
\ No newline at end of file
......@@ -36,8 +36,10 @@ namespace aruco_tracking {
dynamic_reconfigure::Server<aruco_tracking::ArucoMarkerConfig> server;
double marker_side_length_;
double half_board_side_length_;
std::map<std::string, cv::Ptr<cv::aruco::Board>> boards_;
void onInit() override;
void createBoards(ros::NodeHandle nh);
void infoCB(const sensor_msgs::CameraInfoConstPtr &info_msg);
......
#pragma once
#include <vector>
//ros
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
//messages
#include <geometry_msgs/TransformStamped.h>
#include <tf2_msgs/TFMessage.h>
#include <std_msgs/Float64.h>
namespace aruco_tracking {
class EvalNodelet : public nodelet::Nodelet {
private:
ros::Subscriber pos_sub_;
std::vector<ros::Publisher> jitter_pos_pubs_ = std::vector<ros::Publisher>();
std::vector<ros::Publisher> jitter_ort_pubs_ = std::vector<ros::Publisher>();
std::vector<ros::Publisher> variation_pos_pubs_ = std::vector<ros::Publisher>();
std::vector<ros::Publisher> variation_ort_pubs_ = std::vector<ros::Publisher>();
std::vector<std::string> tracker_names;
const size_t history_size = 50;
tf2::Vector3 last_pos;
tf2::Vector3 last_slope;
std::vector<double> angle_history;
size_t angle_history_index = 0;
tf2::Quaternion last_ort;
tf2::Quaternion last_ort_slope;
std::vector<double> angle_ort_history;
size_t angle_ort_history_index = 0;
std::vector<tf2::Vector3> pos_history;
tf2::Vector3 history_sum;
size_t pos_history_index = 0;
std::vector<tf2::Quaternion> ort_history;
size_t ort_history_index = 0;
void onInit() override;
void posCB(const tf2_msgs::TFMessageConstPtr &pos_msg);
std_msgs::Float64 jitter_pos(const tf2::Vector3 &pos);
std_msgs::Float64 jitter_ort(const tf2::Quaternion &ort);
std_msgs::Float64 variation_pos(const tf2::Vector3 &pos);
std_msgs::Float64 variation_ort(const tf2::Quaternion &ort);
};
}
......@@ -3,9 +3,13 @@
<launch>
<arg name="rviz_config" default="$(find aruco_tracking)/launch/aruco_config.rviz" />
<arg name="aruco_params" default="$(find aruco_tracking)/config/aruco_params_extended.yaml" />
<arg name="bag" default="" />
<!-- RVIZ -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rviz_config)" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rviz_config)" required="true" />
<!-- RQT-Plot -->
<node name="rqt_plot" pkg="rqt_plot" type="rqt_plot" args="/aruco_tracking/jitter_pos_cube /aruco_tracking/jitter_ort_cube /aruco_tracking/variation_pos_cube /aruco_tracking/variation_ort_cube"/>
<group ns="aruco_tracking">
<arg name="manager_name" value="aruco_manager" />
......@@ -16,7 +20,14 @@
<!-- Launch nodelet manager -->
<node name="$(arg manager_name)" pkg="nodelet" type="nodelet" args="manager" output="screen" />
<!-- <load nodelets -->
<!-- Image rectification via nodelets -->
<include file="$(find image_proc)/launch/image_proc.launch" ns="/camhs4">
<arg name="manager" value="/aruco_tracking/$(arg manager_name)" />
</include>
<include file="$(find image_proc)/launch/image_proc.launch" ns="/camhs6">
<arg name="manager" value="/aruco_tracking/$(arg manager_name)" />
</include>
<!-- Load nodelets -->
<node name="camhs4" pkg="nodelet" type="nodelet" args="load aruco_tracking/Nodelet $(arg manager_name)" output="screen">
<rosparam file="$(find aruco_tracking)/config/aruco_tracking_params_camhs4.yaml" command="load" />
</node>
......@@ -24,9 +35,19 @@
<rosparam file="$(find aruco_tracking)/config/aruco_tracking_params_camhs6.yaml" command="load" />
</node>
<node name="tracking_eval" pkg="nodelet" type="nodelet" args="load aruco_tracking/EvalNodelet $(arg manager_name)" output="screen">
<rosparam file="$(find aruco_tracking)/config/tracking_eval.yaml" command="load"/>
</node>
<node name="tracking" pkg="aruco_tracking" type="track_boards.py" output="screen" />
</group>
<node name="image_proc_hs4" pkg="image_proc" type="image_proc" ns="camhs4"/>
<node name="image_proc_hs6" pkg="image_proc" type="image_proc" ns="camhs6"/>
<!-- Either start rosbag play (if argument 'bag' is present) -->
<node if="$(eval bool(bag))" name="rosbag_play" pkg="rosbag" type="play" args="-l $(arg bag)" />
<!-- or start real cameras -->
<group if="$(eval not bag)">
<include file="$(dirname)/usb_cam_camhs4.launch" />
<include file="$(dirname)/usb_cam_camhs6.launch" />
</group>
</launch>
\ No newline at end of file
......@@ -7,7 +7,6 @@ import rospy
import tf2_ros
import matplotlib.pyplot as plt
from tf import transformations as tf
from tf2_ros import transform_broadcaster
from geometry_msgs.msg import Pose, TransformStamped, Vector3, Quaternion
from std_msgs.msg import ColorRGBA
from visualization_msgs.msg import Marker
......@@ -31,7 +30,7 @@ def qp_from_matrix(T):
def matrix_from_qp(qp):
"""Retrieve matrix from (quaterniion, position) tuple"""
"""Retrieve matrix from (quaternion, position) tuple"""
q, p = qp
T = tf.quaternion_matrix(q)
T[0:3, 3] = p
......@@ -54,7 +53,7 @@ class BoardTracker(object):
self.name = name
self.markers = dict()
self.T = None
markers = rospy.get_param('{name}/markers'.format(name=name), dict())
markers = rospy.get_param(f'{name}/markers', dict())
# convert transform from position, axis*angle representation to (inverse) 4x4 matrix
for m in markers.keys():
pos = numpy.array(markers[m][:3])
......@@ -114,6 +113,7 @@ class CameraTrackers(BoardTracker):
class Tracking(object):
def __init__(self, marker_list, reference_frame='world'):
self.world = CameraTrackers(reference_frame)
self.boards = [BoardTracker(name) for name in marker_list]
self.board_markers = dict() # store marker transforms received from individual cameras
self.marker_sub = rospy.Subscriber('markers', ArucoMarkers, self.marker_cb)
......@@ -160,8 +160,10 @@ class Tracking(object):
t.header.stamp = stamp
t.header.frame_id = parent
t.child_frame_id = child
t.transform.translation = Vector3(*T[0:3, 3])
t.transform.rotation = Quaternion(*tf.quaternion_from_matrix(T))
vec = T[0:3, 3]
t.transform.translation = Vector3(*vec)
quart = tf.quaternion_from_matrix(T)
t.transform.rotation = Quaternion(*quart)
self.tf_broadcaster.sendTransform(t)
......
#include <pluginlib/class_list_macros.h>
#include <opencv2/calib3d.hpp>
#include <aruco_tracking_nodelet.h>
#include <aruco_tracking/ArucoMarkers.h>
#include <opencv2/calib3d.hpp>
......@@ -47,6 +48,54 @@ namespace aruco_tracking {
f = boost::bind(&Nodelet::ReconfigureCallback,this, _1, _2);
server.setCallback(f);
try {
createBoards(nh);
} catch (const std::exception &e) {
std::cerr << e.what() << std::endl;
throw;
}
}
float extractFloat(XmlRpc::XmlRpcValue& v) {
if (v.getType() == XmlRpc::XmlRpcValue::TypeDouble)
return static_cast<double>(v);
else if (v.getType() == XmlRpc::XmlRpcValue::TypeInt)
return static_cast<int>(v);
else
return 0.0;
}
void Nodelet::createBoards(ros::NodeHandle nh) {
const float s = marker_side_length_ / 2.;
cv::Matx44f corners({-s, s, 0, 1, // x y z 1
s, s, 0, 1,
s, -s, 0, 1,
-s, -s, 0, 1});
XmlRpc::XmlRpcValue boards;
nh.getParam("boards", boards); // read board definitions from ROS parameter server
// iterate over all boards
for (auto bit=boards.begin(), bend=boards.end(); bit != bend; ++bit) {
const std::string &name = bit->first;
auto &markers = bit->second;
std::vector<int> marker_ids;
std::vector<cv::Mat> marker_points;
// iterate over all markers of the board
for (auto mit=markers.begin(), mend=markers.end(); mit != mend; ++mit) {
marker_ids.push_back(std::stoi(mit->first));
auto &vec = mit->second;
cv::Vec3f tvec { extractFloat(vec[0]), extractFloat(vec[1]), extractFloat(vec[2]) };
cv::Vec3f rvec { extractFloat(vec[3]), extractFloat(vec[4]), extractFloat(vec[5]) };
cv::Mat T;
cv::Rodrigues(rvec, T);
cv::hconcat(T, tvec, T);
marker_points.push_back(T * corners.t());
}
boards_.insert(std::make_pair(name, cv::aruco::Board::create(marker_points, dictionary_, marker_ids)));
}
}
void Nodelet::estimatePoseSingleMarkers(cv::InputArrayOfArrays _corners, float markerLength,
......@@ -127,8 +176,23 @@ namespace aruco_tracking {
std::vector<std::vector<cv::Point2f>> cubeBoardCorners;
std::vector<std::vector<cv::Point2f>> gridBoardCorners;
std::vector <std::vector<cv::Point2f>> corners, rejected;
cv::aruco::detectMarkers(cv_ptr->image, dictionary_, corners, ids, m_detector_params, rejected);
cv::aruco::drawDetectedMarkers(cv_ptr->image, corners, ids);
cv::aruco::detectMarkers(cv_ptr->image, dictionary_, corners, ids, detectorParams_, rejected);
cv::aruco::drawDetectedMarkers(cv_ptr->image, corners, ids); // debug image
// estimate board pose(s)
cv::Vec3d rvec, tvec;
for (const auto& pair : boards_) {
try {
std::cerr << cam_id_ << ":" << pair.first << ": ";
if (cv::aruco::estimatePoseBoard(corners, ids, pair.second, camMatrix_, distCoeffs_, rvec, tvec))
std::cerr << tvec << " " << rvec;
else
std::cerr << "failure";
} catch (const std::exception &e) {
std::cerr << e.what();
}
std::cerr << std::endl;
}
for(int k =0;k<ids.size();k++){
if(ids[k]==0 || ids[k]==6 ||ids[k]==7 || ids[k]==8 ){
......@@ -199,7 +263,7 @@ namespace aruco_tracking {
cv::aruco::estimatePoseBoard(cubeBoardCorners,cubeBoardIds,board,camMatrix_,distCoeffs_,rvecs,tvecs);
}
// process all individual markers and publish their pose
// process all boards and publish their pose w.r.t. the camera
ArucoMarkers markers;
markers.header.frame_id = cam_id_;
markers.header.stamp = ros::Time::now(); // cv_ptr->header.stamp;
......
#include <pluginlib/class_list_macros.h>
#include <tracking_eval_nodelet.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
namespace aruco_tracking {
void EvalNodelet::onInit() {
ros::NodeHandle nh = getNodeHandle();
ros::NodeHandle np = getPrivateNodeHandle();
if (!np.getParam("tracker_names", tracker_names)) {
ROS_WARN(" Failed to fetch tracker names to evaluate.");
return;
}
pos_sub_ = nh.subscribe(
"/tf",
1,
&EvalNodelet::posCB,
this);
for (const auto &item: tracker_names) {
jitter_pos_pubs_.push_back(nh.advertise<std_msgs::Float64>("jitter_pos_" + item, 1));
jitter_ort_pubs_.push_back(nh.advertise<std_msgs::Float64>("jitter_ort_" + item, 1));
variation_pos_pubs_.push_back(nh.advertise<std_msgs::Float64>("variation_pos_" + item, 1));
variation_ort_pubs_.push_back(nh.advertise<std_msgs::Float64>("variation_ort_" + item, 1));
}
angle_history = std::vector<double>(history_size);
last_pos = tf2::Vector3(1, 1, 1);
last_slope = tf2::Vector3(1, 1, 1);
angle_ort_history = std::vector<double>(history_size);
last_ort = tf2::Quaternion(1, 0, 0, 0);
last_ort_slope = tf2::Quaternion(1, 0, 0, 0);
pos_history = std::vector<tf2::Vector3>(history_size);
history_sum = tf2::Vector3(0, 0, 0);
ort_history = std::vector<tf2::Quaternion>(history_size);
}
void EvalNodelet::posCB(const tf2_msgs::TFMessageConstPtr &pos_msg) {
const std::vector<geometry_msgs::TransformStamped> transforms = pos_msg.get()->transforms;
for (const auto & transform : transforms) {
for (size_t i = 0; i < tracker_names.size(); ++i) {
if (transform.child_frame_id == tracker_names[i]) {
tf2::Vector3 pos;
tf2::Quaternion ort;
tf2::convert(transform.transform.translation, pos);
tf2::convert(transform.transform.rotation, ort);
jitter_pos_pubs_[i].publish(jitter_pos(pos));
jitter_ort_pubs_[i].publish(jitter_ort(ort));
variation_pos_pubs_[i].publish(variation_pos(pos));
variation_ort_pubs_[i].publish(variation_ort(ort));
}
}
}
}
std_msgs::Float64 EvalNodelet::jitter_pos(const tf2::Vector3 &pos) {
tf2::Vector3 slope = pos - last_pos;
angle_history[angle_history_index] = slope.angle(last_slope);
last_pos = pos;
last_slope = slope;
angle_history_index = (angle_history_index + 1) % history_size;
double average = 0;
for (const auto &item: angle_history) {
average += item;
}
average /= (double) history_size;
std_msgs::Float64 msg;
msg.data = average;
return msg;
}
std_msgs::Float64 EvalNodelet::jitter_ort(const tf2::Quaternion &ort) {
tf2::Quaternion slope = ort - last_ort;
angle_ort_history[angle_ort_history_index] = slope.angleShortestPath(last_ort_slope);
last_ort = ort;
last_ort_slope = slope;
angle_ort_history_index = (angle_ort_history_index + 1) % history_size;
double average = 0;
for (const auto &item: angle_ort_history) {
average += item;
}
average /= (double) history_size;
std_msgs::Float64 msg;
msg.data = average;
return msg;
}
std_msgs::Float64 EvalNodelet::variation_pos(const tf2::Vector3 &pos) {
history_sum -= pos_history[pos_history_index];
history_sum += pos;
pos_history[pos_history_index] = pos;
pos_history_index = (pos_history_index + 1) % history_size;
tf2::Vector3 avg_pos = history_sum / (double) history_size;
double average = 0;
for (const auto &item: pos_history) {
tf2::Vector3 delta_pos = item - avg_pos;
average += delta_pos.length();
}
average /= (double) history_size;
std_msgs::Float64 msg;
msg.data = average;
return msg;
}
std_msgs::Float64 EvalNodelet::variation_ort(const tf2::Quaternion &ort) {
ort_history[ort_history_index] = ort;
ort_history_index = (ort_history_index + 1) % history_size;
//calculate quaternion avg
tf2::Quaternion avg = ort_history[0];
for (size_t i = 1; i < history_size; ++i) {
avg = avg.slerp(ort_history[i], 1.0/((double)(i + 1)));
}
double average = 0;
for (const auto &item : ort_history) {
average += item.angleShortestPath(avg);
}
average /= (double) history_size;
std_msgs::Float64 msg;
msg.data = average;
return msg;
}
}
PLUGINLIB_EXPORT_CLASS(aruco_tracking::EvalNodelet, nodelet::Nodelet)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment