int MarkerDetectorImpl::DetectAdditional(IplImage *image, Camera *cam, bool visualize, double max_track_error)
{
assert(image->origin == 0); // Currently only top-left origin supported
if(!labeling) return -1;
double error=-1;
int orientation;
int count=0;
vector<vector<PointDouble> >& blob_corners = labeling->blob_corners;
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(); ++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);
_markers_push_back(mn);
count++;
blob_corners[track_i].clear(); // We don't want to handle this again...
if (visualize) {
mn->Visualize(image, cam, CV_RGB(0,255,255));
}
}
}
return count;
}
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();
}
