void GetMarkerPoses(IplImage *image, ARCloud &cloud) { //Detect and track the markers if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)) { printf("\n--------------------------\n\n"); for (size_t i=0; i<marker_detector.markers->size(); i++) { vector<cv::Point, Eigen::aligned_allocator<cv::Point> > pixels; Marker *m = &((*marker_detector.markers)[i]); int id = m->GetId(); cout << "******* ID: " << id << endl; int resol = m->GetRes(); int ori = m->ros_orientation; PointDouble pt1, pt2, pt3, pt4; pt4 = m->ros_marker_points_img[0]; pt3 = m->ros_marker_points_img[resol-1]; pt1 = m->ros_marker_points_img[(resol*resol)-resol]; pt2 = m->ros_marker_points_img[(resol*resol)-1]; m->ros_corners_3D[0] = cloud(pt1.x, pt1.y); m->ros_corners_3D[1] = cloud(pt2.x, pt2.y); m->ros_corners_3D[2] = cloud(pt3.x, pt3.y); m->ros_corners_3D[3] = cloud(pt4.x, pt4.y); if(ori >= 0 && ori < 4){ if(ori != 0){ std::rotate(m->ros_corners_3D.begin(), m->ros_corners_3D.begin() + ori, m->ros_corners_3D.end()); } } else ROS_ERROR("FindMarkerBundles: Bad Orientation: %i for ID: %i", ori, id); //Get the 3D marker points BOOST_FOREACH (const PointDouble& p, m->ros_marker_points_img) pixels.push_back(cv::Point(p.x, p.y)); ARCloud::Ptr selected_points = ata::filterCloud(cloud, pixels); //Use the kinect data to find a plane and pose for the marker int ret = PlaneFitPoseImprovement(i, m->ros_corners_3D, selected_points, cloud, m->pose); } }
int MarkerDetectorImpl::Detect(IplImage *image, Camera *cam, bool track, bool visualize, double max_new_marker_error, double max_track_error, LabelingMethod labeling_method, bool update_pose) { assert(image->origin == 0); // Currently only top-left origin supported double error=-1; // Swap marker tables _swap_marker_tables(); _markers_clear(); switch(labeling_method) { case CVSEQ : if(!labeling) labeling = new LabelingCvSeq(); ((LabelingCvSeq*)labeling)->SetOptions(detect_pose_grayscale); break; } labeling->SetCamera(cam); labeling->LabelSquares(image, visualize); vector<vector<PointDouble> >& blob_corners = labeling->blob_corners; IplImage* gray = labeling->gray; int orientation; // When tracking we find the best matching blob and test if it is near enough? if (track) { for (size_t ii=0; ii<_track_markers_size(); ii++) { Marker *mn = _track_markers_at(ii); if (mn->GetError(Marker::DECODE_ERROR|Marker::MARGIN_ERROR) > 0) continue; // We track only perfectly decoded markers int track_i=-1; int track_orientation=0; double track_error=1e200; for(unsigned i = 0; i < blob_corners.size()/*blobs_ret.size()*/; ++i) { if (blob_corners[i].empty()) continue; mn->CompareCorners(blob_corners[i], &orientation, &error); if (error < track_error) { track_i = i; track_orientation = orientation; track_error = error; } } if (track_error <= max_track_error) { mn->SetError(Marker::DECODE_ERROR, 0); mn->SetError(Marker::MARGIN_ERROR, 0); mn->SetError(Marker::TRACK_ERROR, track_error); mn->UpdatePose(blob_corners[track_i], cam, track_orientation, update_pose); _markers_push_back(mn); blob_corners[track_i].clear(); // We don't want to handle this again... if (visualize) mn->Visualize(image, cam, CV_RGB(255,255,0)); } } } // Now we go through the rest of the blobs -- in case there are new markers... for(size_t i = 0; i < blob_corners.size(); ++i) { if (blob_corners[i].empty()) continue; Marker *mn = new_M(edge_length, res, margin); if (mn->UpdateContent(blob_corners[i], gray, cam) && mn->DecodeContent(&orientation) && (mn->GetError(Marker::MARGIN_ERROR | Marker::DECODE_ERROR) <= max_new_marker_error)) { if (map_edge_length.find(mn->GetId()) != map_edge_length.end()) { mn->SetMarkerSize(map_edge_length[mn->GetId()], res, margin); } mn->UpdatePose(blob_corners[i], cam, orientation, update_pose); _markers_push_back(mn); if (visualize) mn->Visualize(image, cam, CV_RGB(255,0,0)); } delete mn; } return (int) _markers_size(); }
// 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, ARCloud &cloud) { for(int i=0; i<n_bundles; i++){ master_visible[i] = false; bundles_seen[i] = 0; } //Detect and track the markers if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)) { //printf("\n--------------------------\n\n"); for (size_t i=0; i<marker_detector.markers->size(); i++) { vector<cv::Point, Eigen::aligned_allocator<cv::Point> > pixels; Marker *m = &((*marker_detector.markers)[i]); int id = m->GetId(); //cout << "******* ID: " << id << endl; //Get the 3D points of the outer corners /* PointDouble corner0 = m->marker_corners_img[0]; PointDouble corner1 = m->marker_corners_img[1]; PointDouble corner2 = m->marker_corners_img[2]; PointDouble corner3 = m->marker_corners_img[3]; m->ros_corners_3D[0] = cloud(corner0.x, corner0.y); m->ros_corners_3D[1] = cloud(corner1.x, corner1.y); m->ros_corners_3D[2] = cloud(corner2.x, corner2.y); m->ros_corners_3D[3] = cloud(corner3.x, corner3.y); */ //Get the 3D inner corner points - more stable than outer corners that can "fall off" object int resol = m->GetRes(); int ori = m->ros_orientation; PointDouble pt1, pt2, pt3, pt4; pt4 = m->ros_marker_points_img[0]; pt3 = m->ros_marker_points_img[resol-1]; pt1 = m->ros_marker_points_img[(resol*resol)-resol]; pt2 = m->ros_marker_points_img[(resol*resol)-1]; m->ros_corners_3D[0] = cloud(pt1.x, pt1.y); m->ros_corners_3D[1] = cloud(pt2.x, pt2.y); m->ros_corners_3D[2] = cloud(pt3.x, pt3.y); m->ros_corners_3D[3] = cloud(pt4.x, pt4.y); if(ori >= 0 && ori < 4){ if(ori != 0){ std::rotate(m->ros_corners_3D.begin(), m->ros_corners_3D.begin() + ori, m->ros_corners_3D.end()); } } else ROS_ERROR("FindMarkerBundles: Bad Orientation: %i for ID: %i", ori, id); //Check if we have spotted a master tag int master_ind = -1; for(int j=0; j<n_bundles; j++){ if(id == master_id[j]) master_visible[j] = true; master_ind = j; } //Mark the bundle that marker belongs to as "seen" int bundle_ind = -1; for(int j=0; j<n_bundles; j++){ for(int k=0; k<bundle_indices[j].size(); k++){ if(bundle_indices[j][k] == id){ bundle_ind = j; bundles_seen[j] += 1; break; } } } //Get the 3D marker points BOOST_FOREACH (const PointDouble& p, m->ros_marker_points_img) pixels.push_back(cv::Point(p.x, p.y)); ARCloud::Ptr selected_points = ata::filterCloud(cloud, pixels); //Use the kinect data to find a plane and pose for the marker int ret = PlaneFitPoseImprovement(i, m->ros_corners_3D, selected_points, cloud, m->pose); //If the plane fit fails... if(ret < 0){ //Mark this tag as invalid m->valid = false; //If this was a master tag, reset its visibility if(master_ind >= 0) master_visible[master_ind] = false; //decrement the number of markers seen in this bundle bundles_seen[bundle_ind] -= 1; } else m->valid = true; } //For each master tag, infer the 3D position of its corners from other visible tags //Then, do a plane fit to those new corners ARCloud inferred_corners; for(int i=0; i<n_bundles; i++){ if(bundles_seen[i] > 0){ //if(master_visible[i] == false){ if(InferCorners(cloud, *(multi_marker_bundles[i]), inferred_corners) >= 0){ ARCloud::Ptr inferred_cloud(new ARCloud(inferred_corners)); PlaneFitPoseImprovement(i+5000, inferred_corners, inferred_cloud, cloud, bundlePoses[i]); } //} //If master is visible, use it directly instead of inferring pose //else{ // for (size_t j=0; j<marker_detector.markers->size(); j++){ // Marker *m = &((*marker_detector.markers)[j]); // if(m->GetId() == master_id[i]) // bundlePoses[i] = m->pose; // } //} Pose ret_pose; if(med_filt_size > 0){ med_filts[i]->addPose(bundlePoses[i]); med_filts[i]->getMedian(ret_pose); bundlePoses[i] = ret_pose; } } } } }