int read_triggers(struct storage *store, trigger ** tp)
{
for (;;) {
trigger_type *ttype;
char zText[128];
READ_TOK(store, zText, sizeof(zText));
if (!strcmp(zText, "end"))
break;
ttype = tt_find(zText);
assert(ttype || !"unknown trigger-type");
*tp = t_new(ttype);
if (ttype->read) {
int i = ttype->read(*tp, store);
switch (i) {
case AT_READ_OK:
tp = &(*tp)->next;
break;
case AT_READ_FAIL:
t_free(*tp);
*tp = NULL;
break;
default:
assert(!"invalid return value");
break;
}
}
}
return 0;
}
trigger *trigger_removecurse(curse * c, unit * target)
{
trigger *t = t_new(&tt_removecurse);
removecurse_data *td = (removecurse_data *) t->data.v;
td->curse = c;
td->target = target;
return t;
}
trigger *trigger_gate(building * b, region * target)
{
trigger *t = t_new(&tt_gate);
gate_data *td = (gate_data *) t->data.v;
td->gate = b;
td->target = target;
return t;
}
trigger *trigger_giveitem(unit * u, const item_type * itype, int number)
{
trigger *t = t_new(&tt_giveitem);
giveitem_data *td = (giveitem_data *)t->data.v;
td->number = number;
td->u = u;
td->itype = itype;
return t;
}
trigger *trigger_createunit(region * r, struct faction * f,
const struct race * rc, int number)
{
trigger *t = t_new(&tt_createunit);
createunit_data *td = (createunit_data *)t->data.v;
td->r = r;
td->f = f;
td->race = rc;
td->number = number;
return t;
}
trigger *trigger_unitmessage(unit * target, const char *string, int type,
int level)
{
trigger *t = t_new(&tt_unitmessage);
unitmessage_data *td = (unitmessage_data *) t->data.v;
td->target = target;
td->string = _strdup(string);
td->type = type;
td->level = level;
return t;
}
trigger *trigger_createcurse(struct unit * mage, struct unit * target,
const curse_type * ct, double vigour, int duration, double effect, int men)
{
trigger *t = t_new(&tt_createcurse);
createcurse_data *td = (createcurse_data *)t->data.v;
td->mage = mage;
td->target = target;
td->type = ct;
td->vigour = vigour;
td->duration = duration;
td->effect = effect;
td->men = men;
return t;
}
void rosSpin() {
double dt;
Eigen::Vector3d t(0.0, 0.0, 0.0);
Eigen::Vector3d t_new(0.0, 0.0, 0.0);
Eigen::Vector3d t_vel(0.0, 0.0, 0.0);
Eigen::Vector3d t_imu(0.0, 0.0, 0.0);
Eigen::Vector3d t_arm(-0.19, 0.0, 0.02);
Eigen::Quaterniond q_imu;
ros::Rate rate(15);
ros::Time time_prev, time_now;
time_prev = ros::Time::now();
while(ros::ok()) {
ros::spinOnce();
time_now = ros::Time::now();
dt = (time_now - time_prev).toSec();
time_prev = time_now;
// Compute Rotation
q_imu = w_imu.getQuaternionForAngularChange(dt);
q = q * q_imu;
// Compute Translation
t_imu = v_wheel.getRotationArm()*v_wheel.getTranslation() - q_imu.toRotationMatrix()*t_arm + t_arm;
t_new = t + q.toRotationMatrix()*t_imu;
t_vel = (t_new - t)/dt;
t = t_new;
setPose(t, q, t_new);
pub_data.publish(odometry);
rate.sleep();
}
};
void
tunDeviceRecForceXValuators (TunDevicePtr tun, int n)
{
if (n >= tun->nof_xvaluators)
{
int i;
tun->xval_to_lval_tbl = tun->xval_to_lval_tbl
? t_renew (TunValuatorPtr, tun->xval_to_lval_tbl, n)
: t_new (TunValuatorPtr, n);
for (i = tun->nof_xvaluators; i < n; i++)
tun->xval_to_lval_tbl [i] = NULL;
tun->nof_xvaluators = n;
}
}
static void
index_storage_get_status_cache_fields(struct mailbox *box,
struct mailbox_status *status_r)
{
const struct mail_cache_field *fields;
enum mail_cache_decision_type dec;
ARRAY_TYPE(const_string) *cache_fields;
unsigned int i, count;
fields = mail_cache_register_get_list(box->cache,
pool_datastack_create(), &count);
cache_fields = t_new(ARRAY_TYPE(const_string), 1);
t_array_init(cache_fields, count);
for (i = 0; i < count; i++) {
dec = fields[i].decision & ~MAIL_CACHE_DECISION_FORCED;
if (dec != MAIL_CACHE_DECISION_NO)
array_append(cache_fields, &fields[i].name, 1);
}
status_r->cache_fields = cache_fields;
}
trigger *trigger_clonedied(unit * u)
{
trigger *t = t_new(&tt_clonedied);
t->data.v = (void *)u;
return t;
}
trigger *trigger_killunit(unit * u)
{
trigger *t = t_new(&tt_killunit);
t->data.v = (void *)u;
return t;
}
trigger *trigger_xmasgate(building * b)
{
trigger *t = t_new(&tt_xmasgate);
t->data.v = b;
return t;
}
void
tunInitDeviceRec (int fd, LocalDevicePtr local, TunDevicePtr tun, TunDeviceInfo info)
{
if (TUN_DEVICE_HAS_VAL_ABS (info) || TUN_DEVICE_HAS_VAL_REL (info))
{ /* valuators first ... */
int i,first_valuator = -1;
int last_valuator;
int nof_valuators = 0;
TunValuatorPtr val;
TunAbsValuatorInfo aval_info;
if (TUN_DEVICE_HAS_VAL_ABS (info))
{ /* absolute valuators */
for (i = 0, first_valuator = -1; i < ABS_MAX; i++)
if (TUN_DEVICE_TEST_VAL_ABS (info, i))
{
last_valuator = i;
if (first_valuator == -1)
first_valuator = i;
nof_valuators ++;
TLOG ("[%s] Found absolute valuator %d (%s)", local->name,
i, tunGetAbsValuatorName (i));
}
tun->first_avaluator = first_valuator;
tun->nof_avaluators = last_valuator - first_valuator + 1;
tun->avaluators = t_new (TunValuatorRec, tun->nof_avaluators);
for (i = first_valuator; i <= last_valuator; i++)
{
val = tun->avaluators + (i - first_valuator);
val->xiv = -1;
if (TUN_DEVICE_TEST_VAL_ABS (info, i))
{
val->lid = i;
tunQueryAbsValuator (fd, i, &aval_info);
}
else
val->lid = -1;
val->value = aval_info.value;
val->rel_value = aval_info.value;
val->min = aval_info.min;
val->max = aval_info.max;
TLOG ("Konfigurace valuatoru %d: (%d,%d,%d)",
i, val->value, val->min, val->max);
val->upsidedown = FALSE;
val->mouse_wheel_hack = FALSE;
val->abs_is_relspeed = FALSE;
val->is_absolute = TRUE;
}
} /* absolute valuators */
if (TUN_DEVICE_HAS_VAL_REL (info))
{ /* relative valuators */
for (i = 0, first_valuator = -1; i < REL_MAX; i++)
if (TUN_DEVICE_TEST_VAL_REL (info, i))
{
if (i > last_valuator) last_valuator = i;
if (first_valuator == -1) first_valuator = i;
nof_valuators++;
TLOG ("[%s] Found relative valuator %d (%s)", local->name,
i, tunGetRelValuatorName (i));
}
tun->first_rvaluator = first_valuator;
tun->nof_rvaluators = last_valuator - first_valuator + 1;
tun->rvaluators = t_new (TunValuatorRec, tun->nof_rvaluators);
for (i = first_valuator; i <= last_valuator; i++)
{
val = tun->rvaluators + (i - first_valuator);
if (TUN_DEVICE_TEST_VAL_REL (info, i))
val->lid = i;
else
val->lid = -1;
val->xiv = -1;
val->value = 0;
val->min = val->max = -1;
val->upsidedown = FALSE;
val->mouse_wheel_hack = FALSE;
val->abs_is_relspeed = FALSE;
val->is_absolute = FALSE;
}
} /* relative valuators */
tunDeviceRecForceXValuators (tun, nof_valuators);
} /* valuators */
TLOG ("Furt tady");
if (TUN_DEVICE_HAS_KEYS (info))
{
int nof_buttons, min_button, max_button;
int nof_keys, min_key, max_key;
int i;
min_button = max_button = min_key = max_key = -1;
nof_buttons = nof_keys = 0;
for (i = 0; i < KEY_MAX; i++)
if (TUN_DEVICE_TEST_KEY (info, i))
{
if (i < BTN_MISC)
{ /* key */
nof_keys ++;
if (min_key == -1) min_key = i;
max_key = i;
TLOG ("[%s] Found key %d (%s)", local->name, i,
tunGetKeyName (i));
}
else
{ /* button */
nof_buttons ++;
if (min_button == -1) min_button = i;
max_button = i;
TLOG ("[%s] Found button %d (%s)", local->name, i,
tunGetKeyName (i)) ;
}
}
if (nof_buttons)
{
tun->nof_lbuttons = max_button - min_button + 1;
tun->first_lbutton = min_button;
tun->lbut_to_xbut_tbl = t_new (short int, tun->nof_lbuttons);
for (i = 0; i < tun->nof_lbuttons; i++)
tun->lbut_to_xbut_tbl [i] = TUN_BUTTON_UNASSIGNED;
}
}
}
trigger *trigger_shock(unit * u)
{
trigger *t = t_new(&tt_shock);
t->data.v = (void *)u;
return t;
}
static void
doit (void)
{
struct taia stamp;
struct taia deadline;
int j = 0, r = 0, iolen = 0;
for (;;)
{
taia_now (&stamp);
taia_uint (&deadline, 120);
taia_add (&deadline, &deadline, &stamp);
iolen = 0;
udp53io = io + iolen++;
udp53io->fd = udp53;
udp53io->events = IOPAUSE_READ;
tcp53io = io + iolen++;
tcp53io->fd = tcp53;
tcp53io->events = IOPAUSE_READ;
for (j = 0; j < MAXUDP; ++j)
{
if (u[j].active)
{
u[j].io = io + iolen++;
query_io (&u[j].q, u[j].io, &deadline);
}
}
for (j = 0; j < MAXTCP; ++j)
{
if (t[j].active)
{
t[j].io = io + iolen++;
if (t[j].state == 0)
query_io (&t[j].q, t[j].io, &deadline);
else
{
if (taia_less (&t[j].timeout, &deadline))
deadline = t[j].timeout;
t[j].io->fd = t[j].tcp;
t[j].io->events = (t[j].state > 0) ?
IOPAUSE_READ : IOPAUSE_WRITE;
}
}
}
iopause (io, iolen, &deadline, &stamp);
for (j = 0; j < MAXUDP; ++j)
{
if (u[j].active)
{
r = query_get (&u[j].q, u[j].io, &stamp);
if (r == -1)
u_drop (j);
if (r == 1)
u_respond (j);
}
}
for (j = 0; j < MAXTCP; ++j)
{
if (t[j].active)
{
if (t[j].io->revents)
t_timeout (j);
if (t[j].state == 0)
{
r = query_get (&t[j].q, t[j].io, &stamp);
if (r == -1)
t_drop (j);
if (r == 1)
t_respond (j);
}
else
if (t[j].io->revents || taia_less (&t[j].timeout, &stamp))
t_rw (j);
}
}
if (udp53io && udp53io->revents)
u_new();
if (tcp53io && tcp53io->revents)
t_new();
}
}
trigger *trigger_unguard(building * b)
{
trigger *t = t_new(&tt_unguard);
t->data.v = (void *)b;
return t;
}
static trigger *trigger_caldera(building * b)
{
trigger *t = t_new(&tt_caldera);
t->data.v = b;
return t;
}
/* ------------------------------------------------------------------------- */
void MainWindow::on_method2_clicked()
{
int index_prev;
int index_now;
QString ymlFile;
vector<KeyPoint> imgpts1_tmp;
vector<KeyPoint> imgpts2_tmp;
imgpts.resize(filelist.size());
on_PB_Sift_clicked();
cout << endl << endl << endl << "Using Method 2:" <<endl;
vector<DMatch> matches;
cv::Matx34d P_0;
cv::Matx34d P_1;
P_0 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
cv::Mat_<double> t_prev = (cv::Mat_<double>(3,1) << 0, 0, 0);
cv::Mat_<double> R_prev = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
cv::Mat_<double> R_prev_inv = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
cv::Mat_<double> t_now = (cv::Mat_<double>(3,1) << 0, 0, 0);
cv::Mat_<double> R_now = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
cv::Mat_<double> t_new = (cv::Mat_<double>(3,1) << 0, 0, 0);
cv::Mat_<double> R_new = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
reconstruct_first_two_view();
std::cout << "Pmat[0] = " << endl << Pmats[0]<<endl;
std::cout << "Pmat[1] = " << endl << Pmats[1]<<endl;
for(index_now = 2; index_now<filelist.size(); index_now++)
{
cout << endl << endl << endl <<endl;
cout << "dealing with " << filelist.at(index_now).fileName().toStdString() << endl;
cout << endl;
index_prev = index_now - 1;
descriptors1.release();
descriptors1 = descriptors2;
descriptors2.release();
ymlFile = ymlFileDir;
ymlFile.append(filelist.at(index_now).fileName()).append(".yml");
restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_now],descriptors2);
matches.clear();
//matching
matching_fb_matcher(descriptors1,descriptors2,matches);
matching_good_matching_filter(matches);
imggoodpts1.clear();
imggoodpts2.clear();
P_0 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
P_1 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
outCloud.clear();
if(FindCameraMatrices(K,Kinv,distcoeff,
imgpts[index_prev],imgpts[index_now],
imggoodpts1,imggoodpts2,
P_0,P_1,
matches,
outCloud))
{//if can find camera matries
R_prev(0,0) = Pmats[index_prev](0,0);
R_prev(0,1) = Pmats[index_prev](0,1);
R_prev(0,2) = Pmats[index_prev](0,2);
R_prev(1,0) = Pmats[index_prev](1,0);
R_prev(1,1) = Pmats[index_prev](1,1);
R_prev(1,2) = Pmats[index_prev](1,2);
R_prev(2,0) = Pmats[index_prev](2,0);
R_prev(2,1) = Pmats[index_prev](2,1);
R_prev(2,2) = Pmats[index_prev](2,2);
t_prev(0) = Pmats[index_prev](0,3);
t_prev(1) = Pmats[index_prev](1,3);
t_prev(2) = Pmats[index_prev](2,3);
R_now(0,0) = P_1(0,0);
R_now(0,1) = P_1(0,1);
R_now(0,2) = P_1(0,2);
R_now(1,0) = P_1(1,0);
R_now(1,1) = P_1(1,1);
R_now(1,2) = P_1(1,2);
R_now(2,0) = P_1(2,0);
R_now(2,1) = P_1(2,1);
R_now(2,2) = P_1(2,2);
t_now(0) = P_1(0,3);
t_now(1) = P_1(1,3);
t_now(2) = P_1(2,3);
invert(R_prev, R_prev_inv);
t_new = t_prev + R_prev * t_now ;
R_new = R_now * R_prev;
// //store estimated pose
Pmats[index_now] = cv::Matx34d (R_new(0,0),R_new(0,1),R_new(0,2),t_new(0),
R_new(1,0),R_new(1,1),R_new(1,2),t_new(1),
R_new(2,0),R_new(2,1),R_new(2,2),t_new(2));
cout << "Pmats[index_now]:" << endl << Pmats[index_now] << endl;
}
else
{
break;
}
}
cout << endl;
cout << endl;
cout << endl;
//visualization
imgpts.clear();
imgpts.resize(filelist.size());
for( index_now = 1; index_now<filelist.size(); index_now++)
{
index_prev = index_now - 1;
descriptors1.release();
descriptors2.release();
ymlFile = ymlFileDir;
ymlFile.append(filelist.at(index_prev).fileName()).append(".yml");
cout << ymlFile.toStdString()<< endl;
restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_prev],descriptors1);
ymlFile = ymlFileDir;
ymlFile.append(filelist.at(index_now).fileName()).append(".yml");
cout << ymlFile.toStdString()<< endl;
restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_now],descriptors2);
matches.clear();
//matching
matching_fb_matcher(descriptors1,descriptors2,matches);
matching_good_matching_filter(matches);
imgpts1_tmp.clear();
imgpts2_tmp.clear();
GetAlignedPointsFromMatch(imgpts[index_prev], imgpts[index_now], matches, imgpts1_tmp, imgpts2_tmp);
cout << imgpts1_tmp.size() << endl;
cout << imgpts1_tmp.size() << endl;
outCloud.clear();
std::vector<cv::KeyPoint> correspImg1Pt;
double mean_proj_err = TriangulatePoints(imgpts1_tmp, imgpts2_tmp, K, Kinv,distcoeff, Pmats[index_prev], Pmats[index_now], outCloud, correspImg1Pt);
std::vector<CloudPoint> outCloud_tmp;
outCloud_tmp.clear();
for (unsigned int i=0; i<outCloud.size(); i++)
{
if(outCloud[i].reprojection_error <= 3){
//cout << "surving" << endl;
outCloud[i].imgpt_for_img.resize(filelist.size());
for(int j = 0; j<filelist.size();j++)
{
outCloud[i].imgpt_for_img[j] = -1;
}
outCloud[i].imgpt_for_img[index_now] = matches[i].trainIdx;
outCloud_tmp.push_back(outCloud[i]);
}
}
outCloud.clear();
outCloud= outCloud_tmp;
cout << outCloud_tmp.size() << endl;
for(unsigned int i=0;i<outCloud.size();i++)
{
outCloud_all.push_back(outCloud[i]);
}
outCloud_new = outCloud;
}
for( int i = 0; i<filelist.size(); i++)
Pmats[i](1,3) =0;
GetRGBForPointCloud(outCloud_all,pointCloudRGB);
ui->widget->update(getPointCloud(),
getPointCloudRGB(),
getCameras());
cout << "finished" <<endl <<endl;
}
