SampleMultiMarkerBundle.cpp

This is an example that automatically recognising MultiMarker setups using MultiMarkerFiltered and optimizes it with MultiMarkerBundle.

#include "CvTestbed.h"
#include "MarkerDetector.h"
#include "Pose.h"
#include "Shared.h"
using namespace alvar;
using namespace std;
int visualize=1;
const int nof_markers = 8;
const double marker_size = 4;
bool add_measurement=false;
bool optimize = false;
bool optimize_done = false;
std::stringstream calibrationFilename;
double GetMultiMarkerPose(IplImage *image, Camera *cam, Pose &pose) {
static bool init=true;
if (init) {
cout<<"Using manual multimarker approach with MultiMarkerInitializer & MultiMarkerBundle."<<endl;
cout<<"Use 'p' for taking keyframes and 'o' for optimizing."<<endl;
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);
}
double error = -1;
if (!optimize_done) {
if (marker_detector.Detect(image, cam, true, (visualize == 1), 0.0)) {
if (visualize == 2)
error = multi_marker_init->Update(marker_detector.markers, cam, pose, image);
else
error = multi_marker_init->Update(marker_detector.markers, cam, pose);
}
} else {
if (marker_detector.Detect(image, cam, true, (visualize == 1), 0.0)) {
if (visualize == 2)
error = multi_marker_bundle->Update(marker_detector.markers, cam, pose, image);
else
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, (visualize == 3)) > 0))
{
if (visualize == 3)
error = multi_marker_bundle->Update(marker_detector.markers, cam, pose, image);
else
error = multi_marker_bundle->Update(marker_detector.markers, cam, pose);
}
}
}
if (marker_detector.markers->size() >= 2) {
cout<<"Adding measurement..."<<endl;
multi_marker_init->MeasurementsAdd(marker_detector.markers);
}
}
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 pose;
multi_marker_init->getMeasurementPose(i, cam, pose);
const std::vector<MultiMarkerInitializer::MarkerMeasurement> markers
= multi_marker_init->getMeasurementMarkers(i);
multi_marker_bundle->MeasurementsAdd(&markers, pose);
}
// 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;
} else {
cout<<"Optimizing FAILED!"<<endl;
}
}
optimize=false;
}
return error;
}
void videocallback(IplImage *image)
{
static Camera cam;
static 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;
}
marker_detector.SetMarkerSize(marker_size);
}
// Manual approach (use 'p' for taking keyframes and 'o' for optimizing)
double error = GetMultiMarkerPose(image, &cam, pose);
// Visualize a blue marker at the origo.
static Marker foo;
if ((error >= 0) && (error < 5))
{
foo.pose = pose;
}
foo.Visualize(image, &cam, CV_RGB(0,0,255));
if (flip_image) {
cvFlip(image);
image->origin = !image->origin;
}
}
int keycallback(int key)
{
static bool fixed = false;
if(key == 'r')
{
cout<<"Reseting multi marker"<<endl;
multi_marker_init->Reset();
multi_marker_init->MeasurementsReset();
multi_marker_bundle->Reset();
multi_marker_bundle->MeasurementsReset();
add_measurement = false;
optimize = false;
optimize_done = false;
}
else if(key == 'v')
{
}
else if(key == 'l')
{
if(multi_marker_bundle->Load("mmarker.xml", FILE_FORMAT_XML))
{
cout<<"Multi marker loaded"<<endl;
multi_marker_init->PointCloudCopy(multi_marker_bundle);
optimize_done = true;
}
else
cout<<"Cannot load multi marker"<<endl;
}
else if(key == 's')
{
if(multi_marker_bundle->Save("mmarker.xml", FILE_FORMAT_XML))
cout<<"Multi marker saved"<<endl;
else
cout<<"Cannot save multi marker"<<endl;
}
else if(key == 'p')
{
}
else if(key == 'o')
{
optimize=true;
}
else return key;
return 0;
}
int main(int argc, char *argv[])
{
try {
// Output usage message
std::string filename(argv[0]);
filename = filename.substr(filename.find_last_of('\\') + 1);
std::cout << "SampleMultiMarkerBundle" << std::endl;
std::cout << "=======================" << std::endl;
std::cout << std::endl;
std::cout << "Description:" << std::endl;
std::cout << " This is an example of how to use the 'MultiMarkerBundle' class to" << std::endl;
std::cout << " automatically deduce and optimize 'MultiMarker' setups." << std::endl;
std::cout << std::endl;
std::cout << "Usage:" << std::endl;
std::cout << " " << filename << " [device]" << std::endl;
std::cout << std::endl;
std::cout << " device integer selecting device from enumeration list (default 0)" << std::endl;
std::cout << " highgui capture devices are prefered" << std::endl;
std::cout << std::endl;
std::cout << "Keyboard Shortcuts:" << std::endl;
std::cout << " l: load marker configuration from mmarker.txt" << std::endl;
std::cout << " s: save marker configuration to mmarker.txt" << std::endl;
std::cout << " r: reset marker configuration" << std::endl;
std::cout << " p: add measurement" << std::endl;
std::cout << " o: optimize bundle" << std::endl;
std::cout << " q: quit" << std::endl;
std::cout << std::endl;
// Initialise CvTestbed
// Enumerate possible capture plugins
if (plugins.size() < 1) {
std::cout << "Could not find any capture plugins." << std::endl;
return 0;
}
// Display capture plugins
std::cout << "Available Plugins: ";
std::cout << std::endl;
// Enumerate possible capture devices
if (devices.size() < 1) {
std::cout << "Could not find any capture devices." << std::endl;
return 0;
}
// Check command line argument for which device to use
int selectedDevice = defaultDevice(devices);
if (argc > 1) {
selectedDevice = atoi(argv[1]);
}
if (selectedDevice >= (int)devices.size()) {
selectedDevice = defaultDevice(devices);
}
// Display capture devices
std::cout << "Enumerated Capture Devices:" << std::endl;
outputEnumeratedDevices(devices, selectedDevice);
std::cout << std::endl;
// Create capture object from camera
Capture *cap = CaptureFactory::instance()->createCapture(devices[selectedDevice]);
std::string uniqueName = devices[selectedDevice].uniqueName();
// Handle capture lifecycle and start video capture
// Note that loadSettings/saveSettings are not supported by all plugins
if (cap) {
std::stringstream settingsFilename;
settingsFilename << "camera_settings_" << uniqueName << ".xml";
calibrationFilename << "camera_calibration_" << uniqueName << ".xml";
cap->start();
cap->setResolution(640, 480);
if (cap->loadSettings(settingsFilename.str())) {
std::cout << "Loading settings: " << settingsFilename.str() << std::endl;
}
std::stringstream title;
title << "SampleMultiMarkerBundle (" << cap->captureDevice().captureType() << ")";
CvTestbed::Instance().StartVideo(cap, title.str().c_str());
if (cap->saveSettings(settingsFilename.str())) {
std::cout << "Saving settings: " << settingsFilename.str() << std::endl;
}
cap->stop();
delete cap;
}
else if (CvTestbed::Instance().StartVideo(0, argv[0])) {
}
else {
std::cout << "Could not initialize the selected capture backend." << std::endl;
}
return 0;
}
catch (const std::exception &e) {
std::cout << "Exception: " << e.what() << endl;
}
catch (...) {
std::cout << "Exception: unknown" << std::endl;
}
}


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Mon Jun 10 2019 12:47:03