int main(int argc, char **argv){
individual pop[POP_SIZE], newPop[POP_SIZE];
int generation=1;
initialize_population(pop);
evaluation(pop);
sort(pop);
while(generation <= GENERATIONS){
printf("-------------------------------------\n");
printf("Generation %d:\n", generation);
print_best(pop);
//print_population(pop);
create_new_population(pop, newPop);
evaluation(newPop);
population_replacement(pop, newPop);
generation++;
}
return 0;
}
int main(int argc, const char *argv[])
{
// 'true' satisfaccion restricciones duras
bool type = true;
// 'true' selecciona mejores
bool cloneSelType = true;
// 'true' remplaza los mas malos
bool replaceType = true;
// 'true' Se clonan mediante formula
bool cloneType = true;
timespec ts, te;
clock_gettime(CLOCK_REALTIME, &ts);
population[POP].fitness = 10000;
if(!checkFile(argc,argv))
return 0;
changeParam(argv);
// cout << "ClonationFactor: "<<clonationFactor << endl;
// cout << "ClonationRate: " << clonationRate << endl;
// cout << "ReplaceRate: " << replaceRate << endl;
inputReading(argv[1]);
generation = 0;
initPopulation(type);
evaluation(population, type);
while(generation<GENS){
cleanPops();
selection(clonationRate, population);
clonation(tmpPop, cloneType);
hypermutation();
evaluation(clonePop, type);
cloneSelection(clonePop, cloneSelType);
cloneInsertion();
newGeneration(replaceRate, replaceType);
evaluation(population, type);
generation++;
}
clock_gettime(CLOCK_REALTIME, &te);
// printFile(argv[1],population[0].gene,population[0].fitness,(te.tv_sec-ts.tv_sec), (te.tv_nsec-ts.tv_nsec));
// printFile(population[0].fitness);
cout << replaceRate << " " << population[0].fitness << " " << (te.tv_sec-ts.tv_sec)<<"."<<(te.tv_nsec-ts.tv_nsec) << endl;
return 0;
}
void Parser::expression(MethodGenerationContext* mgenc) {
Peek();
if (nextSym == Assign)
assignation(mgenc);
else
evaluation(mgenc);
}
int main (int argc, char **argv) {
int i, flag;
flag = 0;
initiate(); // 产生初始化群体
evaluation(0); // 对初始化种群进行评估、排序
for (i = 0; i < MAXloop; i++) { // 进入进化循环，当循环次数超过MAXloop所规定的值时终止循环，flag值保持为0
cross(); // 进行交叉操作
evaluation(1); // 对子种群进行评估、排序
selection(); // 从父、子种群中选择最优的NUM个作为新的父种群
if (record() == 1) { // 如果满足终止规则1时将flag置1，停止循环
flag = 1;
break;
}
mutation(); // 进行变异操作
}
showresult(flag); // 按flag值显示寻优结果
return 0;
}
int application_request(int fd,data_person * users,int * newid)
{
binary_semaphore_wait(semaphore);
person person;
simple_person * simple_person;
int ans=0;
clsvbuff buff;
if(read_msg(fd,&buff)<0)
return -1;
switch(buff.opc)
{
case GET_INFO:
ans = get_info(&buff,users,newid,&person);
ans_get_info(fd,&buff,ans,&person);
break;
case GET_MATCHS:
ans = get_matchs(&buff,users,&simple_person);
ans_get_matchs(fd,ans,&buff,&simple_person);
break;
case SHOW_PEOPLE:
ans = show_people(&buff,users,newid,&simple_person);
ans_show_people(fd,ans,&buff,&simple_person);
break;
case REGISTER:
ans=register_user(&buff,users,newid);
ans_register_user(fd,ans,&buff);
break;
case LOGIN: /*In this case ans doubles as opOK*/
ans = login(&buff,users,newid);
ans_login(fd,ans,&buff);
break;
case EVALUATION: /*In this case ans doubles as opOK*/
users[buff.id_client-1].last_evaluation=buff.data.clsv_evaluation.id;
if(buff.data.clsv_evaluation.like==true)
{
ans = evaluation(&buff,users);
ans_evaluation(fd,ans,&buff);
}
else
ans_evaluation(fd,0,&buff);
break;
default: /*OPC NOT RECOGNIZED*/
return -1;
break;
}
binary_semaphore_post(semaphore);
return 0;
}
void main()
{
int i,flag;
flag=0;
initiate(); //产生初始化种群
evaluation( 0 ); //对初始化种群进行评估、排序
for( i = 0 ; i < MAXloop ; i++ )
{
cross(); //进行交叉操作
evaluation( 1 ); //对子种群进行评估、排序
selection(); //对父子种群中选择最优的NUM个作为新的父种群
if( record() == 1 ) //满足终止规则1，则flag=1并停止循环
{
flag = 1;
break;
}
mutation(); //变异操作
}
showresult( flag ); //按照flag显示寻优结果
}
void Parser::assignation(MethodGenerationContext* mgenc) {
list<StdString> l;
assignments(mgenc, l);
evaluation(mgenc);
list<StdString>::iterator i;
for (i = l.begin(); i != l.end(); ++i)
bcGen->EmitDUP(mgenc);
for (i = l.begin(); i != l.end(); ++i)
genPopVariable(mgenc, (*i));
}
static evaluation parse_filename(const std::string& str)
{
//Antwoorden_Vragenlijst_NWI-I00032 (2013-01-29)_67288_Geavanceerd Programmeren.xls.csv
//Results_survey_NWI-IBC017_2014-11-04_758478_Calculus and Probability Theory.csv
const static boost::regex r1(".*_(?:NWI-)?([^\\_^-]+) \\((.+)\\)_([0-9]+)_(.+)\\.csv");
const static boost::regex r2(".*_(?:NWI-)?([^\\_^-]+)_(.+)_([0-9]+)_(.+)\\.csv");
boost::smatch match;
if(!boost::regex_match(str, match, r1) && !boost::regex_match(str, match, r2))
throw std::runtime_error(std::string("Can not parse filename (complete): ") + str);
return evaluation(match[1], match[4], match[3], match[2]);
}
void mutation_function(population& p)
{
for(auto i = p.begin(); i != p.end(); ++i)
{
float prob = 1 - i->adapt; // probability of mutation
float r = uniform_random(); // random float between <0,1)
if(r < prob)
{
mutation::random_transposition(i->perm);
i->eval = evaluation(i->perm); // after mutation is done we have to evaluate this specimen again
}
}
}
/**
* \brief Creates OpenCL
*
*/
inline void svc_createOCL(){
if (baseclass_ocl_node_deviceId==NULL) {
if (initOCLInstance()){
svc_SetUpOclObjects(baseclass_ocl_node_deviceId);
}else{
error("FATAL ERROR: Instantiating the Device: Failed to instatiate the device!\n");
abort();
}
}
else if (evaluation()) {
svc_releaseOclObjects();
svc_SetUpOclObjects(baseclass_ocl_node_deviceId);
}
}
population initial_population()
{
population pop;
for(int i = 0; i < population_size; ++i)
{
specimen s;
s.perm = permutation(N, permutation::type::random);
s.eval = s.adapt = 0.0;
pop.push_back(s);
}
best_specimen = pop[0];
best_specimen.eval = evaluation(pop[0].perm);
return pop;
}
/**
* Call the evaluation function
*
* @param state a state to evaluate
* @return the utility value of the state
*/
int eval( struct State * state )
{
return evaluation( opposite_player( state->player), state->player, state->board ) -
evaluation( state->player, opposite_player( state->player ), state->board );
}
void crossover_function(population& p)
{
population new_population;
std::vector<int> cross_set;
std::vector<int>::iterator it;
float rd;
// choose which specimen will crossover
for(int i=0; i<population_size; i++)
{
rd = (randid()*1.0f) / (RAND_MAX*1.0f*population_size);
if(rd > p[i].adapt)
cross_set.push_back(i);
}
// we want even number of parents
if(cross_set.size() & 1)
cross_set.pop_back();
// more randomness
random_shuffle ( cross_set.begin(), cross_set.end(), p_randgenid );
it = cross_set.begin();
// crossover each pair from left to right
while( it != cross_set.end())
{
std::pair<permutation,permutation> desc;
crossover::type ctype;
if(cfg.compare_operators)
{
switch(randid() % xop_count)
{
case 0: ctype = crossover::type::OX; break;
case 1: ctype = crossover::type::CX; break;
case 2:
default: ctype = crossover::type::PMX; break;
}
desc = crossover::random_crossover(ctype, p[*it].perm, p[*(it+1)].perm);
}
else
{
desc = crossover::random_crossover(cfg.crossover_type, p[*it].perm, p[*(it+1)].perm);
}
specimen ch1, ch2;
ch1.perm = desc.first;
ch1.eval = evaluation(ch1.perm);
ch1.adapt = 0.0;
ch2.perm = desc.second;
ch2.eval = evaluation(ch2.perm);
ch2.adapt = 0.0;
if(cfg.compare_operators && ( ((p[*it].eval + p[*(it+1)].eval) / (ch1.eval + ch2.eval)) >= 1 ) )
{
switch(ctype)
{
case crossover::type::OX: ox_count++; break;
case crossover::type::CX: cx_count++; break;
case crossover::type::PMX:
default: pmx_count++; break;
}
}
it++; it++;
x_count++;
new_population.push_back(ch1);
new_population.push_back(ch2);
}
p.insert(p.end(), new_population.begin(), new_population.end());
}
void evaluate_population(population& p)
{
for(unsigned int i=0; i<p.size(); i++)
p[i].eval = evaluation(p[i].perm);
}
int main(int argc, char *argv[])
{
if(argc == 1)
{
std::cerr << "usage:" << std::endl;
std::cerr << argv[0] << " imgfile" << std::endl;
return -1;
}
//load source image as grayscale
cv::Mat img_src = cv::imread(argv[1], 0);
cv::Mat img_src_32, img_dst, img_dst_32, img_match_sift, img_match_kaze;
//initialize
img_src.convertTo(img_src_32, CV_32F, 1.0/255.0,0);
//initialize SIFT feature detector descriptor
cv::SiftFeatureDetector sift_detector;
cv::SiftDescriptorExtractor sift_extractor;
cv::BruteForceMatcher<cv::L2<float> > matcher;
//SIFT feature extraction from source image
std::vector<cv::KeyPoint> sift_kp_src;
sift_detector.detect(img_src, sift_kp_src);
cv::Mat sift_src;
sift_extractor.compute(img_src, sift_kp_src, sift_src);
//initialize KAZE feature detector descriptor
toptions kazeopt_src = get_default_toptions(img_src.cols, img_src.rows);
KAZE kazeevol_src(kazeopt_src);
//KAZE feature extraction from source image
kazeevol_src.Create_Nonlinear_Scale_Space(img_src_32);
std::vector<Ipoint> kaze_kp_src;
kazeevol_src.Feature_Detection(kaze_kp_src);
kazeevol_src.Feature_Description(kaze_kp_src);
translate(img_src, img_dst, 0, 1.0, 0, 0);
toptions kazeopt_dst = get_default_toptions(img_dst.cols, img_dst.rows);
KAZE kazeevol_dst(kazeopt_dst);
double rot = 0.0;
double scale = 1.0;
int noise = 0;
double blur = 0;
int key;
while(key = cv::waitKey(10))
{
bool show = true;
if(key == 65362) scale = std::min(scale + 0.1, 1.5);
else if(key == 65364) scale = std::max(scale - 0.1, 0.5);
else if(key == 65361) rot += 5;
else if(key == 65363) rot -= 5;
else if(key == 'z') noise = std::min(noise + 5, 50);
else if(key == 'x') noise = std::max(noise - 5, 0);
else if(key == 'c') blur = std::min(blur + 0.5, 20.0);
else if(key == 'v') blur = std::max(blur - 0.5, 0.0);
else show = false;
if(show)
{
std::cout << "**********" << std::endl;
std::cout << "scale factor : " << scale << std::endl;
std::cout << "rotation : " << rot << " degree" << std::endl;
std::cout << "white noise max power : " << noise << std::endl;
std::cout << "gaussian blur sigma : " << blur << std::endl;
}
const cv::Mat m = translate(img_src, img_dst, rot, scale, noise, blur);
//feature extraction from destination image
std::vector<cv::KeyPoint> sift_kp_dst;
sift_detector.detect(img_dst, sift_kp_dst);
cv::Mat sift_dst;
sift_extractor.compute(img_dst, sift_kp_dst, sift_dst);
//match SIFT
cv::vector<cv::DMatch> corr;
matcher.match(sift_src, sift_dst, corr);
cv::drawMatches(img_src, sift_kp_src, img_dst, sift_kp_dst, corr, img_match_sift);
cv::putText(img_match_sift, "SIFT", cv::Point(30, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 3);
//initialize KAZE feature detector descriptor
img_dst.convertTo(img_dst_32, CV_32F, 1.0/255.0,0);
//KAZE feature extraction from source image
kazeevol_dst.Create_Nonlinear_Scale_Space(img_dst_32);
std::vector<Ipoint> kaze_kp_dst;
kazeevol_dst.Feature_Detection(kaze_kp_dst);
kazeevol_dst.Feature_Description(kaze_kp_dst);
//match KAZE
std::vector<int> kaze_corr;
Matching_Descriptor(kaze_kp_src, kaze_kp_dst, kaze_corr);
//draw KAZEmatch
const std::vector<cv::DMatch> kaze_corr2 = convert_KAZEMatch_DMatch(kaze_corr);
const std::vector<cv::KeyPoint> kaze_kp1 = convert_Ipoint_Keypoint(kaze_kp_src);
const std::vector<cv::KeyPoint> kaze_kp2 = convert_Ipoint_Keypoint(kaze_kp_dst);
cv::drawMatches(img_src, kaze_kp1, img_dst, kaze_kp2, kaze_corr2, img_match_kaze);
cv::putText(img_match_kaze, "KAZE", cv::Point(30, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 3);
cv::Mat result;
cv::Mat src[] = {img_match_sift, img_match_kaze};
cv::vconcat(src, 2, result);
cv::imshow("hoge", result);
if(show)
{
std::cout << "SIFT:";
evaluation(sift_kp_src, sift_kp_dst, corr, m);
std::cout << "KAZE:";
evaluation(kaze_kp1, kaze_kp2, kaze_corr2, m);
}
}
}
void pose_estimation_from_line_correspondence(Eigen::MatrixXf start_points,
Eigen::MatrixXf end_points,
Eigen::MatrixXf directions,
Eigen::MatrixXf points,
Eigen::MatrixXf &rot_cw,
Eigen::VectorXf &pos_cw)
{
int n = start_points.cols();
if(n != directions.cols())
{
return;
}
if(n<4)
{
return;
}
float condition_err_threshold = 1e-3;
Eigen::VectorXf cosAngleThreshold(3);
cosAngleThreshold << 1.1, 0.9659, 0.8660;
Eigen::MatrixXf optimumrot_cw(3,3);
Eigen::VectorXf optimumpos_cw(3);
std::vector<float> lineLenVec(n,1);
vfloat3 l1;
vfloat3 l2;
vfloat3 nc1;
vfloat3 Vw1;
vfloat3 Xm;
vfloat3 Ym;
vfloat3 Zm;
Eigen::MatrixXf Rot(3,3);
std::vector<vfloat3> nc_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> Vw_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> n_c(n,vfloat3(0,0,0));
Eigen::MatrixXf Rx(3,3);
int line_id;
for(int HowToChooseFixedTwoLines = 1 ; HowToChooseFixedTwoLines <=3 ; HowToChooseFixedTwoLines++)
{
if(HowToChooseFixedTwoLines==1)
{
#pragma omp parallel for
for(int i = 0; i < n ; i++ )
{
// to correct
float lineLen = 10;
lineLenVec[i] = lineLen;
}
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
vfloat3 temp;
temp = start_points.col(0);
start_points.col(0) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(0);
end_points.col(0) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(line_id);
directions.col(0) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(0);
points.col(0) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
l1 = start_points.col(0) - end_points.col(0);
l1 = l1/l1.norm();
}
for(int i = 1; i < n; i++)
{
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
l2 = start_points.col(line_id) - end_points.col(line_id);
l2 = l2/l2.norm();
lineLenVec[line_id] = 0;
MatrixXf cosAngle(3,3);
cosAngle = (l1.transpose()*l2).cwiseAbs();
if(cosAngle.maxCoeff() < cosAngleThreshold[HowToChooseFixedTwoLines])
{
break;
}
}
vfloat3 temp;
temp = start_points.col(1);
start_points.col(1) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(1);
end_points.col(1) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(1);
directions.col(1) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(1);
points.col(1) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
// The rotation matrix R_wc is decomposed in way which is slightly different from the description in the paper,
// but the framework is the same.
// R_wc = (Rot') * R * Rot = (Rot') * (Ry(theta) * Rz(phi) * Rx(psi)) * Rot
nc1 = x_cross(start_points.col(1),end_points.col(1));
nc1 = nc1/nc1.norm();
Vw1 = directions.col(1);
Vw1 = Vw1/Vw1.norm();
//the X axis of Model frame
Xm = x_cross(nc1,Vw1);
Xm = Xm/Xm.norm();
//the Y axis of Model frame
Ym = nc1;
//the Z axis of Model frame
Zm = x_cross(Xm,Zm);
Zm = Zm/Zm.norm();
//Rot * [Xm, Ym, Zm] = I.
Rot.col(0) = Xm;
Rot.col(1) = Ym;
Rot.col(2) = Zm;
Rot = Rot.transpose();
//rotate all the vector by Rot.
//nc_bar(:,i) = Rot * nc(:,i)
//Vw_bar(:,i) = Rot * Vw(:,i)
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
vfloat3 nc = x_cross(start_points.col(1),end_points.col(1));
nc = nc/nc.norm();
n_c[i] = nc;
nc_bar[i] = Rot * nc;
Vw_bar[i] = Rot * directions.col(i);
}
//Determine the angle psi, it is the angle between z axis and Vw_bar(:,1).
//The rotation matrix Rx(psi) rotates Vw_bar(:,1) to z axis
float cospsi = (Vw_bar[1])[2];//the angle between z axis and Vw_bar(:,1); cospsi=[0,0,1] * Vw_bar(:,1);.
float sinpsi= sqrt(1 - cospsi*cospsi);
Rx.row(0) = vfloat3(1,0,0);
Rx.row(1) = vfloat3(0,cospsi,-sinpsi);
Rx.row(2) = vfloat3(0,sinpsi,cospsi);
vfloat3 Zaxis = Rx * Vw_bar[1];
if(1-fabs(Zaxis[3]) > 1e-5)
{
Rx = Rx.transpose();
}
//estimate the rotation angle phi by least square residual.
//i.e the rotation matrix Rz(phi)
vfloat3 Vm2 = Rx * Vw_bar[1];
float A2 = Vm2[0];
float B2 = Vm2[1];
float C2 = Vm2[2];
float x2 = (nc_bar[1])[0];
float y2 = (nc_bar[1])[1];
float z2 = (nc_bar[1])[2];
Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
Eigen::VectorXf coeff(9);
std::vector<float> coef(9,0); //coefficients of equation (7)
Eigen::VectorXf polyDF = VectorXf::Zero(16); //%dF = ployDF(1) * t^15 + ployDF(2) * t^14 + ... + ployDF(15) * t + ployDF(16);
//construct the polynomial F'
#pragma omp parallel for
for(int i = 3 ; i < n ; i++)
{
vfloat3 Vm3 = Rx*Vw_bar[i];
float A3 = Vm3[0];
float B3 = Vm3[1];
float C3 = Vm3[2];
float x3 = (nc_bar[i])[0];
float y3 = (nc_bar[i])[1];
float z3 = (nc_bar[i])[2];
float u11 = -z2*A2*y3*B3 + y2*B2*z3*A3;
float u12 = -y2*A2*z3*B3 + z2*B2*y3*A3;
float u13 = -y2*B2*z3*B3 + z2*B2*y3*B3 + y2*A2*z3*A3 - z2*A2*y3*A3;
float u14 = -y2*B2*x3*C3 + x2*C2*y3*B3;
float u15 = x2*C2*y3*A3 - y2*A2*x3*C3;
float u21 = -x2*A2*y3*B3 + y2*B2*x3*A3;
float u22 = -y2*A2*x3*B3 + x2*B2*y3*A3;
float u23 = x2*B2*y3*B3 - y2*B2*x3*B3 - x2*A2*y3*A3 + y2*A2*x3*A3;
float u24 = y2*B2*z3*C3 - z2*C2*y3*B3;
float u25 = y2*A2*z3*C3 - z2*C2*y3*A3;
float u31 = -x2*A2*z3*A3 + z2*A2*x3*A3;
float u32 = -x2*B2*z3*B3 + z2*B2*x3*B3;
float u33 = x2*A2*z3*B3 - z2*A2*x3*B3 + x2*B2*z3*A3 - z2*B2*x3*A3;
float u34 = z2*A2*z3*C3 + x2*A2*x3*C3 - z2*C2*z3*A3 - x2*C2*x3*A3;
float u35 = -z2*B2*z3*C3 - x2*B2*x3*C3 + z2*C2*z3*B3 + x2*C2*x3*B3;
float u36 = -x2*C2*z3*C3 + z2*C2*x3*C3;
float a4 = u11*u11 + u12*u12 - u13*u13 - 2*u11*u12 + u21*u21 + u22*u22 - u23*u23
-2*u21*u22 - u31*u31 - u32*u32 + u33*u33 + 2*u31*u32;
float a3 =2*(u11*u14 - u13*u15 - u12*u14 + u21*u24 - u23*u25
- u22*u24 - u31*u34 + u33*u35 + u32*u34);
float a2 =-2*u12*u12 + u13*u13 + u14*u14 - u15*u15 + 2*u11*u12 - 2*u22*u22 + u23*u23
+ u24*u24 - u25*u25 +2*u21*u22+ 2*u32*u32 - u33*u33
- u34*u34 + u35*u35 -2*u31*u32- 2*u31*u36 + 2*u32*u36;
float a1 =2*(u12*u14 + u13*u15 + u22*u24 + u23*u25 - u32*u34 - u33*u35 - u34*u36);
float a0 = u12*u12 + u15*u15+ u22*u22 + u25*u25 - u32*u32 - u35*u35 - u36*u36 - 2*u32*u36;
float b3 =2*(u11*u13 - u12*u13 + u21*u23 - u22*u23 - u31*u33 + u32*u33);
float b2 =2*(u11*u15 - u12*u15 + u13*u14 + u21*u25 - u22*u25 + u23*u24 - u31*u35 + u32*u35 - u33*u34);
float b1 =2*(u12*u13 + u14*u15 + u22*u23 + u24*u25 - u32*u33 - u34*u35 - u33*u36);
float b0 =2*(u12*u15 + u22*u25 - u32*u35 - u35*u36);
float d0 = a0*a0 - b0*b0;
float d1 = 2*(a0*a1 - b0*b1);
float d2 = a1*a1 + 2*a0*a2 + b0*b0 - b1*b1 - 2*b0*b2;
float d3 = 2*(a0*a3 + a1*a2 + b0*b1 - b1*b2 - b0*b3);
float d4 = a2*a2 + 2*a0*a4 + 2*a1*a3 + b1*b1 + 2*b0*b2 - b2*b2 - 2*b1*b3;
float d5 = 2*(a1*a4 + a2*a3 + b1*b2 + b0*b3 - b2*b3);
float d6 = a3*a3 + 2*a2*a4 + b2*b2 - b3*b3 + 2*b1*b3;
float d7 = 2*(a3*a4 + b2*b3);
float d8 = a4*a4 + b3*b3;
std::vector<float> v { a4, a3, a2, a1, a0, b3, b2, b1, b0 };
Eigen::VectorXf vp;
vp << a4, a3, a2, a1, a0, b3, b2, b1, b0 ;
//coef = coef + v;
coeff = coeff + vp;
polyDF[0] = polyDF[0] + 8*d8*d8;
polyDF[1] = polyDF[1] + 15* d7*d8;
polyDF[2] = polyDF[2] + 14* d6*d8 + 7*d7*d7;
polyDF[3] = polyDF[3] + 13*(d5*d8 + d6*d7);
polyDF[4] = polyDF[4] + 12*(d4*d8 + d5*d7) + 6*d6*d6;
polyDF[5] = polyDF[5] + 11*(d3*d8 + d4*d7 + d5*d6);
polyDF[6] = polyDF[6] + 10*(d2*d8 + d3*d7 + d4*d6) + 5*d5*d5;
polyDF[7] = polyDF[7] + 9 *(d1*d8 + d2*d7 + d3*d6 + d4*d5);
polyDF[8] = polyDF[8] + 8 *(d1*d7 + d2*d6 + d3*d5) + 4*d4*d4 + 8*d0*d8;
polyDF[9] = polyDF[9] + 7 *(d1*d6 + d2*d5 + d3*d4) + 7*d0*d7;
polyDF[10] = polyDF[10] + 6 *(d1*d5 + d2*d4) + 3*d3*d3 + 6*d0*d6;
polyDF[11] = polyDF[11] + 5 *(d1*d4 + d2*d3)+ 5*d0*d5;
polyDF[12] = polyDF[12] + 4 * d1*d3 + 2*d2*d2 + 4*d0*d4;
polyDF[13] = polyDF[13] + 3 * d1*d2 + 3*d0*d3;
polyDF[14] = polyDF[14] + d1*d1 + 2*d0*d2;
polyDF[15] = polyDF[15] + d0*d1;
}
Eigen::VectorXd coefficientPoly = VectorXd::Zero(16);
for(int j =0; j < 16 ; j++)
{
coefficientPoly[j] = polyDF[15-j];
}
//solve polyDF
solver.compute(coefficientPoly);
const Eigen::PolynomialSolver<double, Eigen::Dynamic>::RootsType & r = solver.roots();
Eigen::VectorXd rs(r.rows());
Eigen::VectorXd is(r.rows());
std::vector<float> min_roots;
for(int j =0;j<r.rows();j++)
{
rs[j] = fabs(r[j].real());
is[j] = fabs(r[j].imag());
}
float maxreal = rs.maxCoeff();
for(int j = 0 ; j < rs.rows() ; j++ )
{
if(is[j]/maxreal <= 0.001)
{
min_roots.push_back(rs[j]);
}
}
std::vector<float> temp_v(15);
std::vector<float> poly(15);
for(int j = 0 ; j < 15 ; j++)
{
temp_v[j] = j+1;
}
for(int j = 0 ; j < 15 ; j++)
{
poly[j] = polyDF[j]*temp_v[j];
}
Eigen::Matrix<double,16,1> polynomial;
Eigen::VectorXd evaluation(min_roots.size());
for( int j = 0; j < min_roots.size(); j++ )
{
evaluation[j] = poly_eval( polynomial, min_roots[j] );
}
std::vector<float> minRoots;
for( int j = 0; j < min_roots.size(); j++ )
{
if(!evaluation[j]<=0)
{
minRoots.push_back(min_roots[j]);
}
}
if(minRoots.size()==0)
{
cout << "No solution" << endl;
return;
}
int numOfRoots = minRoots.size();
//for each minimum, we try to find a solution of the camera pose, then
//choose the one with the least reprojection residual as the optimum of the solution.
float minimalReprojectionError = 100;
// In general, there are two solutions which yields small re-projection error
// or condition error:"n_c * R_wc * V_w=0". One of the solution transforms the
// world scene behind the camera center, the other solution transforms the world
// scene in front of camera center. While only the latter one is correct.
// This can easily be checked by verifying their Z coordinates in the camera frame.
// P_c(Z) must be larger than 0 if it's in front of the camera.
for(int rootId = 0 ; rootId < numOfRoots ; rootId++)
{
float cosphi = minRoots[rootId];
float sign1 = sign_of_number(coeff[0] * pow(cosphi,4)
+ coeff[1] * pow(cosphi,3) + coeff[2] * pow(cosphi,2)
+ coeff[3] * cosphi + coeff[4]);
float sign2 = sign_of_number(coeff[5] * pow(cosphi,3)
+ coeff[6] * pow(cosphi,2) + coeff[7] * cosphi + coeff[8]);
float sinphi= -sign1*sign2*sqrt(abs(1-cosphi*cosphi));
Eigen::MatrixXf Rz(3,3);
Rz.row(0) = vfloat3(cosphi,-sinphi,0);
Rz.row(1) = vfloat3(sinphi,cosphi,0);
Rz.row(2) = vfloat3(0,0,1);
//now, according to Sec4.3, we estimate the rotation angle theta
//and the translation vector at a time.
Eigen::MatrixXf RzRxRot(3,3);
RzRxRot = Rz*Rx*Rot;
//According to the fact that n_i^C should be orthogonal to Pi^c and Vi^c, we
//have: scalarproduct(Vi^c, ni^c) = 0 and scalarproduct(Pi^c, ni^c) = 0.
//where Vi^c = Rwc * Vi^w, Pi^c = Rwc *(Pi^w - pos_cw) = Rwc * Pi^w - pos;
//Using the above two constraints to construct linear equation system Mat about
//[costheta, sintheta, tx, ty, tz, 1].
Eigen::MatrixXf Matrice(2*n-1,6);
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
float nxi = (nc_bar[i])[0];
float nyi = (nc_bar[i])[1];
float nzi = (nc_bar[i])[2];
Eigen::VectorXf Vm(3);
Vm = RzRxRot * directions.col(i);
float Vxi = Vm[0];
float Vyi = Vm[1];
float Vzi = Vm[2];
Eigen::VectorXf Pm(3);
Pm = RzRxRot * points.col(i);
float Pxi = Pm(1);
float Pyi = Pm(2);
float Pzi = Pm(3);
//apply the constraint scalarproduct(Vi^c, ni^c) = 0
//if i=1, then scalarproduct(Vi^c, ni^c) always be 0
if(i>1)
{
Matrice(2*i-3, 0) = nxi * Vxi + nzi * Vzi;
Matrice(2*i-3, 1) = nxi * Vzi - nzi * Vxi;
Matrice(2*i-3, 5) = nyi * Vyi;
}
//apply the constraint scalarproduct(Pi^c, ni^c) = 0
Matrice(2*i-2, 0) = nxi * Pxi + nzi * Pzi;
Matrice(2*i-2, 1) = nxi * Pzi - nzi * Pxi;
Matrice(2*i-2, 2) = -nxi;
Matrice(2*i-2, 3) = -nyi;
Matrice(2*i-2, 4) = -nzi;
Matrice(2*i-2, 5) = nyi * Pyi;
}
//solve the linear system Mat * [costheta, sintheta, tx, ty, tz, 1]' = 0 using SVD,
JacobiSVD<MatrixXf> svd(Matrice, ComputeThinU | ComputeThinV);
Eigen::MatrixXf VMat = svd.matrixV();
Eigen::VectorXf vec(2*n-1);
//the last column of Vmat;
vec = VMat.col(5);
//the condition that the last element of vec should be 1.
vec = vec/vec[5];
//the condition costheta^2+sintheta^2 = 1;
float normalizeTheta = 1/sqrt(vec[0]*vec[1]+vec[1]*vec[1]);
float costheta = vec[0]*normalizeTheta;
float sintheta = vec[1]*normalizeTheta;
Eigen::MatrixXf Ry(3,3);
Ry << costheta, 0, sintheta , 0, 1, 0 , -sintheta, 0, costheta;
//now, we get the rotation matrix rot_wc and translation pos_wc
Eigen::MatrixXf rot_wc(3,3);
rot_wc = (Rot.transpose()) * (Ry * Rz * Rx) * Rot;
Eigen::VectorXf pos_wc(3);
pos_wc = - Rot.transpose() * vec.segment(2,4);
//now normalize the camera pose by 3D alignment. We first translate the points
//on line in the world frame Pw to points in the camera frame Pc. Then we project
//Pc onto the line interpretation plane as Pc_new. So we could call the point
//alignment algorithm to normalize the camera by aligning Pc_new and Pw.
//In order to improve the accuracy of the aligment step, we choose two points for each
//lines. The first point is Pwi, the second point is the closest point on line i to camera center.
//(Pw2i = Pwi - (Pwi'*Vwi)*Vwi.)
Eigen::MatrixXf Pw2(3,n);
Pw2.setZero();
Eigen::MatrixXf Pc_new(3,n);
Pc_new.setZero();
Eigen::MatrixXf Pc2_new(3,n);
Pc2_new.setZero();
for(int i = 0 ; i < n ; i++)
{
vfloat3 nci = n_c[i];
vfloat3 Pwi = points.col(i);
vfloat3 Vwi = directions.col(i);
//first point on line i
vfloat3 Pci;
Pci = rot_wc * Pwi + pos_wc;
Pc_new.col(i) = Pci - (Pci.transpose() * nci) * nci;
//second point is the closest point on line i to camera center.
vfloat3 Pw2i;
Pw2i = Pwi - (Pwi.transpose() * Vwi) * Vwi;
Pw2.col(i) = Pw2i;
vfloat3 Pc2i;
Pc2i = rot_wc * Pw2i + pos_wc;
Pc2_new.col(i) = Pc2i - ( Pc2i.transpose() * nci ) * nci;
}
MatrixXf XXc(Pc_new.rows(), Pc_new.cols()+Pc2_new.cols());
XXc << Pc_new, Pc2_new;
MatrixXf XXw(points.rows(), points.cols()+Pw2.cols());
XXw << points, Pw2;
int nm = points.cols()+Pw2.cols();
cal_campose(XXc,XXw,nm,rot_wc,pos_wc);
pos_cw = -rot_wc.transpose() * pos_wc;
//check the condition n_c^T * rot_wc * V_w = 0;
float conditionErr = 0;
for(int i =0 ; i < n ; i++)
{
float val = ( (n_c[i]).transpose() * rot_wc * directions.col(i) );
conditionErr = conditionErr + val*val;
}
if(conditionErr/n < condition_err_threshold || HowToChooseFixedTwoLines ==3)
{
//check whether the world scene is in front of the camera.
int numLineInFrontofCamera = 0;
if(HowToChooseFixedTwoLines<3)
{
for(int i = 0 ; i < n ; i++)
{
vfloat3 P_c = rot_wc * (points.col(i) - pos_cw);
if(P_c[2]>0)
{
numLineInFrontofCamera++;
}
}
}
else
{
numLineInFrontofCamera = n;
}
if(numLineInFrontofCamera > 0.5*n)
{
//most of the lines are in front of camera, then check the reprojection error.
int reprojectionError = 0;
for(int i =0; i < n ; i++)
{
//line projection function
vfloat3 nc = rot_wc * x_cross(points.col(i) - pos_cw , directions.col(i));
float h1 = nc.transpose() * start_points.col(i);
float h2 = nc.transpose() * end_points.col(i);
float lineLen = (start_points.col(i) - end_points.col(i)).norm()/3;
reprojectionError += lineLen * (h1*h1 + h1*h2 + h2*h2) / (nc[0]*nc[0]+nc[1]*nc[1]);
}
if(reprojectionError < minimalReprojectionError)
{
optimumrot_cw = rot_wc.transpose();
optimumpos_cw = pos_cw;
minimalReprojectionError = reprojectionError;
}
}
}
}
if(optimumrot_cw.rows()>0)
{
break;
}
}
pos_cw = optimumpos_cw;
rot_cw = optimumrot_cw;
}
int Heuristic::minMax(int level, int alpha, int beta, unsigned long long downArray[]) {
int tmp = 0; // temporary value which is compared to max value
int winCheck=0;
set<unsigned long long int> tempMoveSet;
set<unsigned long long int>::iterator iter, iterEnd, iterTmp;
if (!aiPlayer->working) { // if timeout!
return 0; // value doesn't matter
}
winCheck = b->terminal();
if (winCheck == color+2) // computer wins
return PLUS_INFINITY / level;
if (winCheck == 1) // draw
return 0;
if (winCheck != 0) // opponent wins
return MINUS_INFINITY;
// Leaf
if (level == maxLevel) {
iteration++;
return evaluation();
}
// MAX
if (level % 2 == 0) {
// getting possible moves
b->calculatePossibleMoves(color);
tempMoveSet = b->getPossibleMoves(color);
if (firstMove) {
if (level == maxLevel-2) { // in next minMax call start exploring tree without first move path
firstMove = false;
}
tempMoveSet.erase(bestLeafPath[level]); // delete best move from the set
// copied part from above
b->movePiece(bestLeafPath[level], color); // do the best move
unsigned long long * upArray; // array given to upper minMax call
upArray = new unsigned long long [ARRAY_SIZE];
memset(upArray,0,sizeof(unsigned long long)*ARRAY_SIZE); // cleaning array with 0
tmp = minMax(level+1, alpha, beta, upArray);
b->undoMove();
b->clean();
b->calculatePossibleMoves(color);
if (tmp > alpha) {
alpha = tmp;
downArray[level] = bestLeafPath[level]; // store best move so far (at current level)
for(int i = level+1; i<maxLevel; ++i) { // copy good moves from higher levels
downArray[i] = upArray[i];
}
if (level == 0) {
actualMoveMutex.acquire();
actualMove = bestLeafPath[level];
actualMoveMutex.release();
}
}
delete[] upArray;
}
iter = tempMoveSet.begin();
iterEnd = tempMoveSet.end();
for (; iter != iterEnd; ++iter) {
if (alpha >= beta) // while (alpha < beta)
break;
b->movePiece(*iter, color);
unsigned long long * upArray; // array given to upper minMax call
upArray = new unsigned long long [ARRAY_SIZE];
memset(upArray,0,sizeof(unsigned long long)*ARRAY_SIZE); // cleaning array with 0
tmp = minMax(level+1, alpha, beta, upArray);
b->undoMove();
b->clean();
b->calculatePossibleMoves(color);
if (tmp > alpha) {
alpha = tmp;
downArray[level] = *iter; // store best move so far (at current level)
for(int i = level+1; i<maxLevel; ++i) { // copy good moves from higher levels
downArray[i] = upArray[i];
}
if (level == 0) {
actualMoveMutex.acquire();
actualMove = *iter;
actualMoveMutex.release();
}
}
delete[] upArray;
}
return alpha;
}
// MIN
else {
// getting possible moves
b->calculatePossibleMoves(!color);
tempMoveSet = b->getPossibleMoves(!color);
if (firstMove) {
if (level == maxLevel-2) { // in next minMax call start exploring tree without first move path
firstMove = false;
}
tempMoveSet.erase(bestLeafPath[level]); // delete best move from the set
// copied part from above
b->movePiece(bestLeafPath[level], !color); // do the best move
unsigned long long * upArray; // array given to upper minMax call
upArray = new unsigned long long [ARRAY_SIZE];
memset(upArray,0,sizeof(unsigned long long)*ARRAY_SIZE); // cleaning array with 0
tmp = minMax(level+1, alpha, beta, upArray);
b->undoMove();
b->clean();
b->calculatePossibleMoves(!color);
if (tmp < beta) {
beta = tmp;
downArray[level] = bestLeafPath[level]; // store best move so far (at current level)
for(int i = level+1; i<maxLevel; ++i) { // copy good moves from higher levels
downArray[i] = upArray[i];
}
}
delete[] upArray;
}
iter = tempMoveSet.begin();
iterEnd = tempMoveSet.end();
for (; iter != iterEnd; ++iter) {
if (alpha >= beta) // while (alpha < beta)
break;
b->movePiece(*iter, !color);
unsigned long long * upArray; // array given to upper minMax call
upArray = new unsigned long long [ARRAY_SIZE];
memset(upArray,0,sizeof(unsigned long long)*ARRAY_SIZE); // cleaning array with 0
tmp = minMax(level+1, alpha, beta, upArray);
b->undoMove();
b->clean();
b->calculatePossibleMoves(!color);
if (tmp < beta) {
beta = tmp;
downArray[level] = *iter; // store best move so far (at current level)
for(int i = level+1; i<maxLevel; ++i) { // copy good moves from higher levels
downArray[i] = upArray[i];
}
}
delete[] upArray;
}
return beta;
}
}
int othello_ai::turn(othello16 o_upper, int player, int backpoint, int depth) {
// 检查超时
int runtime = get_time();
if (runtime > 1800) {
std::cerr<<"time "<<runtime<<" overflow"<<std::endl;
return (player == o.mycolor) ? MAXVALUE : -MAXVALUE;
}
-- depth;
// std::cerr<<"player "<<player<<" my color "<<o.mycolor<<std::endl;
othello16 o_t;
int value_t, value_mark;
std::pair<int, int> step_mark (-1, -1);
// 初始化value_mark
if (player == o.mycolor) {
value_mark = -MAXVALUE;
} else {
value_mark = MAXVALUE;
}
std::vector< std::pair<int, int> > nextlist = o_upper.allmove(player);
std::string map = o_upper.tostring();
if ( nextlist.size() == 0 ) {
// 无可下点
std::cerr<<"nowhere to move ";
return (player == o.mycolor) ? -MAXVALUE : MAXVALUE;
} else if (depth <= 0) {
// 到达最深层，计算估价函数值
// std::cerr<<"tree hight "<<depth<<std::endl;
std::vector< std::pair<int, int> >::iterator it;
for (it = nextlist.begin() ; it != nextlist.end(); ++ it) {
// 初始化当前棋盘
o_t.init(o.mycolor, map);
// 落子
o_t.play(player, (*it).first, (*it).second);
// string2map(o_t.tostring());
value_t = evaluation(o_t, player);
std::cerr<<std::endl<<"depth "<<DEPTH - depth<<":";
for (int k = 0; k < DEPTH - depth; k ++)
std::cerr<<" ";
std::cerr<<"leaf ("<<(*it).first<<", "<<(*it).second<<") value "<<value_t<<", ";
if ( (player == o.mycolor && value_t >= value_mark)
|| (player != o.mycolor && value_t <= value_mark) ) {
value_mark = value_t;
}
// 剪枝
if (player == o.mycolor && value_mark >= backpoint) {
std::cerr<<"cut beta parent value "<<backpoint<<" current value "<<value_mark<<std::endl;
return value_mark;
} else if (player != o.mycolor && value_mark <= backpoint) {
std::cerr<<"cut alpha parent value "<<backpoint<<" current value "<<value_mark<<std::endl;
return value_mark;
}
}
return value_mark;
} else {
// 普通情况
std::vector< std::pair<int, int> >::iterator it;
for (it = nextlist.begin() ; it != nextlist.end(); ++ it) {
// 初始化当前棋盘
o_t.init(o.mycolor, map);
// 落子
o_t.play(player, (*it).first, (*it).second);
// string2map(o_t.tostring());
value_t = turn(o_t, 3 - player, value_mark, depth);
if (value_t == MAXVALUE || value_t == -MAXVALUE) {
// 子问题超时
break;
}
std::cerr<<std::endl<<"depth "<<DEPTH - depth<<":";
for (int k = 0; k < DEPTH - depth; k ++)
std::cerr<<" ";
std::cerr<<"node ("<<(*it).first<<", "<<(*it).second<<") value "<<value_t<<", ";
if ( (player == o.mycolor && value_t >= value_mark)// 己方落子
|| (player != o.mycolor && value_t <= value_mark) ) {//对方落子
value_mark = value_t;
step_mark.first = (*it).first;
step_mark.second = (*it).second;
}
// 剪枝
if (player == o.mycolor && value_mark >= backpoint) {
std::cerr<<"cut beta parent value "<<backpoint<<" current value "<<value_mark<<std::endl;
break;
} else if (player != o.mycolor && value_mark <= backpoint) {
std::cerr<<"cut alpha parent value "<<backpoint<<" current value "<<value_mark<<std::endl;
break;
}
}
if (depth = DEPTH - 1) {
next.first = step_mark.first;
next.second = step_mark.second;
}
return value_mark;
}
}