int main()
{
Value a = 17;
Value b = 25;
Plus c = Plus(&a,&b);
std::cout << evl(a) << std::endl;
std::cout << evl(b) << std::endl;
std::cout << evl(c) << std::endl;
}
int evl(const Expr& e)
{
mch::var<const Expr*> e1, e2;
mch::var<int> n;
Match(e)
{
Case(mch::C<Value> (n) ) return n;
Case(mch::C<Plus> (e1,e2)) return evl(*e1) + evl(*e2);
Case(mch::C<Minus> (e1,e2)) return evl(*e1) - evl(*e2);
Case(mch::C<Times> (e1,e2)) return evl(*e1) * evl(*e2);
Case(mch::C<Divide>(e1,e2)) return evl(*e1) / evl(*e2);
}
EndMatch
}
int put( int put)
{
int sp;
/* kifu[c-4]=put;*/
sekisa();
printf("com:%d,man:%d",com,man);
if(put==0) return(1);
sp=evl();
printf("sp:%d",sp);
init();
/*kifusaisei();*/
return (0);
}
int min_level(int f,int level,int alpha,int beta)
{
int p,j,ev,pf,i,all,tmp;
int ban_cpy[101];
ev=0;
for(i=11;i<=88;i++)
ban_cpy[i]=ban[i];
if(level==0 && e==ON){
ev+=evl()*7;
tmp=sekisa();
if(com==0){ev=-10000;return ev;}
else if((man+com)<D)ev-=tmp*31;
ev+=enableput()*87;
ev+=corner()*2048;
ev+=xuti()*256;
ev+=cuti()*128;
ev+=Hen1();
ev+=Hen2();
return ev;
}
else if (level==0){
return(sekisa());
}
pf=OFF;tmp=10000;
for(j=0;j<=60;j++)
{
all=0;
p=tbl[j];
all=check(p,MAN,OFF);
if(all){
pf=ON;
all=check(p,MAN,ON);
tmp=max_level(OFF,level-1,alpha,beta);
if(beta>tmp) beta=tmp;
for(i=11;i<89;i++)
ban[i]=ban_cpy[i];
}
if(beta<=alpha) return (beta);
}
if(pf!=OFF) return(beta);
if(f==ON)return (sekisa());
return max_level(ON,level-1,alpha,beta);
}
void WQLCompile::_factoring()
{
int i = 0,n = eval_heap.size();
//for (int i=eval_heap.size()-1; i >= 0; i--)
while (i < n)
{
int _found = 0;
int index = 0;
// look for expressions (A | B) & C ---> A & C | A & B
if (eval_heap[i].op == WQL_AND)
{
if (eval_heap[i].is_terminal1 == EVAL_HEAP)
{
index = eval_heap[i].opn1; // remember the index
if (eval_heap[index].op == WQL_OR)
{
_found = 1;
}
}
if ((_found == 0) && (eval_heap[i].is_terminal2 == EVAL_HEAP))
{
index = eval_heap[i].opn2; // remember the index
if (eval_heap[index].op == WQL_OR)
{
_found = 2;
}
}
if (_found != 0)
{
stack_el s;
if (_found == 1)
{
s = eval_heap[i].getSecond();
}
else
{
s = eval_heap[i].getFirst();
}
// insert two new expression before entry i
eval_el evl(false, WQL_OR, i+1, EVAL_HEAP, i, EVAL_HEAP);
if (i < static_cast<int>(eval_heap.size())-1)
{
eval_heap.insert(i+1, evl);
}
else
{
eval_heap.append(evl);
}
eval_heap.insert(i+1, evl);
for (int j=eval_heap.size()-1; j > i + 2; j--)
{
//eval_heap[j] = eval_heap[j-2];
// adjust pointers
if ((eval_heap[j].is_terminal1 == EVAL_HEAP) &&
(eval_heap[j].opn1 >= i))
{
eval_heap[j].opn1 += 2;
}
if ((eval_heap[j].is_terminal2 == EVAL_HEAP) &&
(eval_heap[j].opn2 >= i))
{
eval_heap[j].opn2 += 2;
}
}
n+=2; // increase size of array
// generate the new expressions : new OR expression
// first new AND expression
eval_heap[i+1].mark = false;
eval_heap[i+1].op = WQL_AND;
eval_heap[i+1].setFirst(s);
eval_heap[i+1].setSecond( eval_heap[index].getFirst());
eval_heap[i+1].order();
// second new AND expression
eval_heap[i].mark = false;
eval_heap[i].op = WQL_AND;
eval_heap[i].setFirst(s);
eval_heap[i].setSecond( eval_heap[index].getSecond());
eval_heap[i].order();
// mark the indexed expression as inactive
//eval_heap[index].op = WQL_IS_TRUE; possible disconnects
i--;
} /* endif _found > 0 */
} /* endif found AND operator */
i++; // increase pointer
}
}
void FeatureViewer::publishFeatureCovariances(rgbdtools::RGBDFrame& frame)
{
// create markers
visualization_msgs::Marker marker;
marker.header.stamp.sec = frame.header.stamp.sec;
marker.header.stamp.nsec = frame.header.stamp.nsec;
marker.header.seq = frame.header.seq;
marker.header.frame_id = frame.header.frame_id;
marker.type = visualization_msgs::Marker::LINE_LIST;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
marker.color.a = 1.0;
marker.scale.x = 0.0025;
marker.action = visualization_msgs::Marker::ADD;
marker.ns = "covariances";
marker.id = 0;
marker.lifetime = ros::Duration();
for (unsigned int kp_idx = 0; kp_idx < frame.keypoints.size(); ++kp_idx)
{
if (!frame.kp_valid[kp_idx]) continue;
const Vector3f& kp_mean = frame.kp_means[kp_idx];
const Matrix3f& kp_cov = frame.kp_covariances[kp_idx];
// transform Eigen to OpenCV matrices
cv::Mat m(3, 1, CV_64F);
for (int j = 0; j < 3; ++j)
m.at<double>(j, 0) = kp_mean(j, 0);
cv::Mat cov(3, 3, CV_64F);
for (int j = 0; j < 3; ++j)
for (int i = 0; i < 3; ++i)
cov.at<double>(j, i) = kp_cov(j, i);
// compute eigenvectors
cv::Mat evl(1, 3, CV_64F);
cv::Mat evt(3, 3, CV_64F);
cv::eigen(cov, evl, evt);
double mx = m.at<double>(0,0);
double my = m.at<double>(1,0);
double mz = m.at<double>(2,0);
for (int e = 0; e < 3; ++e)
{
geometry_msgs::Point a;
geometry_msgs::Point b;
double sigma = sqrt(evl.at<double>(0,e));
double scale = sigma * 3.0;
tf::Vector3 evt_tf(evt.at<double>(e,0),
evt.at<double>(e,1),
evt.at<double>(e,2));
a.x = mx + evt_tf.getX() * scale;
a.y = my + evt_tf.getY() * scale;
a.z = mz + evt_tf.getZ() * scale;
b.x = mx - evt_tf.getX() * scale;
b.y = my - evt_tf.getY() * scale;
b.z = mz - evt_tf.getZ() * scale;
marker.points.push_back(a);
marker.points.push_back(b);
}
}
covariances_publisher_.publish(marker);
}
