bool CalibrationArea::on_button_press_event(GdkEventButton *event)
{
// Handle click
time_elapsed = 0;
bool success = calibrator->add_click((int)event->x_root, (int)event->y_root);
if (!success && calibrator->get_numclicks() == 0) {
draw_message("Mis-click detected, restarting...");
} else {
draw_message(NULL);
}
// Are we done yet?
if (calibrator->get_numclicks() >= 4) {
// Recalibrate
success = calibrator->finish(display_width, display_height);
if (success) {
exit(0);
} else {
// TODO, in GUI ?
fprintf(stderr, "Error: unable to apply or save configuration values");
exit(1);
}
}
// Force a redraw
redraw();
return true;
}
bool GuiCalibratorX11::on_button_press_event(int x, int y)
{
// Clear window, maybe a bit overdone, but easiest for me atm.
// (goal is to clear possible message and other clicks)
XClearWindow(display, win);
// Handle click
time_elapsed = 0;
bool success = calibrator->add_click(x, y);
if (!success && calibrator->get_numclicks() == 0) {
draw_message("Mis-click detected, restarting...");
}
// Are we done yet?
if (calibrator->get_numclicks() >= 4) {
// Recalibrate
success = calibrator->finish();
if (success) {
exit(0);
} else {
// TODO, in GUI ?
fprintf(stderr, "Error: unable to apply or save configuration values");
exit(1);
}
}
// Force a redraw
redraw();
return true;
}
int main(int argc, char** argv)
{
dataLog.makeFile();
p.connect(); //connect our publisher
c.setLogger(dataLog); //set the log
c.setPub(p); //set the publisher
int counter = 0; //count how many calibration points received
connect();
while (1)
{
subscribe(c); //listen for a calibration point
counter++;
if (counter >= 9) //if all points received
{
c.calibrate(); //calculate calibration matrix
calibrated = true; //calibration is complete
counter = 0; //reset counter in case of recalibration
thread t1(runMapper, c); //start sending the screen points to the oculus
t1.detach(); //detach the thread
}
}
return 0;
}
void GuiCalibratorX11::redraw()
{
#ifdef HAVE_X11_XRANDR
// check that screensize did not change
int nsizes;
XRRScreenSize* randrsize = XRRSizes(display, screen_num, &nsizes);
if (nsizes != 0 && (display_width != randrsize->width ||
display_height != randrsize->height)) {
set_display_size(randrsize->width, randrsize->height);
}
#endif
// Print the text
int text_height = font_info->ascent + font_info->descent;
int text_width = -1;
for (int i = 0; i != help_lines; i++) {
text_width = std::max(text_width, XTextWidth(font_info,
help_text[i].c_str(), help_text[i].length()));
}
int x = (display_width - text_width) / 2;
int y = (display_height - text_height) / 2 - 60;
XSetForeground(display, gc, pixel[BLACK]);
XSetLineAttributes(display, gc, 2, LineSolid, CapRound, JoinRound);
XDrawRectangle(display, win, gc, x - 10, y - (help_lines*text_height) - 10,
text_width + 20, (help_lines*text_height) + 20);
// Print help lines
y -= 3;
for (int i = help_lines-1; i != -1; i--) {
int w = XTextWidth(font_info, help_text[i].c_str(), help_text[i].length());
XDrawString(display, win, gc, x + (text_width-w)/2, y,
help_text[i].c_str(), help_text[i].length());
y -= text_height;
}
// Draw the points
for (int i = 0; i <= calibrator->get_numclicks(); i++) {
// set color: already clicked or not
if (i < calibrator->get_numclicks())
XSetForeground(display, gc, pixel[WHITE]);
else
XSetForeground(display, gc, pixel[RED]);
XSetLineAttributes(display, gc, 1, LineSolid, CapRound, JoinRound);
XDrawLine(display, win, gc, X[i] - cross_lines, Y[i],
X[i] + cross_lines, Y[i]);
XDrawLine(display, win, gc, X[i], Y[i] - cross_lines,
X[i], Y[i] + cross_lines);
XDrawArc(display, win, gc, X[i] - cross_circle, Y[i] - cross_circle,
(2 * cross_circle), (2 * cross_circle), 0, 360 * 64);
}
// Draw the clock background
XSetForeground(display, gc, pixel[DIMGRAY]);
XSetLineAttributes(display, gc, 0, LineSolid, CapRound, JoinRound);
XFillArc(display, win, gc, (display_width-clock_radius)/2, (display_height - clock_radius)/2,
clock_radius, clock_radius, 0, 360 * 64);
}
void subscribe(Calibrator c)
{
//Listen for calibration signal from Oculus
string msg = s_recv(subscriber4);
cout << "got " << msg << endl;
dataLog.log("got " + msg + "\n");
//Check if already calibrated
if (calibrated)
{
//If already calibrated then begin a new calibration
cout << "New Calibration" << endl;
dataLog.log("Beginning new calibration\n");
calibrated = false;
//Set the counter in Calibrator back to zero
c.recalibrate();
}
string msg2 = "";
string msg3 = "";
string msg4 = "";
int b = 1;
//Receive pupil coordinates from tracker
while (b)
{
msg2 = s_recv(subscriber3);
cout << "got " << msg2 << endl;
msg3 = s_recv(subscriber3);
cout << "got " << msg3 << endl;
msg4 = s_recv(subscriber3);
cout << "got " << msg4 << endl;
//Reject if a zero value is received
if (atof(msg3.c_str()) != 0.0 && atof(msg4.c_str()) != 0.0)
{
b = 0; //Accept if no zeros
}
}
cout << "Trying to add x: " << msg3.c_str() << endl;
dataLog.log("Trying to add x : " + msg3 + "\n");
//Send to the Calibrator
c.setX(atof(msg3.c_str()));
cout << "Trying to add y: " << msg4.c_str() << endl;
dataLog.log("Trying to add x : " + msg4 + "\n");
c.setY(atof(msg4.c_str()));
}
int main (int argc, char ** argv)
{
// if(pcl::console::find_switch(argc, argv, "-h") || pcl::console::find_switch(argc, argv, "--help"))
// print_help(argv);
cout << "Calibrate RGBD360 multisensor\n";
Calibrator calibrator;
calibrator.run();
cout << "EXIT\n";
return (0);
}
void runMapper(Calibrator c)
{
//Stop if recalibration is in progress
while(calibrated)
{
std::string msg2 = "";
std::string msg3 = "";
std::string msg4 = "";
int b = 1;
while (b)
{
msg2 = s_recv(subscriber5);
std::cout << "got " << msg2 << std::endl;
msg3 = s_recv(subscriber5);
std::cout << "got " << msg3 << std::endl;
msg4 = s_recv(subscriber5);
std::cout << "got " << msg4 << std::endl;
if (atof(msg3.c_str()) != 0.0 && atof(msg4.c_str()) != 0.0) {
b = 0;
}
}
cout << "To be mapped: " << msg3 << ", " << msg4 << endl;
dataLog.log("To be mapped: " + msg3 + ", " + msg4 + "\n");
//Send to the mapper to map to screen points
c.homography_map_point(atof(msg3.c_str()), atof(msg4.c_str()));
}
}
void setup() {
useInputStream(iStream);
useOutputStream(oStream);
calibrator.setCalibrateFunction(processAccelerometerData);
calibrator.addCalibrateProcess("Resting",
"Rest accelerometer on flat surface.", restingDataCollected);
useCalibrator(calibrator);
DTW dtw(false, true, null_rej);
pipeline.setClassifier(dtw);
usePipeline(pipeline);
registerTuneable(null_rej, 0.1, 5.0, "Variability",
"How different from the training data a new gesture can be and "
"still be considered the same gesture. The higher the number, the "
"more different it can be.", updateVariability);
useTrainingSampleChecker(checkTrainingSample);
}
void setup()
{
stream.setLabelsForAllDimensions({"x", "y", "z"});
useStream(stream);
useOutputStream(oStream);
calibrator.setCalibrateFunction(processAccelerometerData);
calibrator.addCalibrateProcess("Resting",
"Rest accelerometer on flat surface.", restingDataCollected);
useCalibrator(calibrator);
pipeline.addFeatureExtractionModule(FeatureApply(3, 1, dotProduct));
pipeline.addFeatureExtractionModule(TimeseriesBuffer(80, 1));
pipeline.addFeatureExtractionModule(FeatureApply(80, 1, stddev));
pipeline.addFeatureExtractionModule(FeatureApply(1, 1, threshold));
usePipeline(pipeline);
registerTuneable(t, 0, 1.0, "Walking Threshold",
"How much the accelerometer data needs to be "
"changing to be considered walking.");
}
void CalibrationArea::set_display_size(int width, int height) {
display_width = width;
display_height = height;
// Compute absolute circle centers
const int delta_x = display_width/num_blocks;
const int delta_y = display_height/num_blocks;
X[UL] = delta_x; Y[UL] = delta_y;
X[UR] = display_width - delta_x - 1; Y[UR] = delta_y;
X[LL] = delta_x; Y[LL] = display_height - delta_y - 1;
X[LR] = display_width - delta_x - 1; Y[LR] = display_height - delta_y - 1;
// reset calibration if already started
calibrator->reset();
}
int main(int argc, char** argv)
{
////////////////////////////////////////////////////////////////////
// Create command line options. Check if we should print usage.
GetPot cl(argc,argv);
if(cl.search(3, "-help", "-h", "?") || argc < 2) {
std::cout << sUriInfo << std::endl;
return -1;
}
////////////////////////////////////////////////////////////////////
// Default configuration values
// Default grid printed on US Letter
int grid_preset = GridPresetGWUSmall;
double grid_spacing;
int grid_rows;
int grid_cols;
uint32_t grid_seed;
double grid_large_rad;
double grid_small_rad;
std::string save_grid;
// Use no input cameras by default
std::vector<calibu::CameraAndPose > input_cameras;
// Fix cameras intrinsic parameters during optimisation, changing
// only their relative poses.
bool fix_intrinsics = false;
// Require user to start playing the video
bool start_paused = false;
// Output file for camera rig
std::string output_filename = "cameras.xml";
////////////////////////////////////////////////////////////////////
// Parse command line
grid_preset = cl.follow(grid_preset, "-grid-preset");
switch(grid_preset)
{
case GridPresetGWUSmall:
grid_spacing = 0.254 / 18; // meters
grid_large_rad = 0.00423; // m
grid_small_rad = 0.00283; // m
grid_rows = 10; // grid dots
grid_cols = 19; // grid dots
grid_seed = 71;
break;
case GridPresetGoogleLarge:
grid_spacing = 0.03156; // meters
grid_large_rad = 0.00889; // m
grid_small_rad = 0.00635; // m
grid_rows = 36; // grid dots
grid_cols = 25; // grid dots
grid_seed = 71;
break;
}
grid_spacing = cl.follow(grid_spacing,"-grid-spacing");
grid_seed = cl.follow((int)grid_seed,"-grid-seed");
grid_cols = cl.follow((int)grid_cols,"-grid-cols");
grid_rows = cl.follow((int)grid_rows,"-grid-rows");
grid_large_rad = cl.follow(grid_large_rad,"-grid-large-rad");
grid_small_rad = cl.follow(grid_small_rad,"-grid-small-rad");
fix_intrinsics = cl.search(2, "-fix-intrinsics", "-f");
start_paused = cl.search(2, "-paused", "-p");
output_filename = cl.follow(output_filename.c_str(), 2, "-output", "-o");
save_grid = cl.follow("", "-save-grid");
const Eigen::Vector2i grid_size(grid_cols, grid_rows);
////////////////////////////////////////////////////////////////////
// Setup Grid pattern
ConicFinder conic_finder;
conic_finder.Params().conic_min_area = 4.0;
conic_finder.Params().conic_min_density = 0.6;
conic_finder.Params().conic_min_aspect = 0.2;
std::unique_ptr<TargetGridDot> target;
if(grid_preset == GridPresetGoogleLarge)
target.reset(new TargetGridDot(grid_spacing, GoogleLargeGrid()));
else if(grid_preset == GridPresetGWUSmall)
target.reset(new TargetGridDot(grid_spacing, GWUSmallGrid()));
else
target.reset(new TargetGridDot(grid_spacing, grid_size, grid_seed));
// Save grid and exit if required
if(!save_grid.empty()) {
saveGrid(*target, save_grid, grid_large_rad, grid_small_rad);
return 0;
}
////////////////////////////////////////////////////////////////////
// Setup Video Source
// Last argument or parameter - Video URI
std::string cam_param = cl.follow("", 2, "-video_url", "-cam");
std::string uri = cam_param.empty() ? argv[argc-1] : cam_param;
hal::Camera cam;
try {
cam = hal::Camera( uri );
} catch (...) {
if(!cam_param.empty())
std::cerr << "Could not create camera from URI: " << uri
<< std::endl;
return -1;
}
if(cam.Empty()) return -1;
// For the moment, assume all N cameras have same resolution
const size_t N = cam.NumChannels();
const size_t w = cam.Width();
const size_t h = cam.Height();
std::shared_ptr<pb::ImageArray> imageArray = pb::ImageArray::Create();
cam.Capture(*imageArray);
std::vector<cv::Mat> vImages;
std::vector<uint64_t> vSerialNos;
vImages.reserve(imageArray->Size());
vSerialNos.reserve(imageArray->Size());
for( int i = 0; i < imageArray->Size(); ++i ){
pb::Image& image = *(*imageArray)[i];
cv::Mat im = image.Mat();
if( im.type() != CV_8UC1 ){
std::cerr << "Input channels must be GRAY8 format. Use "
"Convert:[fmt=MONO8]// video scheme." << std::endl;
} else {
vImages.emplace_back(im);
vSerialNos.emplace_back(image.SerialNumber());
}
}
// Load camera hints from command line
cl.disable_loop();
cl.reset_cursor();
for(std::string filename = cl.follow("",2,"-cameras","-c");
!filename.empty(); filename = cl.follow("",2,"-cameras","-c") ) {
const size_t i = input_cameras.size();
if(i < N) {
if(filename == "fov") {
CameraModelT<Fov> starting_cam(w, h);
starting_cam.Params() << 300, 300, w/2.0, h/2.0, 0.2;
input_cameras.push_back( CameraAndPose(CameraModel(starting_cam), Sophus::SE3d() ) );
}else if(filename == "poly2") {
CameraModelT<Poly2> starting_cam(w, h);
starting_cam.Params() << 300, 300, w/2.0, h/2.0, 0.0, 0.0;
input_cameras.push_back( CameraAndPose(CameraModel(starting_cam), Sophus::SE3d() ) );
}else if(filename == "poly3" || filename =="poly") {
CameraModelT<Poly3> starting_cam(w, h);
starting_cam.Params() << 300, 300, w/2.0, h/2.0, 0.0, 0.0, 0.0;
input_cameras.push_back( CameraAndPose(CameraModel(starting_cam), Sophus::SE3d() ) );
}else if(filename == "kb4") {
CameraModelT<ProjectionKannalaBrandt> starting_cam(w, h);
starting_cam.Params() << 300, 300, w/2.0, h/2.0, 0.0, 0.0, 0.0, 0.0;
input_cameras.push_back( CameraAndPose(CameraModel(starting_cam), Sophus::SE3d() ) );
}else{
const CameraRig rig = ReadXmlRig(filename);
for(const CameraModelAndTransform& cop : rig.cameras ) {
input_cameras.push_back( CameraAndPose(cop.camera, cop.T_wc.inverse()) );
}
}
}else{
throw std::runtime_error("Too many camera files provided.");
}
}
if(input_cameras.size() > 0 && input_cameras.size() != N) {
std::cerr << "Number of cameras specified in files does not match video source" << std::endl;
return -1;
}
////////////////////////////////////////////////////////////////////
// Setup image processing pipeline
ImageProcessing image_processing(w,h);
image_processing.Params().black_on_white = true;
image_processing.Params().at_threshold = 0.9;
image_processing.Params().at_window_ratio = 30.0;
CVarUtils::AttachCVar("proc.adaptive.threshold", &image_processing.Params().at_threshold);
CVarUtils::AttachCVar("proc.adaptive.window_ratio", &image_processing.Params().at_window_ratio);
CVarUtils::AttachCVar("proc.black_on_white", &image_processing.Params().black_on_white);
////////////////////////////////////////////////////////////////////
// Initialize Calibration object and tracking params
Calibrator calibrator;
calibrator.FixCameraIntrinsics(fix_intrinsics);
int calib_cams[N];
bool tracking_good[N];
std::vector<Sophus::SE3d> T_hw;
T_hw.resize(N);
for(size_t i=0; i<N; ++i) {
const int w_i = cam.Width();
const int h_i = cam.Height();
if(i < input_cameras.size() ) {
input_cameras[i].camera.SetSerialNumber(vSerialNos[i]);
calib_cams[i] = calibrator.AddCamera(
input_cameras[i].camera, input_cameras[i].T_ck
);
}else{
// Generic starting set of parameters.
CameraModelT<Fov> starting_cam(w_i, h_i);
starting_cam.Params() << 300, 300, w_i/2.0, h_i/2.0, 0.2;
starting_cam.SetSerialNumber(vSerialNos[i]);
calib_cams[i] = calibrator.AddCamera(
CameraModel(starting_cam),
Sophus::SE3d()
);
}
}
////////////////////////////////////////////////////////////////////
// Setup GUI
const int PANEL_WIDTH = 150;
pangolin::CreateWindowAndBind("Main",(N+1)*w/2.0+PANEL_WIDTH,h/2.0);
// Make things look prettier...
glEnable(GL_LINE_SMOOTH);
glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );
glHint( GL_LINE_SMOOTH_HINT, GL_NICEST );
glEnable (GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glDepthFunc( GL_LEQUAL );
glEnable( GL_DEPTH_TEST );
glLineWidth(1.7);
// Pangolin 3D Render state
pangolin::OpenGlRenderState stacks;
stacks.SetProjectionMatrix(pangolin::ProjectionMatrixRDF_TopLeft(640,480,420,420,320,240,0.01,1E6));
stacks.SetModelViewMatrix(pangolin::ModelViewLookAtRDF(0,0,-0.5, 0,0,0, 0, -1, 0) );
// Create viewport for video with fixed aspect
pangolin::CreatePanel("ui").SetBounds(1.0,0.0,0,pangolin::Attach::Pix(PANEL_WIDTH));
pangolin::View& container = pangolin::CreateDisplay()
.SetBounds(1.0,0.0, pangolin::Attach::Pix(PANEL_WIDTH),1.0)
.SetLayout(pangolin::LayoutEqual);
// Add view for each camera stream
for(size_t c=0; c < N; ++c) {
container.AddDisplay( pangolin::CreateDisplay().SetAspect(w/(float)h) );
}
// Add 3d view, attach input handler
pangolin::Handler3D handler(stacks);
pangolin::View& v3D = pangolin::CreateDisplay().SetAspect((float)w/h).SetHandler(&handler);
container.AddDisplay(v3D);
// OpenGl Texture for video frame
pangolin::GlTexture tex(w,h,GL_LUMINANCE8);
////////////////////////////////////////////////////////////////////
// Display Variables
pangolin::Var<bool> run("ui.Play video", !start_paused, true);
pangolin::Var<double> disp_mse("ui.MSE");
pangolin::Var<int> disp_frame("ui.frame");
pangolin::Var<bool> add("ui.Add Frames", true, true);
pangolin::Var<bool> disp_thresh("ui.Display Thresh",false);
pangolin::Var<bool> disp_lines("ui.Display Lines",true);
pangolin::Var<bool> disp_cross("ui.Display crosses",true);
pangolin::Var<bool> disp_bbox("ui.Display bbox",true);
////////////////////////////////////////////////////////////////////
// Key shortcuts
// 1,2,3,... keys hide and show viewports
for(size_t i=0; i<container.NumChildren(); ++i) {
pangolin::RegisterKeyPressCallback('1'+i, [&container,i](){container[i].ToggleShow();} );
}
pangolin::RegisterKeyPressCallback('[', [&](){calibrator.Start();} );
pangolin::RegisterKeyPressCallback(']', [&](){calibrator.Stop();} );
bool step = false;
pangolin::RegisterKeyPressCallback(pangolin::PANGO_SPECIAL+ pangolin::PANGO_KEY_RIGHT, [&](){step = true;} );
pangolin::RegisterKeyPressCallback(' ', [&](){run = !run;} );
pangolin::RegisterKeyPressCallback('r', [&](){calibrator.PrintResults();} );
pangolin::RegisterKeyPressCallback('q', &pangolin::Quit);
////////////////////////////////////////////////////////////////////
// Main event loop
for(int frame=0; !pangolin::ShouldQuit();){
const bool go = (frame==0) || run || pangolin::Pushed(step);
int calib_frame = -1;
if( go ) {
if( cam.Capture( vImages ) ){
if(add) {
calib_frame = calibrator.AddFrame(Sophus::SE3d(Sophus::SO3d(), Eigen::Vector3d(0,0,1000)) );
}
++frame;
}else{
run = false;
}
}
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
for(size_t iI = 0; iI < N; ++iI)
{
if (vImages.size() != N) break;
image_processing.Process( vImages[iI].data, vImages[iI].cols, vImages[iI].rows, vImages[iI].cols );
conic_finder.Find( image_processing );
const std::vector<Conic, Eigen::aligned_allocator<Conic> >& conics =
conic_finder.Conics();
std::vector<int> ellipse_target_map;
tracking_good[iI] = target->FindTarget(
image_processing, conic_finder.Conics(),
ellipse_target_map
);
if(tracking_good[iI]) {
// Generate map and point structures
std::vector<Eigen::Vector2d,
Eigen::aligned_allocator<Eigen::Vector2d> > ellipses;
for( size_t i=0; i < conics.size(); ++i ) {
ellipses.push_back(conics[i].center);
}
// find camera pose given intrinsics
PosePnPRansac(
calibrator.GetCamera(iI).camera, ellipses, target->Circles3D(),
ellipse_target_map,
0, 0, &T_hw[iI]
);
if(calib_frame >= 0) {
if(iI==0 || !tracking_good[0]) {
// Initialize pose of frame for least squares optimisation
calibrator.GetFrame(calib_frame) = T_hw[iI];
}
for(size_t p=0; p < ellipses.size(); ++p) {
const Eigen::Vector2d pc = ellipses[p];
const Eigen::Vector2i pg = target->Map()[p].pg;
if( 0<= pg(0) && pg(0) < grid_size(0) && 0<= pg(1) && pg(1) < grid_size(1) )
{
const Eigen::Vector3d pg3d = grid_spacing * Eigen::Vector3d(pg(0), pg(1), 0);
// TODO: Add these correspondences in bulk to avoid
// hitting mutex each time.
calibrator.AddObservation(calib_frame, calib_cams[iI], pg3d, pc );
}
}
}
}
if(container[iI].IsShown()) {
container[iI].ActivateScissorAndClear();
glColor3f(1,1,1);
// Display camera image
if(!disp_thresh) {
tex.Upload(image_processing.Img(),GL_LUMINANCE,GL_UNSIGNED_BYTE);
tex.RenderToViewportFlipY();
}else{
tex.Upload(image_processing.ImgThresh(),GL_LUMINANCE,GL_UNSIGNED_BYTE);
tex.RenderToViewportFlipY();
}
// Setup orthographic pixel drawing
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-0.5,w-0.5,h-0.5,-0.5,0,1.0);
glMatrixMode(GL_MODELVIEW);
if(disp_lines) {
for(std::list<LineGroup>::const_iterator i = target->LineGroups().begin(); i != target->LineGroups().end(); ++i)
{
glColor3f(0.5,0.5,0.5);
glBegin(GL_LINE_STRIP);
for(std::list<size_t>::const_iterator el = i->ops.begin(); el != i->ops.end(); ++el)
{
const Eigen::Vector2d p = conics[*el].center;
glVertex2d(p(0), p(1));
}
glEnd();
}
}
if(disp_cross) {
for( size_t i=0; i < conics.size(); ++i ) {
const Eigen::Vector2d pc = conics[i].center;
pangolin::glColorBin( target->Map()[i].value, 2);
pangolin::glDrawCross(pc, conics[i].bbox.Width()*0.75 );
}
}
if(disp_bbox) {
for( size_t i=0; i < conics.size(); ++i ) {
const Eigen::Vector2i pg = tracking_good[iI] ? target->Map()[i].pg : Eigen::Vector2i(0,0);
if( 0<= pg(0) && pg(0) < grid_size(0) && 0<= pg(1) && pg(1) < grid_size(1) ) {
pangolin::glColorBin(pg(1)*grid_size(0)+pg(0), grid_size(0)*grid_size(1));
glDrawRectPerimeter(conics[i].bbox);
}
}
}
}
}
if(v3D.IsShown()) {
v3D.ActivateScissorAndClear(stacks);
calibu::glDrawTarget(*target, Eigen::Vector2d(0,0), 1.0, 0.8, 1.0);
for(size_t c=0; c< calibrator.NumCameras(); ++c) {
const Eigen::Matrix3d Kinv = calibrator.GetCamera(c).camera.Kinv();
const CameraAndPose cap = calibrator.GetCamera(c);
const Sophus::SE3d T_ck = cap.T_ck;
// Draw keyframes
pangolin::glColorBin(c, 2, 0.2);
for(size_t k=0; k< calibrator.NumFrames(); ++k) {
pangolin::glDrawAxis((T_ck * calibrator.GetFrame(k)).inverse().matrix(), 0.01);
}
// Draw current camera
if(tracking_good[c]) {
pangolin::glColorBin(c, 2, 0.5);
pangolin::glDrawFrustrum(Kinv,w,h,T_hw[c].inverse().matrix(),0.05);
}
}
}
disp_mse = calibrator.MeanSquareError();
disp_frame = frame;
// Process window events via GLUT
pangolin::FinishFrame();
}
calibrator.Stop();
calibrator.PrintResults();
calibrator.WriteCameraModels(output_filename);
}
bool CalibrationArea::on_expose_event(GdkEventExpose *event)
{
// check that screensize did not change
if (display_width != get_width() ||
display_height != get_height()) {
set_display_size(get_width(), get_height());
}
Glib::RefPtr<Gdk::Window> window = get_window();
if (window) {
Cairo::RefPtr<Cairo::Context> cr = window->create_cairo_context();
cr->save();
cr->rectangle(event->area.x, event->area.y, event->area.width, event->area.height);
cr->clip();
// Print the text
cr->set_font_size(font_size);
double text_height = -1;
double text_width = -1;
Cairo::TextExtents extent;
for (int i = 0; i != help_lines; i++) {
cr->get_text_extents(help_text[i], extent);
text_width = std::max(text_width, extent.width);
text_height = std::max(text_height, extent.height);
}
text_height += 2;
double x = (display_width - text_width) / 2;
double y = (display_height - text_height) / 2 - 60;
cr->set_line_width(2);
cr->rectangle(x - 10, y - (help_lines*text_height) - 10,
text_width + 20, (help_lines*text_height) + 20);
// Print help lines
y -= 3;
for (int i = help_lines-1; i != -1; i--) {
cr->get_text_extents(help_text[i], extent);
cr->move_to(x + (text_width-extent.width)/2, y);
cr->show_text(help_text[i]);
y -= text_height;
}
cr->stroke();
// Draw the points
for (int i = 0; i <= calibrator->get_numclicks(); i++) {
// set color: already clicked or not
if (i < calibrator->get_numclicks())
cr->set_source_rgb(1.0, 1.0, 1.0);
else
cr->set_source_rgb(0.8, 0.0, 0.0);
cr->set_line_width(1);
cr->move_to(X[i] - cross_lines, Y[i]);
cr->rel_line_to(cross_lines*2, 0);
cr->move_to(X[i], Y[i] - cross_lines);
cr->rel_line_to(0, cross_lines*2);
cr->stroke();
cr->arc(X[i], Y[i], cross_circle, 0.0, 2.0 * M_PI);
cr->stroke();
}
// Draw the clock background
cr->arc(display_width/2, display_height/2, clock_radius/2, 0.0, 2.0 * M_PI);
cr->set_source_rgb(0.5, 0.5, 0.5);
cr->fill_preserve();
cr->stroke();
cr->set_line_width(clock_line_width);
cr->arc(display_width/2, display_height/2, (clock_radius - clock_line_width)/2,
3/2.0*M_PI, (3/2.0*M_PI) + ((double)time_elapsed/(double)max_time) * 2*M_PI);
cr->set_source_rgb(0.0, 0.0, 0.0);
cr->stroke();
// Draw the message (if any)
if (message != NULL) {
// Frame the message
cr->set_font_size(font_size);
Cairo::TextExtents extent;
cr->get_text_extents(this->message, extent);
text_width = extent.width;
text_height = extent.height;
x = (display_width - text_width) / 2;
y = (display_height - text_height + clock_radius) / 2 + 60;
cr->set_line_width(2);
cr->rectangle(x - 10, y - text_height - 10,
text_width + 20, text_height + 25);
// Print the message
cr->move_to(x, y);
cr->show_text(this->message);
cr->stroke();
}
cr->restore();
}
return true;
}
void CalibratorLogImpl::outputState()
{
osOutput_ << "\nCalibrator iterations completed: " << calibrator_->iterationCount() << endl << endl;
const CalibrationParameters& p = calibrator_->parameters();
osOutput_ << "A: " << p.A << endl;
osOutput_ << "B: " << p.B << endl << endl;
osOutput_ <<
setw(columnWidth_) << "mass_true" <<
setw(3) << "z" <<
setw(columnWidth_) << "f_obs" <<
setw(columnWidth_) << "mass_calc" <<
setw(columnWidth_) << "error (ppm)" <<
setw(3) << "id" <<
" distribution" << endl;
double totalSquaredMassError = 0;
double totalSquaredCalibrationError = 0;
int totalCorrect = 0;
int totalConfident = 0;
int N = calibrator_->measurementCount();
for (int i=0; i<N; i++)
{
// calculations
double f = calibrator_->measurement(i)->frequency;
int z = calibrator_->measurement(i)->charge;
double mz = p.mz(f);
double massCalculated = Ion::neutralMass(mz, z);
double massTrue = 0;
double error = 0;
bool correct = false;
bool confident = false;
if (trueMasses_)
{
// error calculations
massTrue = trueMasses_->at(i);
error = (massCalculated - massTrue)/massTrue;
totalSquaredMassError += error*error;
if (trueParameters_)
{
double term1 = fabs(p.A - trueParameters_->A)/f;
double term2 = fabs(p.B - trueParameters_->B)/(f*f);
double deviation = (term1 + term2)/massTrue;
totalSquaredCalibrationError += deviation*deviation;
}
// id verification
if (calibrator_->massSpread(i) && !calibrator_->massSpread(i)->distribution().empty())
{
const MassSpread::Pair& id = calibrator_->massSpread(i)->distribution()[0];
if (fabs(id.mass-massTrue) < 1e-8)
{
correct = true;
totalCorrect++;
if (id.probability > .95)
{
confident = true;
totalConfident++;
}
}
}
}
// output
osOutput_ <<
setw(columnWidth_) << massTrue <<
setw(3) << z <<
setw(columnWidth_) << f <<
setw(columnWidth_) << massCalculated <<
setw(columnWidth_) << error * 1e6 <<
setw(2) << (correct ? "*" : " ") <<
(confident ? "!" : " ") << " ";
if (calibrator_->massSpread(i))
calibrator_->massSpread(i)->output(osOutput_);
osOutput_ << endl;
}
double rmsMassError = sqrt(totalSquaredMassError / N);
double rmsCalibrationError = sqrt(totalSquaredCalibrationError / N);
osOutput_ << "\nTotal error: " << calibrator_->error() * 1e6 << " ppm\n\n";
// update summary
osSummary_ << calibrator_->iterationCount() << " " <<
p.A << " " <<
p.B << " " <<
rmsMassError * 1e6 << " " <<
calibrator_->error() * 1e6 << " " <<
rmsCalibrationError * 1e6 << " " <<
(double)totalCorrect/N << " " <<
(double)totalConfident/N << endl;
}
Calibrator* Calibrator::make_calibrator(int argc, char** argv)
{
Calibrator *calibrator;
bool list_devices = false;
bool fake = false;
bool precalib = false;
bool reset = false;
XYinfo pre_axys;
const char* pre_device = NULL;
const char* geometry = NULL;
unsigned thr_misclick = 15;
unsigned thr_doubleclick = 7;
OutputType output_type = OUTYPE_AUTO;
// parse input
if (argc > 1) {
for (int i=1; i!=argc; i++) {
// Display help ?
if (strcmp("-h", argv[i]) == 0 ||
strcmp("--help", argv[i]) == 0) {
fprintf(stderr, "xinput_calibrator, v%s\n\n", VERSION);
usage(argv[0], thr_misclick);
exit(0);
} else
// Verbose output ?
if (strcmp("-v", argv[i]) == 0 ||
strcmp("--verbose", argv[i]) == 0) {
verbose = true;
} else
// Just list devices ?
if (strcmp("--list", argv[i]) == 0) {
list_devices = true;
} else
// Select specific device ?
if (strcmp("--device", argv[i]) == 0) {
if (argc > i+1)
pre_device = argv[++i];
else {
fprintf(stderr, "Error: --device needs a device name or id as argument; use --list to list the calibratable input devices.\n\n");
usage(argv[0], thr_misclick);
exit(1);
}
} else
// Get pre-calibration ?
if (strcmp("--precalib", argv[i]) == 0) {
precalib = true;
if (argc > i+1)
pre_axys.x.min = atoi(argv[++i]);
if (argc > i+1)
pre_axys.x.max = atoi(argv[++i]);
if (argc > i+1)
pre_axys.y.min = atoi(argv[++i]);
if (argc > i+1)
pre_axys.y.max = atoi(argv[++i]);
} else
// Get pre-calibration ?
if (strcmp("--reset", argv[i]) == 0) {
reset = true;
} else
// Get mis-click threshold ?
if (strcmp("--misclick", argv[i]) == 0) {
if (argc > i+1)
thr_misclick = atoi(argv[++i]);
else {
fprintf(stderr, "Error: --misclick needs a number (the pixel threshold) as argument. Set to 0 to disable mis-click detection.\n\n");
usage(argv[0], thr_misclick);
exit(1);
}
} else
// Get output type ?
if (strcmp("--output-type", argv[i]) == 0) {
if (argc > i+1) {
i++; // eat it or exit
if (strcmp("auto", argv[i]) == 0)
output_type = OUTYPE_AUTO;
else if (strcmp("xorg.conf.d", argv[i]) == 0)
output_type = OUTYPE_XORGCONFD;
else if (strcmp("hal", argv[i]) == 0)
output_type = OUTYPE_HAL;
else if (strcmp("xinput", argv[i]) == 0)
output_type = OUTYPE_XINPUT;
else {
fprintf(stderr, "Error: --output-type needs one of auto|xorg.conf.d|hal|xinput.\n\n");
usage(argv[0], thr_misclick);
exit(1);
}
} else {
fprintf(stderr, "Error: --output-type needs one argument.\n\n");
usage(argv[0], thr_misclick);
exit(1);
}
} else
// specify window geometry?
if (strcmp("--geometry", argv[i]) == 0) {
geometry = argv[++i];
} else
// Fake calibratable device ?
if (strcmp("--fake", argv[i]) == 0) {
fake = true;
}
// unknown option
else {
fprintf(stderr, "Unknown option: %s\n\n", argv[i]);
usage(argv[0], thr_misclick);
exit(0);
}
}
}
/// Choose the device to calibrate
XID device_id = (XID) -1;
const char* device_name = NULL;
XYinfo device_axys;
if (fake) {
// Fake a calibratable device
device_name = "Fake_device";
device_axys = XYinfo(0,1000,0,1000);
if (verbose) {
printf("DEBUG: Faking device: %s\n", device_name);
}
} else {
// Find the right device
int nr_found = find_device(pre_device, list_devices, device_id, device_name, device_axys);
if (list_devices) {
// printed the list in find_device
if (nr_found == 0)
printf("No calibratable devices found.\n");
exit(2);
}
if (nr_found == 0) {
if (pre_device == NULL)
fprintf (stderr, "Error: No calibratable devices found.\n");
else
fprintf (stderr, "Error: Device \"%s\" not found; use --list to list the calibratable input devices.\n", pre_device);
exit(1);
} else if (nr_found > 1) {
printf ("Warning: multiple calibratable devices found, calibrating last one (%s)\n\tuse --device to select another one.\n", device_name);
}
if (verbose) {
printf("DEBUG: Selected device: %s\n", device_name);
}
}
// Different device/driver, different ways to apply the calibration values
try {
// try Usbtouchscreen driver
calibrator = new CalibratorUsbtouchscreen(device_name, device_axys,
thr_misclick, thr_doubleclick, output_type, geometry);
} catch(WrongCalibratorException& x) {
if (verbose)
printf("DEBUG: Not usbtouchscreen calibrator: %s\n", x.what());
}
if (calibrator == NULL) {
try {
// next, try Evdev driver (with XID)
calibrator = new CalibratorEvdev(device_name, device_axys, device_id,
thr_misclick, thr_doubleclick, output_type, geometry);
} catch(WrongCalibratorException& x) {
if (verbose)
printf("DEBUG: Not evdev calibrator: %s\n", x.what());
}
}
// lastly, presume a standard Xorg driver (evtouch, mutouch, ...)
if (calibrator == NULL) {
calibrator = new CalibratorXorgPrint(device_name, device_axys,
thr_misclick, thr_doubleclick, output_type, geometry);
}
if (calibrator != NULL) {
// override min/max XY from command line ?
if (precalib) {
if (verbose) {
printf("DEBUG: Setting precalibration: %i, %i, %i, %i\n",
pre_axys.x.min, pre_axys.x.max,
pre_axys.y.min, pre_axys.y.max);
}
calibrator->set_old_axys(pre_axys);
} else {
calibrator->detect_axys();
}
if (reset) {
if (verbose)
printf("DEBUG: Resetting calibration parameters:\n");
if (calibrator->apply(device_axys))
calibrator->set_old_axys(device_axys);
else
fprintf(stderr, "Error: failed to reset calibration parameters.\n");
}
}
return calibrator;
}
//int _tmain(int argc, _TCHAR* argv[])
uint main(uint argc, cstr argv[])
{
printf("Random number from 0 to 1 == %lf\n", LRandomFromTo(0., 1.));
LString typeOfCalc;
Calibrator *pC = getCalibrator();
Voltmeter *pV = getVoltmeter(Voltmeter::REFERENCE);
Voltmeter *pVver = getVoltmeter(Voltmeter::VERIFIED);
double Umax = 0., U = 0., step = 0., E1 = 0., E2 = 0., k = 0.1;
const double minThreshold = 0.0000001;
uint F = 0, i = 0, j = 0, ccount = 0;
//char *command[4] = {"Umax","U","F","Eref"};
// Umax = 0 volt U = 0 volt F = 0 Hz Eref = 0.0 volt
// Umax=0 - обязательны параметр нужен для того, чтобы мы не установили напряжение больше порога
// если передается параметр Eref - то устанавливаем эдс, возвращаем U
// елси не передается параметр Eref - то устанавливаем U и возвращаем эдс
// for example fix(equ) U=10 F=1000 Umax=20 Eref=0.508185
typeOfCalc = argv[1];
if (typeOfCalc == "fix") { // установить напряжение
arg[3].seen = true;
}
else if (typeOfCalc == "equ") { // подобрать напряжение
arg[3].req = true;
}
else if(typeOfCalc == "res") { // сбросить напряжение на 0 закрыть выход
arg[0].req = false;
arg[1].req = false;
arg[2].req = false;
}
else if(typeOfCalc == "ref") { // вернуть эдс эталона
arg[0].req = false;
arg[1].req = false;
arg[2].req = false;
}
else if(typeOfCalc == "ver") { // вернуть эдс тестируемого преобразователя
arg[0].req = false;
arg[1].req = false;
arg[2].req = false;
}
else {
fprintf(stderr, "Wrong command: %s\n", argv[1]);
exit(1);
}
for (i = 2; i < argc; i++) {
bool good = false;
for (j = 0; j < countof(arg); j++) {
LString res = getValue(arg[j].key, argv[i]);
if (res == " ") continue;
good = true;
if (arg[j].seen) {
fprintf(stderr, "Duplicate or prohibited: %s\n", arg[j].key);
exit(1);
}
switch (j) {
case 0: Umax = LParseDbl(res); break;
case 1: U = LParseDbl(res); break;
case 2: F = LParseInt(res); break;
case 3: E1 = LParseDbl(res); break;
}
printf("%d=%s\n", j, (cstr)res);
arg[j].seen = true;
}
if (!good) {
fprintf(stderr, "Unexpected: %s\n", argv[i]);
exit(1);
}
}
for (j = 0; j < countof(arg); j++) {
if (arg[j].req) if (!arg[j].seen) {
fprintf(stderr, "Required but not seen: %s\n", arg[j].key);
exit(1);
}
}
if (Umax <= 0. && typeOfCalc != "res" && typeOfCalc != "ver" && typeOfCalc != "ref") {
fprintf(stderr, "Invalid Umax: %lf\n", Umax);
exit(1);
}
if (typeOfCalc == "fix") if (fabs(U) < EPSILON) {
fprintf(stderr, "Invalid U: %lf\n", Umax);
exit(1);
}
printf("Umax=%lf U=%lf F=%d Eref=%lf\n", Umax, U, F, E1);
pC->setBarrier(Umax);
if (typeOfCalc == "fix") {
pC->setOutput(0);
pC->setVoltage(0);
runUpExact(U, F, pC);
E1 = pV->getVoltage();
printf("set voltage -> %2f V | set frequency -> %d Hz | thermo EMF -> %1f\n", U, F, E1);
fprintf(stderr, "Eref=%f", E1);
}
if (typeOfCalc == "equ") {
E2 = pV->getVoltage();
if (E1 < E2) {
step = U*k;
}
else{
step = U*(-k);
}
while (fabs(step) > minThreshold) {
if (Umax <= U) {
fprintf(stderr, "U is greater than Umax\n");
exit(1);
}
runUpExact(U, F, pC);
E2 = pV->getVoltage();
printf("set voltage -> %2f V | set frequency -> %d Hz | thermo EMF -> %1f\n", U, F, E2);
U += step; // этот способ нахождения оказался быстрее всех
if (((E1 < E2) && (step > 0)) || ((E1 > E2) && (step < 0))) {
step *= (-k);
}
}
fprintf(stderr, "Eref=%1f", pV->getVoltage());
}
if (typeOfCalc == "res"){
printf("reset\n");
pC->setOutput(0);
pC->setVoltage(0);
}
if (typeOfCalc == "ref"){
printf("pV->getVoltage=%1f\n", pV->getVoltage());
fprintf(stderr, "Eref=%1f", pV->getVoltage());
}
if (typeOfCalc == "ver"){
printf("pVver->getVoltage=%1f\n", pVver->getVoltage());
fprintf(stderr, "Ever=%1f", pVver->getVoltage());
}
return 0;
}
int main(int argc, char* argv[]){
std::string pose_offset_filename = "./poseoffset";
unsigned cv_width = 128;
unsigned cv_height = 128;
unsigned cv_depth = 128;
float cv_min_d = 0.5;
float cv_max_d = 3.0;
bool undistort = false;
unsigned idwneighbours = 10;
bool using_nni = false;
CMDParser p("basefilename samplesfilename checkerboardviewinitfilename");
p.addOpt("p",1,"poseoffetfilename", "specify the filename of the poseoffset on disk, default: " + pose_offset_filename);
p.addOpt("s",3,"size", "use this calibration volume size (width x height x depth), default: 128 128 256");
p.addOpt("d",2,"depthrange", "use this depth range: 0.5 4.5");
p.addOpt("u", -1, "undistort", "enable undistortion of images before chessboardsampling, default: false");
p.addOpt("n",1,"numneighbours", "the number of neighbours that should be used for IDW inverse distance weighting, default: 10");
p.addOpt("i",-1,"nni", "do use natural neighbor interpolation if possible, default: false");
p.init(argc,argv);
if(p.getArgs().size() != 3)
p.showHelp();
if(p.isOptSet("p")){
pose_offset_filename = p.getOptsString("p")[0];
std::cout << "setting poseoffetfilename to " << pose_offset_filename << std::endl;
}
if(p.isOptSet("s")){
cv_width = p.getOptsInt("s")[0];
cv_height = p.getOptsInt("s")[1];
cv_depth = p.getOptsInt("s")[2];
}
if(p.isOptSet("d")){
cv_min_d = p.getOptsInt("d")[0];
cv_max_d = p.getOptsInt("d")[1];
}
if(p.isOptSet("u")){
undistort = true;
}
if(p.isOptSet("n")){
idwneighbours = p.getOptsInt("n")[0];
std::cout << "setting to numneighbours " << idwneighbours << std::endl;
}
if(p.isOptSet("i")){
using_nni = true;
}
std::string basefilename = p.getArgs()[0];
std::string filename_xyz(basefilename + "_xyz");
std::string filename_uv(basefilename + "_uv");
const std::string filename_yml(basefilename + "_yml");
std::string filename_samples(p.getArgs()[1]);
CalibVolume cv(cv_width, cv_height, cv_depth, cv_min_d, cv_max_d);
RGBDConfig cfg;
cfg.read(filename_yml.c_str());
RGBDSensor sensor(cfg);
Checkerboard cb;
cb.load_pose_offset(pose_offset_filename.c_str());
ChessboardSampling cbs(p.getArgs()[2].c_str(), cfg, undistort);
cbs.init();
glm::mat4 eye_d_to_world = sensor.guess_eye_d_to_world_static(cbs, cb);
std::cerr << "extrinsic of sensor is: " << eye_d_to_world << std::endl;
std::cerr << "PLEASE note, the extrinsic guess can be improved by averaging" << std::endl;
for(unsigned z = 0; z < cv.depth; ++z){
for(unsigned y = 0; y < cv.height; ++y){
for(unsigned x = 0; x < cv.width; ++x){
const unsigned cv_index = (z * cv.width * cv.height) + (y * cv.width) + x;
const float depth = (z + 0.5) * (cv.max_d - cv.min_d)/cv.depth + cv.min_d;
const float xd = (x + 0.5) * sensor.config.size_d.x * 1.0/cv.width;
const float yd = (y + 0.5) * sensor.config.size_d.y * 1.0/cv.height;
glm::vec3 pos3D_local = sensor.calc_pos_d(xd, yd, depth);
glm::vec2 pos2D_rgb = sensor.calc_pos_rgb(pos3D_local);
pos2D_rgb.x /= sensor.config.size_rgb.x;
pos2D_rgb.y /= sensor.config.size_rgb.y;
glm::vec4 pos3D_world = eye_d_to_world * glm::vec4(pos3D_local.x, pos3D_local.y, pos3D_local.z, 1.0);
xyz pos3D;
pos3D.x = pos3D_world.x;
pos3D.y = pos3D_world.y;
pos3D.z = pos3D_world.z;
cv.cv_xyz[cv_index] = pos3D;
uv posUV;
posUV.u = pos2D_rgb.x;
posUV.v = pos2D_rgb.y;
cv.cv_uv[cv_index] = posUV;
}
}
}
// load samples from filename
std::vector<samplePoint> sps;
std::ifstream iff(filename_samples.c_str(), std::ifstream::binary);
const unsigned num_samples_in_file = calcNumFrames(iff,
sizeof(float) +
sizeof(uv) +
sizeof(uv) +
sizeof(xyz) +
sizeof(uv) +
sizeof(glm::vec3) +
sizeof(float));
for(unsigned i = 0; i < num_samples_in_file; ++i){
samplePoint s;
iff.read((char*) &s.depth, sizeof(float));
iff.read((char*) &s.tex_color, sizeof(uv));
iff.read((char*) &s.tex_depth, sizeof(uv));
iff.read((char*) &s.pos_offset, sizeof(xyz));
iff.read((char*) &s.tex_offset, sizeof(uv));
iff.read((char*) glm::value_ptr(s.pos_real), sizeof(glm::vec3));
iff.read((char*) &s.quality, sizeof(float));
sps.push_back(s);
}
iff.close();
// reapply correction offsets
for(unsigned i = 0; i < sps.size(); ++i){
const unsigned cv_width = cv.width;
const unsigned cv_height = cv.height;
const unsigned cv_depth = cv.depth;
const float x = cv_width * ( sps[i].tex_depth.u) / cfg.size_d.x;
const float y = cv_height * ( sps[i].tex_depth.v)/ cfg.size_d.y;
const float z = cv_depth * ( sps[i].depth - cv.min_d)/(cv.max_d - cv.min_d);
xyz pos = getTrilinear(cv.cv_xyz, cv_width, cv_height, cv_depth, x , y , z );
uv tex = getTrilinear(cv.cv_uv, cv_width, cv_height, cv_depth, x , y , z );
sps[i].pos_offset.x = sps[i].pos_real[0] - pos.x;
sps[i].pos_offset.y = sps[i].pos_real[1] - pos.y;
sps[i].pos_offset.z = sps[i].pos_real[2] - pos.z;
sps[i].tex_offset.u = sps[i].tex_color.u/cfg.size_rgb.x - tex.u;
sps[i].tex_offset.v = sps[i].tex_color.v/cfg.size_rgb.y - tex.v;
sps[i].quality = 1.0f;
}
Calibrator c;
c.using_nni = using_nni;
c.applySamples(&cv, sps, cfg, idwneighbours, basefilename.c_str());
cv.save(filename_xyz.c_str(), filename_uv.c_str());
return 0;
}
