char BMP280::begin()
{
// Start up the Arduino's "wire" (I2C) library:
Wire.begin();
return (readCalibration());
}
ComediAnalogIOSoftCal::ComediAnalogIOSoftCal(const char *deviceFile, uint subdevice,
uint *channels, uint nChannels,
uint range, uint aref)
: ComediAnalogIO(deviceFile, subdevice, channels, nChannels, range, aref)
{
Logger(Debug, "ComediAnalogIOSoftCal::ComediAnalogIOSoftCal()\n");
if (!readCalibration())
throw "Unable to read the calibration of the DAQ board.";
}
QWSCalibratedMouseHandler::QWSCalibratedMouseHandler(const QString &, const QString &)
: samples(5), currSample(0), numSamples(0)
{
clearCalibration();
readCalibration();
}
/*
* Initialize library and coefficient for measurements
*/
char BMP280::begin(int sdaPin, int sclPin)
{
Wire.begin(sdaPin,sclPin);
return (readCalibration());
}
uint_least8_t ADS7846::doCalibration(MI0283QT9 *lcd, uint16_t eeprom_addr, uint_least8_t check_eeprom) //touch panel calibration routine
{
uint_least8_t i;
CAL_POINT lcd_points[3] = {CAL_POINT1, CAL_POINT2, CAL_POINT3}; //calibration point postions
CAL_POINT tp_points[3];
//calibration data in EEPROM?
if(readCalibration(eeprom_addr) && check_eeprom)
{
return 0;
}
//clear screen and wait for touch release
lcd->fillScreen(RGB(255,255,255));
#if defined(__AVR__)
lcd->drawTextPGM((lcd->getWidth()/2)-50, (lcd->getHeight()/2)-10, PSTR("Calibration"), RGB(0,0,0), RGB(255,255,255), 1);
#else
lcd->drawText((lcd->getWidth()/2)-50, (lcd->getHeight()/2)-10, "Calibration", RGB(0,0,0), RGB(255,255,255), 1);
#endif
while(getPressure() > MIN_PRESSURE){ service(); };
//show calibration points
for(i=0; i<3; )
{
//draw points
lcd->drawCircle(lcd_points[i].x, lcd_points[i].y, 2, RGB( 0, 0, 0));
lcd->drawCircle(lcd_points[i].x, lcd_points[i].y, 5, RGB( 0, 0, 0));
lcd->drawCircle(lcd_points[i].x, lcd_points[i].y, 10, RGB(255, 0, 0));
//run service routine
service();
//press dectected? -> save point
if(getPressure() > MIN_PRESSURE)
{
lcd->fillCircle(lcd_points[i].x, lcd_points[i].y, 2, RGB(255,0,0));
tp_points[i].x = getXraw();
tp_points[i].y = getYraw();
i++;
//wait and redraw screen
delay(100);
lcd->fillScreen(RGB(255,255,255));
#if defined(__AVR__)
lcd->drawTextPGM((lcd->getWidth()/2)-50, (lcd->getHeight()/2)-10, PSTR("Calibration"), RGB(0,0,0), RGB(255,255,255), 1);
#else
lcd->drawText((lcd->getWidth()/2)-50, (lcd->getHeight()/2)-10, "Calibration", RGB(0,0,0), RGB(255,255,255), 1);
#endif
}
}
//calulate calibration matrix
setCalibration(lcd_points, tp_points);
//save calibration matrix
writeCalibration(eeprom_addr);
//wait for touch release
while(getPressure() > MIN_PRESSURE){ service(); };
return 1;
}
int main(int argc, char **argv)
{
Timerx timer;
if(argc != 5)
{
cerr << "Syntax error. You must provide all these parameters: " << argv[0] << " matcheds_folder calibration_file options_file use_prior (0|1)" << endl;
return -1;
}
char* folder = argv[1];
bool cost = atoi(argv[4]);
Calibration calib = readCalibration(argv[2]);
Options localopts = readOptions(argv[3]);
//Window creation
int win_width = 800;
int win_height = 800;
RenderVO* renderer = new RenderVO("Visual Odometry", win_width, win_height);
//matcheds files
vector<string> files = getDirAsVector(folder);
MatrixXd currentPose = MatrixXd::Identity(4, 4);
bool quit = false;
for(int i=0; i<files.size(); i++)
{
string current_file;
current_file = string(folder) + "/" + files[i];
cout << current_file << endl;
multiMatrix matches = readData(current_file.c_str(), cost);
//estimate the current pose
MatrixXd sol = backprojAVGSACOpt(matches.LC, matches.RC, matches.LP, matches.RP, calib, localopts);
currentPose = currentPose * sol;
cout << currentPose << endl;
//adding new pose and landmarks
renderer->addPose(currentPose, 1);
//Showing images and rendering scene
renderer->update();
//Detecting events
int key = waitKey(5);
switch(char(key))
{
case 'p':
int k2;
do{ k2 = waitKey(-1); } while( (char(k2) != 'p') && (char(k2) != 'q') );
if(char(k2) == 'q')
quit = true;
break;
case 'q':
quit = true;
}
if(quit)
break;
}
if(!quit)
int key = waitKey(-1);
delete renderer;
return 0;
}
