Skip to content
Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: sniekum/ar_track_alvar
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: indigo-devel
Choose a base ref
...
head repository: SV-ROS/ar_track_alvar
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: indigo-devel
Choose a head ref
Able to merge. These branches can be automatically merged.
  • 4 commits
  • 2 files changed
  • 1 contributor

Commits on May 15, 2016

  1. publish debug image

    girtslinde committed May 15, 2016
    Copy the full SHA
    20887dc View commit details
  2. Copy the full SHA
    045b1a5 View commit details
  3. Copy the full SHA
    58149d5 View commit details

Commits on May 21, 2016

  1. add tf_prefix param

    girtslinde committed May 21, 2016
    Copy the full SHA
    d7f9464 View commit details
Showing with 55 additions and 11 deletions.
  1. +32 −11 nodes/FindMarkerBundlesNoKinect.cpp
  2. +23 −0 src/MultiMarker.cpp
43 changes: 32 additions & 11 deletions nodes/FindMarkerBundlesNoKinect.cpp
Original file line number Diff line number Diff line change
@@ -57,6 +57,9 @@ using namespace std;
Camera *cam;
cv_bridge::CvImagePtr cv_ptr_;
image_transport::Subscriber cam_sub_;

image_transport::Publisher debug_pub_;

ros::Publisher arMarkerPub_;
ros::Publisher rvizMarkerPub_;
ar_track_alvar_msgs::AlvarMarkers arPoseMarkers_;
@@ -76,26 +79,27 @@ double max_track_error;
std::string cam_image_topic;
std::string cam_info_topic;
std::string output_frame;
std::string tf_prefix;
int n_bundles = 0;

void GetMultiMarkerPoses(IplImage *image);
void GetMultiMarkerPoses(IplImage *image, bool visualize);
void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg);
void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_msg, tf::StampedTransform &CamToOutput, visualization_msgs::Marker *rvizMarker, ar_track_alvar_msgs::AlvarMarker *ar_pose_marker);


// Updates the bundlePoses of the multi_marker_bundles by detecting markers and using all markers in a bundle to infer the master tag's position
void GetMultiMarkerPoses(IplImage *image) {
void GetMultiMarkerPoses(IplImage *image, bool visualize) {

if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)){
if (marker_detector.Detect(image, cam, true, visualize, max_new_marker_error, max_track_error, CVSEQ, true)){
for(int i=0; i<n_bundles; i++)
multi_marker_bundles[i]->Update(marker_detector.markers, cam, bundlePoses[i]);

if(marker_detector.DetectAdditional(image, cam, false) > 0){
for(int i=0; i<n_bundles; i++){
if ((multi_marker_bundles[i]->SetTrackMarkers(marker_detector, cam, bundlePoses[i], image) > 0))
multi_marker_bundles[i]->Update(marker_detector.markers, cam, bundlePoses[i]);
}
}
// if(marker_detector.DetectAdditional(image, cam, visualize) > 0){
// for(int i=0; i<n_bundles; i++){
// if ((multi_marker_bundles[i]->SetTrackMarkers(marker_detector, cam, bundlePoses[i], image) > 0))
// multi_marker_bundles[i]->Update(marker_detector.markers, cam, bundlePoses[i]);
// }
// }
}
}

@@ -123,7 +127,11 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_

//Publish the cam to marker transform for main marker in each bundle
if(type==MAIN_MARKER){
std::string markerFrame = "ar_marker_";
std::string markerFrame = "/ar_marker_";
if (!tf_prefix.empty()) {
// http://wiki.ros.org/geometry/CoordinateFrameConventions
markerFrame = "/" + tf_prefix + markerFrame;
}
std::stringstream out;
out << id;
std::string id_string = out.str();
@@ -219,7 +227,12 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
// us a cv::Mat. I'm too lazy to change to cv::Mat throughout right now, so I
// do this conversion here -jbinney
IplImage ipl_image = cv_ptr_->image;
GetMultiMarkerPoses(&ipl_image);

bool visualize = (debug_pub_.getNumSubscribers() > 0);

GetMultiMarkerPoses(&ipl_image, visualize);

debug_pub_.publish(cv_ptr_->toImageMsg());

//Draw the observed markers that are visible and note which bundles have at least 1 marker seen
for(int i=0; i<n_bundles; i++)
@@ -281,6 +294,12 @@ int main(int argc, char *argv[])
ros::init (argc, argv, "marker_detect");
ros::NodeHandle n;

string tf_prefix_param;
if (n.searchParam("tf_prefix", tf_prefix_param))
n.getParam(tf_prefix_param, tf_prefix);
ROS_INFO("tf_prefix_param:%s", tf_prefix_param.c_str());
ROS_INFO("tf_prefix:%s", tf_prefix.c_str());

if(argc < 8){
std::cout << std::endl;
cout << "Not enough arguments provided." << endl;
@@ -339,6 +358,8 @@ int main(int argc, char *argv[])
image_transport::ImageTransport it_(n);
cam_sub_ = it_.subscribe (cam_image_topic, 1, &getCapCallback);

debug_pub_ = it_.advertise("ar_debug_img", 1);

ros::spin();

return 0;
23 changes: 23 additions & 0 deletions src/MultiMarker.cpp
Original file line number Diff line number Diff line change
@@ -292,6 +292,24 @@ bool MultiMarker::IsValidMarker(int marker_id) {
return idx != -1 && marker_status[idx] != 0;
}

double crossProduct(const PointDouble &a, const PointDouble &b) {
return a.x * b.y - a.y * b.x;
}

/*
* In an upper-left origin image clockwise is positive
*/
double calcOrientation(const std::vector<PointDouble> &points) {
if (points.size() < 3)
return 0.0;

double sum = crossProduct(points[points.size()-1], points[0]);
for(size_t j = 0; j < points.size()-1; ++j) {
sum += crossProduct(points[j], points[j+1]);
}

return sum;
}

double MultiMarker::_GetPose(MarkerIterator &begin, MarkerIterator &end, Camera* cam, Pose& pose, IplImage* image) {
vector<CvPoint3D64f> world_points;
@@ -312,6 +330,11 @@ double MultiMarker::_GetPose(MarkerIterator &begin, MarkerIterator &end, Camera*

// But only if we have corresponding points in the pointcloud
if (marker_status[index] > 0) {
// Ignore markers facing away from camera - with corners in clockwise order.
// We cannot be seeing those.
if (calcOrientation(marker->marker_corners_img) > 0.0)
continue;

for(size_t j = 0; j < marker->marker_corners.size(); ++j)
{
CvPoint3D64f Xnew = pointcloud[pointcloud_index(id, (int)j)];