// 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) {
if (marker_detector.Detect(image, cam, true, false, 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]);
}
}
}
}
double GetMultiMarkerPose(IplImage *image, Pose &pose) {
static bool init=true;
if (init) {
init=false;
vector<int> id_vector;
for(int i = 0; i < nof_markers; ++i)
id_vector.push_back(i);
// We make the initialization for MultiMarkerBundle using MultiMarkerInitializer
// Each marker needs to be visible in at least two images and at most 64 image are used.
multi_marker_init = new MultiMarkerInitializer(id_vector, 2, 64);
pose.Reset();
multi_marker_init->PointCloudAdd(id_vector[0], marker_size, pose);
multi_marker_bundle = new MultiMarkerBundle(id_vector);
marker_detector.SetMarkerSize(marker_size);
}
double error = -1;
if (!optimize_done) {
if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)) {
error = multi_marker_init->Update(marker_detector.markers, cam, pose);
}
}
else {
if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)) {
error = multi_marker_bundle->Update(marker_detector.markers, cam, pose);
if ((multi_marker_bundle->SetTrackMarkers(marker_detector, cam, pose, image) > 0) && (marker_detector.DetectAdditional(image, cam, false) > 0))
{
error = multi_marker_bundle->Update(marker_detector.markers, cam, pose);
}
}
}
if (add_measurement) {
cout << "Markers seen: " << marker_detector.markers->size() << "\n";
if (marker_detector.markers->size() >= 2) {
cout<<"Adding measurement..."<<endl;
multi_marker_init->MeasurementsAdd(marker_detector.markers);
}
else{
cout << "Not enough markers to capture measurement\n";
}
add_measurement=false;
}
if (optimize) {
cout<<"Initializing..."<<endl;
if (!multi_marker_init->Initialize(cam)) {
cout<<"Initialization failed, add some more measurements."<<endl;
} else {
// Reset the bundle adjuster.
multi_marker_bundle->Reset();
multi_marker_bundle->MeasurementsReset();
// Copy all measurements into the bundle adjuster.
for (int i = 0; i < multi_marker_init->getMeasurementCount(); ++i) {
Pose p2;
multi_marker_init->getMeasurementPose(i, cam, p2);
const std::vector<MultiMarkerInitializer::MarkerMeasurement, Eigen::aligned_allocator<MultiMarkerInitializer::MarkerMeasurement> > markers = multi_marker_init->getMeasurementMarkers(i);
multi_marker_bundle->MeasurementsAdd(&markers, p2);
}
// Initialize the bundle adjuster with initial marker poses.
multi_marker_bundle->PointCloudCopy(multi_marker_init);
cout<<"Optimizing..."<<endl;
if (multi_marker_bundle->Optimize(cam, 0.01, 20)) {
cout<<"Optimizing done"<<endl;
optimize_done=true;
} else {
cout<<"Optimizing FAILED!"<<endl;
}
}
optimize=false;
}
return error;
}
void videocallback(IplImage *image)
{
static Camera cam;
Pose pose;
bool flip_image = (image->origin?true:false);
if (flip_image) {
cvFlip(image);
image->origin = !image->origin;
}
static bool init = true;
if (init)
{
init = false;
// Initialize camera
cout<<"Loading calibration: "<<calibrationFilename.str();
if (cam.SetCalib(calibrationFilename.str().c_str(), image->width, image->height))
{
cout<<" [Ok]"<<endl;
}
else
{
cam.SetRes(image->width, image->height);
cout<<" [Fail]"<<endl;
}
vector<int> id_vector;
for(int i = 0; i < nof_markers; ++i)
id_vector.push_back(i);
// We make the initialization for MultiMarkerBundle using a fixed marker field (can be printed from ALVAR.pdf)
marker_detector.SetMarkerSize(marker_size);
marker_detector.SetMarkerSizeForId(0, marker_size*2);
multi_marker = new MultiMarker(id_vector);
pose.Reset();
multi_marker->PointCloudAdd(0, marker_size*2, pose);
pose.SetTranslation(-marker_size*2.5, +marker_size*1.5, 0);
multi_marker->PointCloudAdd(1, marker_size, pose);
pose.SetTranslation(+marker_size*2.5, +marker_size*1.5, 0);
multi_marker->PointCloudAdd(2, marker_size, pose);
pose.SetTranslation(-marker_size*2.5, -marker_size*1.5, 0);
multi_marker->PointCloudAdd(3, marker_size, pose);
pose.SetTranslation(+marker_size*2.5, -marker_size*1.5, 0);
multi_marker->PointCloudAdd(4, marker_size, pose);
}
double error=-1;
if (marker_detector.Detect(image, &cam, true, (visualize == 1), 0.0)) {
if (detect_additional) {
error = multi_marker->Update(marker_detector.markers, &cam, pose);
multi_marker->SetTrackMarkers(marker_detector, &cam, pose, visualize ? image : NULL);
marker_detector.DetectAdditional(image, &cam, (visualize == 1));
}
if (visualize == 2)
error = multi_marker->Update(marker_detector.markers, &cam, pose, image);
else
error = multi_marker->Update(marker_detector.markers, &cam, pose);
}
static Marker foo;
foo.SetMarkerSize(marker_size*4);
if ((error >= 0) && (error < 5))
{
foo.pose = pose;
}
foo.Visualize(image, &cam);
if (flip_image) {
cvFlip(image);
image->origin = !image->origin;
}
}
