/**
* Checks if button_2 was pressed. Launch calibration or measurement action
*/
void measure_button()
{
if (button_pressed_2)
{
if (calibrate)
{
printf("calibration necesary\n");
calibration();
}
else
{
last_meassured_weight = measure_time_weight(1);
printf("WEIGHT %g \n", roundNo_f(last_meassured_weight, 2));
delay_ms(2000);
}
button_pressed_2 = false;
}
}
void perform(char *argv[])
{
int num = 0;
char cmd = argv[1][1];
switch (cmd)
{
case 'c':
case 'C':
num = select_calibrate(argv[2][0]);
calibration(num);
break;
case 'n':
case 'N':
normal_mode();
break;
/* case 'r':*/
/* case 'R':*/
/* raster_mode();*/
/* break;*/
/* case 'd':*/
/* case 'D':*/
/* num = atoi(argv[2]);*/
/* //debug_mode(num, DEBUG_MODE);*/
/* break;*/
/* case 'v':*/
/* case 'V':*/
/* get_version();*/
/* break;*/
/* case 'i':*/
/* case 'I':*/
/* num = atoi(argv[2]);*/
/* debug_mode(num, INTERNAL_MODE);*/
/* break;*/
/* case 'b':*/
/* case 'B':*/
/* {*/
/* char *fname = argv[2];*/
/* bootloader_mode(fname);*/
/* }*/
/* break;*/
}
}
void baro_MS5534A_init(void)
{
/* 32768Hz on PWM2 */
/* Configure P0.7 pin as PWM */
PINSEL0 &= ~(_BV(14));
PINSEL0 |= _BV(15);
/* No prescaler */
PWMPR = PWM_PRESCALER - 1;
/* To get 32768Hz from a base frequency of 15MHz */
PWMMR0 = PWM_PERIOD;
PWMMR2 = PWM_DUTY;
PWMLER = PWMLER_LATCH0 | PWMLER_LATCH2;
PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
PWMPCR = PWMPCR_ENA2;
#ifdef BARO_MS5534A_W1
words[0] = BARO_MS5534A_W1;
words[1] = BARO_MS5534A_W2;
words[2] = BARO_MS5534A_W3;
words[3] = BARO_MS5534A_W4;
status = STATUS_MEASURE_PRESSURE;
status_read_data = FALSE;
calibration();
#else
status = STATUS_INIT1;
status_read_data = FALSE;
#endif
baro_MS5534A_available = FALSE;
alt_baro_enabled = FALSE;
baro_MS5534A_ground_pressure = 101300;
baro_MS5534A_r = 20.;
baro_MS5534A_sigma2 = 1;
baro_MS5534A_do_reset = FALSE;
}
int main(void)
{
//Init Motor
/*
BYTE_SET(DDRD,0xFE);
PORTB = 0xFF;
PORTB = 0xC0;
DDRA = 0x10;
PORTA = 0x00;
*/
initAdc();
initUART();
initTimer();
// DDRC=0xFF;
//Enable interrupt after all init else Infinite loop
sei();
calibration();
//Enable_watchdog(WDTO_500MS);
for (;;)
{ /* loop forever */
// lireTelecommande();
/* if(timerOvf == 1)
{
*/
Vitesse_D = getUartVitesse();
Angle_D = getUartAngle();
Vg = getAvgSpeedMoteurGauche();
Vd = getAvgSpeedMoteurDroit();
CalculPWM(Vitesse_D,Angle_D,Vg,Vd,&Duty_G,&Duty_D);
/* timerOvf = 0;
}
*/
}
}
void setup()
{
Wire.begin(myAddress);
Wire.onReceive(i2cReceive);
Wire.onRequest(i2cRequest);
pinMode(limit_switch_pin, INPUT);
mySerial.begin(9600); // Serial commuication to motor controller start.
setpoint = 0.0;
calibration(); // Running the calibration code on every start up
input = encoderRead();
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(-127, 127);
myPID.SetSampleTime(20);
}
void MainWindow::createConnections()
{
connect(ui->actionNew, SIGNAL(triggered()), this, SLOT(newproject()));
connect(ui->actionOpen, SIGNAL(triggered()), this, SLOT(openproject()));
connect(ui->actionOpenCamera, SIGNAL(triggered()), this, SLOT(opencamera()));
connect(ui->actionFocusAssistant, SIGNAL(triggered()), this, SLOT(startfocusassistant()));
connect(ui->leftExSlider,SIGNAL(valueChanged(int)),this,SLOT(setexposure()));
connect(ui->rightExSlider,SIGNAL(valueChanged(int)),this,SLOT(setexposure()));
connect(ui->actionBasler, SIGNAL(triggered()), this, SLOT(usebasler()));//暂时用来调试的功能
connect(ui->actionProjector,SIGNAL(triggered()),this,SLOT(projectorcontrol()));
connect(ui->tabWidget, SIGNAL(currentChanged(int)), this, SLOT(selectPath(int)));
connect(ui->actionCalib, SIGNAL(triggered()), this, SLOT(calib()));
connect(ui->captureButton, SIGNAL(clicked()), this, SLOT(capturecalib()));
connect(ui->redoButton, SIGNAL(clicked()), this, SLOT(redocapture()));
connect(ui->calibButton,SIGNAL(clicked()),this,SLOT(calibration()));
connect(ui->actionScan,SIGNAL(triggered()),this,SLOT(scan()));
connect(ui->findPointButton,SIGNAL(clicked()),this,SLOT(pointmatch()));
connect(ui->reFindButton,SIGNAL(clicked()),this,SLOT(refindmatch()));
connect(ui->startScanButton, SIGNAL(clicked()), this, SLOT(startscan()));
connect(ui->multiFreqTest, SIGNAL(clicked()), this, SLOT(testmulitfreq()));
//connect(ui->testR, SIGNAL(clicked()), this, SLOT(test()));//用于测试功能的暂时按键
connect(ui->actionReconstruct,SIGNAL(triggered()),this,SLOT(reconstruct()));
connect(ui->reconstructionButton,SIGNAL(clicked()),this,SLOT(startreconstruct()));
connect(ui->actionSet, SIGNAL(triggered()), this, SLOT(set()));
connect(ui->actionChinese, SIGNAL(triggered()), this, SLOT(switchlanguage()));
connect(ui->actionEnglish, SIGNAL(triggered()), this, SLOT(switchlanguage()));
connect(ui->pSizeValue, SIGNAL(valueChanged(int)), this, SLOT(changePointSize(int)));
connect(ui->loadTest, SIGNAL(clicked()), this, SLOT(loadTestModel()));
connect(ui->actionExit, SIGNAL(triggered()), pj, SLOT(close()));//解决投影窗口不能关掉的问题
connect(ui->actionExit, SIGNAL(triggered()), fa, SLOT(close()));
connect(ui->actionExit, SIGNAL(triggered()), this, SLOT(close()));
}
int main(int argc, const char * argv[])
{
// Change this calibration to yours:
CameraCalibration calibration(545.31565719766058f, 545.31565719766058f, 326.0f, 183.5f);
std::cout << "Input image not specified" << std::endl;
std::cout << "Usage: markerless_ar_demo [filepath to recorded video or image]" << std::endl;
if (argc == 1)
{
processVideo(calibration, cv::VideoCapture());
}
else if (argc == 2)
{
std::string input = argv[1];
cv::Mat testImage = cv::imread(input);
if (!testImage.empty())
{
processSingleImage( calibration, testImage);
}
else
{
cv::VideoCapture cap;
if (cap.open(input))
{
processVideo( calibration, cap);
}
}
}
else
{
std::cerr << "Invalid number of arguments passed" << std::endl;
return 1;
}
return 0;
}
void main(void)
{
sei();
INIT_IO();
INIT_USART();
INIT_TIMER();
DDRB=0xff;
DDRD=0x30;
PORTB=0x06;
PORTC=0xff;
ADMUX=0x60;
ADCSRA=0xcc;
while(1)
{
while(calib_flg)
{
calibration();
}
sensing();
Catch_position();
OCR1A=position_R[position];
OCR1B=position_L[position];
}
}
void main(void) {
/* Configure the oscillator for the device */
ConfigureOscillator();
/* Initialize I/O and Peripherals for application */
InitApp();
resetCS();
startCSConversion();
line1();
writeLCD("START CALIBRATION");
calibration();
writeRegister(MODE, 0x00006000);
writeRegister(VOLTAGE_GAIN, 0x2F9000);
writeRegister(CURRENT_GAIN, 0x3Fe0000);
startCSConversion();
while (1) {
uint24_t status = readRegister(STATUS_REG);
line2();
if (!(status & 0x1)) {
convertExe(status);
writeLCD(buffer);
resetCS();
startCSConversion();
continue;
}
double readVal;
const char * measure;
switch (showStatus) {
case ssConf:
data.lData = readRegister(showStatus);
sprintf(buffer, "%02X%02X%02X ", data.bytes[2], data.bytes[1], data.bytes[0]);
measure = "CONF ";
break;
case ssCycleCount:
data.lData = readRegister(showStatus);
sprintf(buffer, "%d ", data.lData);
measure = "CYCLE ";
break;
case ssStatus:
data.lData = readRegister(showStatus);
sprintf(buffer, "%02X%02X%02X ", data.bytes[2], data.bytes[1], data.bytes[0]);
measure = "STATUS ";
break;
case ssMode:
data.lData = readRegister(showStatus);
sprintf(buffer, "%02X%02X%02X ", data.bytes[2], data.bytes[1], data.bytes[0]);
measure = "MODE ";
break;
case ssTemperature:
data.lData = readRegister(showStatus);
sprintf(buffer, "%02X%02X%02X ", data.bytes[2], data.bytes[1], data.bytes[0]);
measure = "TEMP ";
break;
case ssCurrentGain:
data.lData = readRegister(showStatus);
readVal = convertCurrentGain(data.lData);
measure = "I GAIN ";
sprintf(buffer, "%f ", readVal);
break;
case ssVoltageOffset:
data.lData = readRegister(showStatus);
readVal = convertOffset(data.lData);
measure = "VDC offset";
sprintf(buffer, "%f ", readVal);
break;
case ssVoltageGain:
data.lData = readRegister(showStatus);
readVal = convertCurrentGain(data.lData);
measure = "V GAIN ";
sprintf(buffer, "%f ", readVal);
break;
case ssVoltageAcOffset:
data.lData = readRegister(showStatus);
readVal = convertOffset(data.lData);
measure = "VAC offset";
sprintf(buffer, "%f ", readVal);
break;
case ssCurrent:
data.lData = readRegister(showStatus);
readVal = convertCurrent(data.lData);
measure = "CURRENT ";
sprintf(buffer, "%f ", readVal);
break;
case ssVolt:
data.lData = readRegister(INST_VOLT);
readVal = convertInstVolt(data.lData);
measure = "VOLT ";
sprintf(buffer, "%f ", readVal);
break;
case ssRMSVoltage:
data.lData = readRegister(ssRMSVoltage);
readVal = convertRMSVolt(data.lData);
sprintf(buffer, "%f ", readVal);
measure = "RMS VOLT";
break;
case ssRMSCurrent:
data.lData = readRegister(ssRMSCurrent);
readVal = convertRMSCurrent(data.lData);
sprintf(buffer, "%f ", readVal);
measure = "RMS I";
break;
case ssPower:
data.lData = readRegister(INST_POWER);
readVal = convertPower(data.lData);
measure = "I POWER ";
sprintf(buffer, "%f ", readVal);
break;
case ssActivePower:
data.lData = readRegister(ssActivePower);
readVal = convertPower(data.lData);
measure = "I POWER ";
sprintf(buffer, "%f ", readVal);
break;
default:
data.lData = readRegister(showStatus);
sprintf(buffer, "%02X%02X%02X ", data.bytes[2], data.bytes[1], data.bytes[0]);
measure = " ";
break;
}
line1();
writeLCD(buffer);
line2();
writeLCD(measure);
if (PORTBbits.RB2 == 0) {
if (PORTBbits.RB2 == 0) {
showStatus++;
if (showStatus == ssLast) {
showStatus = ssConf;
}
while (1) {
if (PORTBbits.RB2 == 1) {
if (PORTBbits.RB2 == 1) {
break;
}
}
}
}
}
}
}
int main(int argc, char *argv[])
{
int c;
int clone_err;
int i,rc;
int exit_rc = 0;
int wait_status;
int iterations = 0;
struct sched_param param;
struct rlimit myrlimit;
struct sembuf mysembuf1;
num_active = num_children ; /* In case its unspecified */
while ((c = getopt(argc, argv, "c:t:a:w:r:o:q")) != -1) {
switch (c) {
case 'c': num_children = atoi(optarg); break;
case 't': num_seconds = atoi(optarg); break;
case 'a': num_active = atoi(optarg); break;
case 'w': weight_reschedule_idle = atoi(optarg); break;
case 'r': rnd_compute_time = atoi(optarg); break;
case 'o': foutput = atoi(optarg); break;
case 'q': fquiet = 1; break;
default: usage();
goto exit_main2;
}
}
if ((num_seconds <= 0) ||
(num_children <= 0) || (num_children > MAX_CHILDREN) ||
(num_active <= 0) || (num_active > num_children) ||
(weight_reschedule_idle < 0) || (weight_reschedule_idle > 100) ||
(rnd_compute_time < 0)
)
{
usage();
goto exit_main2;
}
/* Increase limits on number of open files */
/* normally 1024 (cur & max), set to MAX_CHILDREN */
myrlimit.rlim_cur = myrlimit.rlim_max = MAX_CHILDREN*2 ;
if (setrlimit(RLIMIT_NOFILE,&myrlimit) != 0) {
exit_rc = errno ;
perror("setrlimit() ");
goto exit_main2;
}
/* allocate childrens stacks*/
child_stack = malloc(num_children*STACK_SIZE);
if (child_stack == NULL) {
exit_rc = errno;
perror ("malloc of 'child_stack' failed ");
goto exit_main2;
}
/* open num_children pipes */
for (i=0; i< num_children; i++) {
if (pipe(childpipe[i]) < 0) {
exit_rc = errno;
perror ("pipe() ");
goto exit_main3;
}
}
/* calibrate internal loops */
calibration();
probyield = local_exec_count ;
TOKENSZ = sizeof(char);
probyield = (100.0-(double)weight_reschedule_idle)/100.0 ;
probyieldmult = probyield / ((1-probyield)*(1-probyield));
/*
comp_nonyield_rounds = (int) (uniform(rnd_compute_time) * rounds_per_microsecond) ;
comp_yield_rounds = (int) (comp_nonyield_rounds * probyieldmult) ;
printf("Sample subrounds : nonyield %d yield %d, probyield %0.3f\n",
comp_nonyield_rounds, comp_yield_rounds, probyield);
*/
/* start_sem is used to start all children at same time */
start_sem = semget (IPC_PRIVATE, 1, IPC_CREAT | IPC_EXCL | 0660);
if (start_sem == -1) {
exit_rc = errno;
perror("semget(start_sem) IPC_CREATE ");
goto exit_main4;
}
/* allocate/initialize statistic variables */
total = malloc(num_children*sizeof(struct _total));
if (total == NULL) {
exit_rc = errno;
perror ("malloc of 'total' failed ");
goto exit_main3;
}
for (i = 0 ; i < num_children ; i++)
total[i].count = 0;
/* Launch threads */
worker = bouncer;
for (i=0; i< num_children; i++) {
clone_err = __clone(worker,
&child_stack[(i+1)*STACK_SIZE],
CLONE_FLAGS,
(void*)i);
if (clone_err == -1) {
exit_rc = errno;
perror ("clone() ");
goto exit_main5;
}
// printf("\t\tLaunched child %d\n",i);
}
/* Increase priority of parent thread */
param.sched_priority = 90;
rc = sched_setscheduler(getpid(), SCHED_FIFO, ¶m);
if (rc == -1) {
exit_rc = errno;
perror ("sched_setscheduler() ");
goto exit_main5;
}
run_test_time();
exit_main5:
/* wait until all children complete */
for (i = 0 ; i < num_children ; i++) {
rc = waitpid (-1, &wait_status, __WCLONE);
if (rc == -1) {
exit_rc = errno;
perror ("waitpid() ");
}
}
exit_main4:
rc = semctl(start_sem, 0, IPC_RMID, 0);
exit_main3:
/* explicitly close all pipes : unnecessary */
for (i=0; i< num_children; i++) {
close(childpipe[i][0]);
close(childpipe[i][1]);
}
free(child_stack);
exit_main2:
return (exit_rc) ;
}
int main(int argc, const char * argv[])
{
// franquy parameters
float fx = 695.4521167717107;
float fy = 694.5519610122569;
float cx = 337.2059936807979;
float cy = 231.1645822893514;
// tablet parameters
fx=628.6341119951087;
fy=628.7519411113429;
cx=325.3443919995285;
cy=236.0028199018263;
// Change this calibration to yours:
//~ CameraCalibration calibration(526.58037684199849f, 524.65577209994706f, 318.41744018680112f, 202.96659047014398f);
CameraCalibration calibration(fx,fy,cx,cy);
if (argc < 2)
{
std::cout << "Input image not specified" << std::endl;
std::cout << "Usage: markerless_ar_demo <pattern image> [filepath to recorded video or image]" << std::endl;
return 1;
}
// Try to read the pattern:
cv::Mat patternImage = cv::imread(argv[1]);
if (patternImage.empty())
{
std::cout << "Input image cannot be read" << std::endl;
return 2;
}
if (argc == 2)
{
cv::VideoCapture cap(0);
processVideo(patternImage, calibration, cap);
}
else if (argc == 3)
{
std::string input = argv[2];
cv::Mat testImage = cv::imread(input);
if (!testImage.empty())
{
processSingleImage(patternImage, calibration, testImage);
}
else
{
cv::VideoCapture cap(0);
if (cap.open(input))
{
processVideo(patternImage, calibration, cap);
}
}
}
else
{
std::cerr << "Invalid number of arguments passed" << std::endl;
return 1;
}
return 0;
}
void execute(void)
{
int num, type = 1;
char cmd, flag = 'n';
char fname[64] = "/auto.pix";
main_menu();
while (1)
{
printf("Input cmd: ");
cmd = getchar();
while(getchar()!='\n');
switch (cmd)
{
case 't':
case 'T':
ioctl(gldev, RESET_TP, 0);
break;
case 'c':
case 'C':
//num = input_calibrate_menu();
//num = select_calibrate(cmd);
num = 1;
calibration(num);
break;
/* case 'n':*/
/* case 'N':*/
/* normal_mode();*/
/* break;*/
/* case 'r':*/
/* case 'R':*/
/* raster_mode();*/
/* break;*/
/* case 'd':*/
/* case 'D':*/
/* input_debug_param(&type,&flag);*/
/* debug_mode(type, flag);*/
/* break;*/
case 'b':
case 'B':
input_fname(fname);
bootloader_mode(fname);
break;
/* case 'v':*/
/* case 'V':*/
/* get_version();*/
/* break;*/
/* */
/* case 'i':*/
/* case 'I':*/
/* input_debug_param(&type,&flag);*/
/* internal_mode(type, flag);*/
/* break;*/
/* case 't': */
/* case 'T':*/
/* button_raw();*/
/* break; */
/* case 'r':*/
/* case 'R':*/
/* read_eeprom();*/
/* break;*/
/* case 'w':*/
/* case 'W':*/
/* write_eeprom();*/
/* break;*/
case 'm':
case 'M':
main_menu();
break;
case 'x':
case 'X':
return;
}
}
}
int bootloader_mode(char *fname)
{
int i, ret, len, arg = 0;
FILE *fp;
char wbuf[WBUF];
unsigned char flag = 1;
unsigned char reset = 0;
fp = fopen(fname, "r");
if (fp == NULL)
{
puts("Open file error");
return -1;
}
ioctl(gldev, DISABLE_IRQ, 0);
ioctl(gldev, BOOTLOADER_MODE, arg);
while (1)
{
char rbuf[RBUF];
fgets(rbuf, RBUF, fp);
if (feof(fp)) break;
if( !strncmp(rbuf, "!delay", 6))
{
printf("%s", rbuf);
msleep(300);
continue;
}
else if ( rbuf[0]!='$') continue;
else if( !strncmp(rbuf, "$00", 3))
{
flag = 0;
}
if( !strncmp(rbuf, "$03", 3) && flag )
{
reset = 1;
flag = 0;
for (i = 0; i < WBUF; i++)
{
wbuf[i] = 0;
}
ret = write(gldev, wbuf, WBUF);
msleep(150);
if (ret != WBUF)
{
ioctl(gldev, ENABLE_IRQ, 0);
fclose(fp);
puts("Download failed.");
return 1;
}
}
for (i = 0; i < WBUF; i++)
{
wbuf[i] = (hex2char(rbuf[(i + 1) * 2 - 1]) << 4) | hex2char(rbuf[(i + 1) * 2]);
}
printf("%s", rbuf);
ret = write(gldev, wbuf, WBUF);
msleep(150);
if (ret != WBUF)
{
ioctl(gldev, ENABLE_IRQ, 0);
fclose(fp);
puts("Download failed.");
return 1;
}
}
ioctl(gldev, ENABLE_IRQ, 0);
fclose(fp);
puts("Download successed.");
if(ret==WBUF && reset )
{
ioctl(gldev, RESET_TP, 0);
sleep(1);
calibration(1);
}
return 1;
}
int CompassSensor::readEvents(sensors_event_t* data, int count)
{
if (count < 1)
return -EINVAL;
ssize_t n = mInputReader.fill(data_fd);
if (n < 0)
return n;
int numEventReceived = 0;
input_event const* event;
D("%s count = %d ", __func__, count);
while (count && mInputReader.readEvent(&event)) {
D("readEvents event->type = %d, code=%d, value=%d",
event->type, event->code, event->value);
int type = event->type;
if (type == EV_REL && !inputDataOverrun) {
if (event->code == EVENT_TYPE_M_O_X)
mMagneticEvent.data[mConfig->mapper[AXIS_X]] =
COMPASS_CONVERT(event->value, mConfig->scale[AXIS_X]);
else if (event->code == EVENT_TYPE_M_O_Y)
mMagneticEvent.data[mConfig->mapper[AXIS_Y]] =
COMPASS_CONVERT(event->value, mConfig->scale[AXIS_Y]);
else if (event->code == EVENT_TYPE_M_O_Z)
mMagneticEvent.data[mConfig->mapper[AXIS_Z]] =
COMPASS_CONVERT(event->value, mConfig->scale[AXIS_Z]);
} else if (type == EV_SYN) {
int64_t time = timevalToNano(event->time);
/* drop input event overrun data */
if (event->code == SYN_DROPPED) {
E("CompassSensor: input event overrun, dropped event:drop");
inputDataOverrun = 1;
} else if (inputDataOverrun) {
E("CompassSensor: input event overrun, dropped event:sync");
inputDataOverrun = 0;
} else {
if (mEnabled) {
mMagneticEvent.timestamp = time;
/* compass calibration */
calibration(time);
D("CompassSensor magnetic befor filter=[%f, %f, %f] accuracy=%d, time=%lld",
mMagneticEvent.magnetic.x,
mMagneticEvent.magnetic.y,
mMagneticEvent.magnetic.z,
(int)mMagneticEvent.magnetic.status,
mMagneticEvent.timestamp);
/* data filter: used to mitigate data floating */
if (mFilterEn)
filter();
*data++ = mMagneticEvent;
count--;
numEventReceived++;
D("CompassSensor magnetic=[%f, %f, %f] accuracy=%d, time=%lld",
mMagneticEvent.magnetic.x,
mMagneticEvent.magnetic.y,
mMagneticEvent.magnetic.z,
(int)mMagneticEvent.magnetic.status,
mMagneticEvent.timestamp);
}
}
} else {
E("CompassSensor: unknown event (type=%d, code=%d)",
type, event->code);
}
mInputReader.next();
}
return numEventReceived;
}
int
main(int argc, char** argv)
{
cv::Size boardSize;
float squareSize;
double delay;
int imageCount;
std::string cameraModel;
std::string cameraNs;
float minMove;
//========= Handling Program options =========
boost::program_options::options_description desc("Allowed options");
desc.add_options()
("help", "produce help message")
("width,w", boost::program_options::value<int>(&boardSize.width)->default_value(8), "Number of inner corners on the chessboard pattern in x direction")
("height,h", boost::program_options::value<int>(&boardSize.height)->default_value(5), "Number of inner corners on the chessboard pattern in y direction")
("size,s", boost::program_options::value<float>(&squareSize)->default_value(120.f), "Size of one square in mm")
("delay", boost::program_options::value<double>(&delay)->default_value(0.5), "Minimum delay in seconds between captured images")
("count", boost::program_options::value<int>(&imageCount)->default_value(50), "Number of images to be taken for the calibration")
("camera-model", boost::program_options::value<std::string>(&cameraModel)->default_value("mei"), "Camera model: kannala-brandt | mei | pinhole")
("camera-ns", boost::program_options::value<std::string>(&cameraNs)->default_value("cam"), "Camera namespace")
("move,m", boost::program_options::value<float>(&minMove)->default_value(20.f), "Minimal move in px of chessboard to be used")
;
boost::program_options::variables_map vm;
boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
boost::program_options::notify(vm);
if (vm.count("help"))
{
std::cout << desc << std::endl;
return 1;
}
px::Camera::ModelType modelType;
if (boost::iequals(cameraModel, "kannala-brandt"))
{
modelType = px::Camera::KANNALA_BRANDT;
}
else if (boost::iequals(cameraModel, "mei"))
{
modelType = px::Camera::MEI;
}
else if (boost::iequals(cameraModel, "pinhole"))
{
modelType = px::Camera::PINHOLE;
}
else
{
ROS_ERROR("Unknown camera model: %s", cameraModel.c_str());
return 1;
}
switch (modelType)
{
case px::Camera::KANNALA_BRANDT:
ROS_INFO("Camera model: Kannala-Brandt");
break;
case px::Camera::MEI:
ROS_INFO("Camera model: Mei");
break;
case px::Camera::PINHOLE:
ROS_INFO("Camera model: Pinhole");
break;
}
ros::init(argc, argv, "mono_camera_calibration");
ros::NodeHandle nh;
ros::Subscriber cameraInfoSub;
px_comm::CameraInfoPtr cameraInfo;
cameraInfoSub = nh.subscribe<px_comm::CameraInfo>(ros::names::append(cameraNs, "camera_info"), 1,
boost::bind(cameraInfoCallback, _1, boost::ref(cameraInfo)));
ROS_INFO("Waiting for camera information...");
ros::Rate r(50);
while (ros::ok() && cameraInfo.get() == 0)
{
ros::spinOnce();
r.sleep();
}
cameraInfoSub.shutdown();
if (cameraInfo.get() == 0)
{
ROS_ERROR("Aborted calibration due to missing camera information.");
return 1;
}
ROS_INFO("Received camera information.");
ROS_INFO(" Camera name: %s", cameraInfo->camera_name.c_str());
ROS_INFO(" Image width: %d", cameraInfo->image_width);
ROS_INFO(" Image height: %d", cameraInfo->image_height);
px::CameraCalibration calibration(modelType, cameraInfo->camera_name,
cv::Size(cameraInfo->image_width,
cameraInfo->image_height),
boardSize, squareSize / 1000.0f);
calibration.setVerbose(true);
cv::namedWindow("Image");
image_transport::ImageTransport it(nh);
px::AtomicContainer<cv::Mat> frame;
image_transport::Subscriber imageSub;
imageSub = it.subscribe(ros::names::append(cameraNs, "image_raw"), 1,
boost::bind(imageCallback, _1, boost::ref(frame)));
cv::Point2f lastFirstCorner = cv::Point2f(std::numeric_limits<float>::max(),
std::numeric_limits<float>::max());
ros::Time lastFrameTime;
cv::Mat imgView;
while (ros::ok() && calibration.sampleCount() < imageCount)
{
ros::spinOnce();
r.sleep();
if (!frame.available())
{
continue;
}
px::Chessboard chessboard(boardSize, frame.data());
chessboard.findCorners();
if (chessboard.cornersFound())
{
chessboard.getSketch().copyTo(imgView);
}
else
{
frame.data().copyTo(imgView);
}
if (chessboard.cornersFound() &&
(frame.timestamp() - lastFrameTime).toSec() > delay &&
cv::norm(cv::Mat(lastFirstCorner - chessboard.getCorners()[0])) > minMove)
{
lastFirstCorner = chessboard.getCorners()[0];
lastFrameTime = frame.timestamp();
cv::bitwise_not(imgView, imgView);
calibration.addChessboardData(chessboard.getCorners());
}
std::ostringstream oss;
oss << calibration.sampleCount() << " / " << imageCount;
cv::putText(imgView, oss.str(), cv::Point(10,20),
cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255),
1, CV_AA);
cv::imshow("Image", imgView);
cv::waitKey(2);
frame.available() = false;
}
cv::destroyWindow("Image");
if (calibration.sampleCount() < imageCount)
{
ROS_ERROR("Aborted calibration due to insufficient number of detected chessboards.");
return 1;
}
ROS_INFO("Calibrating...");
ros::Time startTime = ros::Time::now();
calibration.calibrate();
calibration.writeParameters(cameraInfo->camera_name + "_camera_calib.yaml");
calibration.writeParameters(cameraInfo);
calibration.writeChessboardData(cameraInfo->camera_name + "_chessboard_data.dat");
ROS_INFO("Calibration took a total time of %.1f sec.",
(ros::Time::now() - startTime).toSec());
ROS_INFO("Wrote calibration file to %s",
(cameraInfo->camera_name + "_camera_calib.yaml").c_str());
// send SetCameraInfo request
ros::ServiceClient cameraInfoClient;
cameraInfoClient = nh.serviceClient<px_comm::SetCameraInfo>(ros::names::append(cameraNs, "set_camera_info"));
px_comm::SetCameraInfo setCameraInfo;
setCameraInfo.request.camera_info = *cameraInfo;
if (cameraInfoClient.call(setCameraInfo))
{
ROS_INFO("Received reply to SetCameraInfo request.");
if (setCameraInfo.response.success == true)
{
ROS_INFO("SetCameraInfo request was processed successfully: %s.",
setCameraInfo.response.status_message.c_str());
}
else
{
ROS_ERROR("SetCameraInfo request was processed successfully.");
}
}
else
{
ROS_ERROR("Did not receive reply to SetCameraInfo request.");
}
cv::Mat mapX, mapY;
px::CameraPtr& camera = calibration.camera();
if (camera->modelType() == px::Camera::PINHOLE)
{
camera->initUndistortRectifyMap(mapX, mapY);
}
else
{
camera->initUndistortRectifyMap(mapX, mapY, 300.0f, 300.0f,
cv::Size(camera->imageWidth(),
camera->imageHeight()),
-1.0f, -1.0f);
}
while (ros::ok())
{
ros::spinOnce();
r.sleep();
if (!frame.available())
{
continue;
}
cv::remap(frame.data(), imgView, mapX, mapY, cv::INTER_LINEAR);
cv::putText(imgView, "Calibrated", cv::Point(10,20),
cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255),
1, CV_AA);
cv::imshow("Image", imgView);
cv::waitKey(2);
frame.available() = false;
}
return 0;
}
int main(int argc,char* argv[])
{
T_NODE* head=NULL;
DRAW_TYPE type;
int prev_begin=0;
int prev_end=0;
int mid_begin=0;
int mid_end=0;
#ifndef __DEBUG__
if(argc != 4)
{
help();
return 0;
}
else
{
if(!strcmp(argv[1],"-null"))
{
type = SHOW_NULL;
}
else if(!strcmp(argv[1],"-nnull"))
{
type = UNSHOW_NULL;
}
else
{
help();
return 0;
}
pPrev=(char*)malloc(sizeof(strlen(argv[2])+1));
memset(pPrev,0,(sizeof(strlen(argv[2])+1)));
strcpy(pPrev,argv[2]);
prev_begin=0;
prev_end=strlen(argv[2])-1;
pMid=(char*)malloc(sizeof(strlen(argv[3])+1));
memset(pMid,0,(sizeof(strlen(argv[3])+1)));
strcpy(pMid,argv[3]);
mid_begin=0;
mid_end=strlen(argv[3])-1;
}
#else
pPrev=(char*)malloc(sizeof(strlen(prev)+1));
memset(pPrev,0,(sizeof(strlen(prev)+1)));
strcpy(pPrev,prev);
prev_begin=0;
prev_end=strlen(prev)-1;
pMid=(char*)malloc(sizeof(strlen(mid)+1));
memset(pMid,0,(sizeof(strlen(mid)+1)));
strcpy(pMid,mid);
mid_begin=0;
mid_end=strlen(mid)-1;
#endif
buildtree(&head,prev_begin,prev_end,mid_begin,mid_end);
if(!calibration(head,pPrev,pMid))
{
printf("FAIL!!! can't build binary tree, please check \n");
}
else
{
drawtree_build_googleAPI_html(head,type,"binarytree_graphic.html");
}
return 0;
}
/**
* \brief Main command interpreter function
*
* This operator analyzes the command arguments and calls the appropriate
* subcommand method.
*/
void guidercommand::operator()(const std::string& /* command */,
const std::vector<std::string>& arguments) {
if (arguments.size() < 2) {
throw command_error("guider command requires more "
"arguments");
}
std::string guiderid = arguments[0];
std::string subcommand = arguments[1];
debug(LOG_DEBUG, DEBUG_LOG, 0, "guiderid: %s", guiderid.c_str());
Guiders guiders;
GuiderWrapper guider = guiders.byname(guiderid);
if (subcommand == "info") {
info(guider, arguments);
return;
}
if (subcommand == "exposure") {
exposure(guider, arguments);
return;
}
if (subcommand == "exposuretime") {
exposuretime(guider, arguments);
return;
}
if (subcommand == "binning") {
binning(guider, arguments);
return;
}
if (subcommand == "size") {
size(guider, arguments);
return;
}
if (subcommand == "offset") {
offset(guider, arguments);
return;
}
if (subcommand == "star") {
star(guider, arguments);
return;
}
if (subcommand == "calibration") {
calibration(guider, arguments);
return;
}
if (subcommand == "start") {
start(guider, arguments);
return;
}
if (subcommand == "stop") {
stop(guider, arguments);
return;
}
if (subcommand == "wait") {
wait(guider, arguments);
return;
}
if (subcommand == "image") {
image(guider, arguments);
return;
}
}
/**
* tries to load the camera calibration
*
* The camera calibration with the format of the libvisensor < 2.0.0 is loaded and parsed to
* ViCameraCalibration vector. If no Xml format could be found or parsed a empty vector is returned.
*
* @return vector of the saved configurations
*
*/
std::vector<visensor::ViCameraCalibration> SensorUpdater::parseXmlCameraCalibration(
const std::string& xml_filename)
{
std::vector<visensor::ViCameraCalibration> output_vector;
boost::property_tree::ptree calibration_tree;
if (!loadPropertyTree(xml_filename, &calibration_tree)) {
exit(1);
}
BOOST_FOREACH(const boost::property_tree::ptree::value_type & iter, calibration_tree.get_child("") ){
visensor::ViCameraCalibration calibration(visensor::ViCameraLensModel::LensModelTypes::RADIAL,
visensor::ViCameraProjectionModel::ProjectionModelTypes::PINHOLE);
std::vector<std::string> elements;
boost::split(elements, iter.first, boost::is_any_of("_"));
if( (elements.size() == 3) && (elements[0] == "cam") ) {
try
{
calibration.cam_id_ = std::stoi(elements[1]);
calibration.slot_id_ = std::stoi(elements[2])/2;
calibration.is_flipped_ = std::stoi(elements[2])%2;
calibration.resolution_[0] = 752;
calibration.resolution_[1] = 480;
//build childtree name
std::string cam_id_str = boost::lexical_cast<std::string>(calibration.cam_id_);
std::string slot_str = boost::lexical_cast<std::string>(std::stoi(elements[2]));
std::string child_tree = std::string("cam_") + cam_id_str + std::string("_") + slot_str + std::string(".");
//load data
visensor::ViCameraLensModelRadial::Ptr lens_model = calibration.getLensModel<visensor::ViCameraLensModelRadial>();
visensor::ViCameraProjectionModelPinhole::Ptr projection_model = calibration.getProjectionModel<visensor::ViCameraProjectionModelPinhole>();
projection_model->focal_length_u_ = calibration_tree.get<double>(child_tree + "fu");
projection_model->focal_length_v_ = calibration_tree.get<double>(child_tree + "fv");
projection_model->principal_point_u_ = calibration_tree.get<double>(child_tree + "cu");
projection_model->principal_point_v_ = calibration_tree.get<double>(child_tree + "cv");
lens_model->k1_ = calibration_tree.get<double>(child_tree + "K0");
lens_model->k2_ = calibration_tree.get<double>(child_tree + "K1");
lens_model->r1_ = calibration_tree.get<double>(child_tree + "K2");
lens_model->r2_ = calibration_tree.get<double>(child_tree + "K3");
calibration.R_.resize(9);
calibration.R_[0] = calibration_tree.get<double>(child_tree + "R00");
calibration.R_[1] = calibration_tree.get<double>(child_tree + "R01");
calibration.R_[2] = calibration_tree.get<double>(child_tree + "R02");
calibration.R_[3] = calibration_tree.get<double>(child_tree + "R10");
calibration.R_[4] = calibration_tree.get<double>(child_tree + "R11");
calibration.R_[5] = calibration_tree.get<double>(child_tree + "R12");
calibration.R_[6] = calibration_tree.get<double>(child_tree + "R20");
calibration.R_[7] = calibration_tree.get<double>(child_tree + "R21");
calibration.R_[8] = calibration_tree.get<double>(child_tree + "R22");
calibration.t_.resize(3);
calibration.t_[0] = calibration_tree.get<double>(child_tree + "t0");
calibration.t_[1] = calibration_tree.get<double>(child_tree + "t1");
calibration.t_[2] = calibration_tree.get<double>(child_tree + "t2");
} catch(std::exception const& ex)
{
std::cout << "failed.\n";
std::cout << "Exception: " << ex.what() << "\n";
return output_vector;
}
output_vector.push_back(calibration);
}
}
return output_vector;
}
int LDWS(unsigned char *img, int width, int height, PointKAIST *pt_left, PointKAIST *pt_right)
{
int result = 0, result2 = 0, i = 0;
///////////calibration(img, width, height, pt_left, pt_right);
search_lane(img, width, height-bottom_margin, pt_left, pt_right);
ransac(img, pt_left, pt_right, 10);
lane_filtering(pt_left, pt_right, width, height);
// 일정 프레임(현재 100)까지 calibration을 수행하도록 하는 구문
if(calibration_frame < 100)
{
calibration(pt_left, pt_right, width, height);
calibration_frame++;
}
//result = warning_system2(pt_left, pt_right);
//tracking(pt_left, pt_right, width, height);
if(pt_left[0].x == 0 && pt_left[0].y == 0)
init(pt_left, pt_right);
if(pt_right[0].x == 0 && pt_right[0].y == 0)
init(pt_left, pt_right);
/**
// 기준선 그리기
for(i=0; i<width; i++)
{
img[horizontal_center*width+i-1] = 255;
img[horizontal_center*width+i] = 255;
img[horizontal_center*width+i+1] = 255;
}
for(i=0; i<height; i++)
{
img[i*width+vertical_center-1] = 255;
img[i*width+vertical_center] = 255;
img[i*width+vertical_center+1] = 255;
}
**/
//printf("noise : %f\n", get_noise(img, width, height, pt_left[1].x, pt_right[1].x, pt_left[1].y));
/*
//printf("%d ", pt_left[0].x);
if((pt_right[0].x - pt_left[0].x) > 300 && pt_right[0].x != 0 && pt_right[0].y != 0 && pt_left[0].x != 0 && pt_right[0].y != 0)
// 한쪽만 움직인 경우는 제외함
{
if((prev_prev_pt_left[0].x - pt_left[0].x) != 0 && (prev_prev_pt_right[0].x - pt_right[0].x) != 0)
//printf("%d %d\n", pt_left[0].x, pt_right[0].x);
// vertical_center로 교체됨
result = warning_system(vertical_center, (pt_right[0].x - pt_left[0].x) / 2 + pt_left[0].x, warning_val, pt_left[0].x, pt_right[0].x);
}
else
{
result = 0;
init(pt_left, pt_right);
}
*/
result = warning_system(vertical_center, (pt_right[0].x - pt_left[0].x) / 2 + pt_left[0].x, warning_val, pt_left[0].x, pt_right[0].x);
//draw_lane(img, width, height, pt_left[0].x, pt_left[0].y, pt_right[0].x, pt_right[0].y, pt_left[2].x, pt_left[2].y, pt_right[2].x, pt_right[2].y, pt_left, pt_right);
// 오른쪽
if(result==1)
{
// 튀는 경우 방지
if(start == 0 && prev_warning > 0)
{
//draw_warning(img, width, height);
result2 = result;
init(pt_left, pt_right);
}
else
result2 = 0;
}
// 왼쪽
else if(result==2)
{
if(start == 0 && prev_warning > 0)
{
//draw_warning2(img, width, height);
result2 = result;
init(pt_left, pt_right);
}
else
result2 = 0;
}
else
{
start = 0;
result = 0;
result2 = 0;
}
prev_prev_pt_left[0].x = pt_left[0].x;
prev_prev_pt_left[0].y = pt_left[0].y;
prev_prev_pt_left[1].x = pt_left[1].x;
prev_prev_pt_left[1].y = pt_left[1].y;
prev_prev_pt_left[2].x = pt_left[2].x;
prev_prev_pt_left[2].y = pt_left[2].y;
prev_prev_pt_right[0].x = pt_right[0].x;
prev_prev_pt_right[0].y = pt_right[0].y;
prev_prev_pt_right[1].x = pt_right[1].x;
prev_prev_pt_right[1].y = pt_right[1].y;
prev_prev_pt_right[2].x = pt_right[2].x;
prev_prev_pt_right[2].y = pt_right[2].y;
// draw_test(img, width, height);
//printf("warning : %d %d\n", prev_warning, result);
//오른쪽 차선 1
if(prev_warning == 1 && result == 2)
{
//draw_warning(img, width, height);
//init(pt_left, pt_right);
return 1;
}
// 왼쪽 차선 2
else if(prev_warning == 2 && result == 1)
{
//draw_warning2(img, width, height);
//init(pt_left, pt_right);
return 2;
}
prev_warning = result;
return 0;
}
int main( void )
{
// Enable interrupts as early as possible
sei();
Timer_Init();
// Set as outputs and stop rotor
DDRD |= (1 << PD1)|(1 << PD2);
PORTD &= ~((1 << PD1)|(1 << PD2));
// Setup ADC
initAdcFeedback();
Can_Message_t txMsg;
txMsg.Id = (CAN_NMT_APP_START << CAN_SHIFT_NMT_TYPE) | (NODE_ID << CAN_SHIFT_NMT_SID);
txMsg.DataLength = 4;
txMsg.RemoteFlag = 0;
txMsg.ExtendedFlag = 1;
txMsg.Data.words[0] = APP_TYPE;
txMsg.Data.words[1] = APP_VERSION;
// Set up callback for CAN reception
BIOS_CanCallback = &can_receive;
// Send CAN_NMT_APP_START
BIOS_CanSend(&txMsg);
// Read calibration value from eeprom
azimuthCalibration = eeprom_read_word( CALIBRATE_AZIMUTH );
// Timer for reading position feedback
Timer_SetTimeout(0, 100, TimerTypeFreeRunning, 0);
Timer_SetTimeout(1, 1000, TimerTypeFreeRunning, 0);
sendStatus( STATUS );
while (1) {
if (Timer_Expired(0)) {
// Periodicly read antennas position
readFeedback();
}
if (Timer_Expired(1)) {
sendStatus(STATUS);
}
if (rxMsgFull) {
switch (rxMsg.Id){
case MSG_CAL_SET: // Set calibration value
if( 2 == rxMsg.DataLength ){
calibration( SET, rxMsg.Data.words[0] );
}
break;
case MSG_CAL_GET: // Get calibration value
if( 0 == rxMsg.DataLength ){
txMsg.Id = MSG_CAL_RET;
txMsg.DataLength = 2;
txMsg.Data.words[0] = calibration( GET, 0 );
BIOS_CanSend(&txMsg);
}
break;
case MSG_ABS: // Start turning to absolute position
if( 2 == rxMsg.DataLength ){
}
break;
case MSG_REL: // Start turning to relative position
if( 2 == rxMsg.DataLength ){
}
break;
case MSG_START: // Start turning
if( 1 == rxMsg.DataLength ){
// First data byte decides direction
controlRelay( rxMsg.Data.bytes[0] );
}
break;
case MSG_STOP: // Stop turning
//if( 1 == rxMsg.DataLength ){
controlRelay( ROTATE_STOP );
//}
break;
case MSG_STATUS: // Get position
if( 0 == rxMsg.DataLength ){
sendStatus(STATUS);
}
break;
default:
break;
}
rxMsgFull = 0; //
}
}
return 0;
}