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

Merge branch 'BoardEstimation2'

parents 9e142672 26cddade
......@@ -26,8 +26,10 @@ namespace aruco_tracking {
cv::Ptr <cv::aruco::DetectorParameters> detectorParams_;
cv::Ptr <cv::aruco::Dictionary> dictionary_;
double marker_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);
......
#include <pluginlib/class_list_macros.h>
#include <opencv2/calib3d.hpp>
#include <aruco_tracking_nodelet.h>
#include <aruco_tracking/ArucoMarkers.h>
#include <tf2/LinearMath/Quaternion.h>
......@@ -36,6 +37,54 @@ namespace aruco_tracking {
marker_pub_ = nh.advertise<aruco_tracking::ArucoMarkers>(nh.resolveName("markers"), 1);
dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionary_id));
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)));
}
}
//get camera params and subscribe to the image
......@@ -63,13 +112,28 @@ namespace aruco_tracking {
std::vector<int> ids;
std::vector <std::vector<cv::Point2f>> corners, rejected;
cv::aruco::detectMarkers(cv_ptr->image, dictionary_, corners, ids, detectorParams_, rejected);
cv::aruco::drawDetectedMarkers(cv_ptr->image, corners, ids);
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;
}
// compute marker poses
std::vector <cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(corners, marker_side_length_, 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;
......
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