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;
}
}
}
}
}
