void eval(void)
{ int tmp ;
int tmp___0 ;
int tmp___1 ;
int tmp___2 ;
{
{
while (1) {
while_3_continue: /* CIL Label */
;
{
tmp___2 = exists_runnable_thread();
}
if (tmp___2) {
} else {
goto while_3_break;
}
if ((int )P_1_st == 0) {
{
tmp = __VERIFIER_nondet_int();
}
if (tmp) {
{
P_1_st = 1;
P_1();
}
} else {
}
} else {
}
if ((int )C_1_st == 0) {
{
tmp___1 = __VERIFIER_nondet_int();
}
if (tmp___1) {
{
C_1_st = 1;
C_1();
}
} else {
}
} else {
}
}
while_3_break: /* CIL Label */
;
}
return;
}
}
void algebraic_number_test()
{
typedef Coefficient_ Coefficient;
typedef Rational_ Rational;
typedef CGAL::internal::Algebraic_real_d_1<Coefficient,Rational, CGAL::Handle_policy_no_union, RepClass > Algebraic_real_d_1;
typedef typename CGAL::Polynomial_type_generator<Coefficient,1>::Type Poly;
CGAL::test_real_embeddable<Algebraic_real_d_1>();
// general test of comparable functionality
// TODO generates a precondition error in Algebraic_real_rep
//NiX::test_real_comparable<Algebraic_real_d_1>();
// test of constructors
Poly P_00(Coefficient(0)); // zero polynomial
Poly P_01(Coefficient(1)); // constant polynomial
Poly P_1(Coefficient(-1),Coefficient(1)); //(x-1)
Poly P_2(Coefficient(-2),Coefficient(1)); //(x-2)
Poly P_3(Coefficient(-3),Coefficient(1)); //(x-3)
Poly P_4(Coefficient(-4),Coefficient(1)); //(x-4)
Poly P_12=P_1*P_2; //(x-1)(x-2)
Poly P_123=P_1*P_2*P_3; //(x-1)(x-2)(x-3)
Poly P_s2(Coefficient(-2),Coefficient(0),Coefficient(1)); //(x^2-2)
Poly P_s3(Coefficient(-3),Coefficient(0),Coefficient(1)); //(x^2-3)
Poly P_s5(-Coefficient(5),Coefficient(0),Coefficient(1));
Poly P_s10(-Coefficient(10),Coefficient(0),Coefficient(1));
Poly P_s30(-Coefficient(30),Coefficient(0),Coefficient(1));
Poly P_s2510= P_s2*P_s5*P_s10;
Poly P_s530= P_s5 * P_s30;
Algebraic_real_d_1 tmp;
Algebraic_real_d_1 tmp1,tmp2;
Rational m;
// general constructors;
// default
// tmp = IS_Rational_ = 0
tmp = Algebraic_real_d_1();
assert(tmp.is_rational());
assert(tmp.rational()==0);
// from int
tmp = Algebraic_real_d_1(1);
assert(tmp.is_rational());
assert(tmp.rational()==1);
tmp = Algebraic_real_d_1(5);
assert(tmp.is_rational());
assert(tmp.rational()==5);
// from Field
tmp = Algebraic_real_d_1(Rational(0));
assert(tmp.is_rational());
assert(tmp.rational()==0);
tmp = Algebraic_real_d_1(Rational(1));
assert(tmp.is_rational());
assert(tmp.rational()==1);
tmp = Algebraic_real_d_1(Rational(5)/ Rational(2));
assert(tmp.is_rational());
assert(tmp.rational()== Rational(5)/ Rational(2));
// general constructor
// tmp = 1
#if 0
tmp = Algebraic_real_d_1(P_1,-2,+2);
// TODO different behavior with leda and core
assert(!tmp.is_rational());
assert(tmp==Rational(1));
assert(tmp.is_rational());
assert(tmp.rational()==1);
#endif
// special constructors
// from int
tmp = Algebraic_real_d_1(2);
assert(tmp.is_rational());
assert(tmp.rational()==Rational(2));
//from Rational
tmp = Algebraic_real_d_1(Rational(2));
assert(tmp.is_rational());
assert(tmp.rational()==Rational(2));
// member functions
// tmp IS_GENERAL == 2;
tmp = Algebraic_real_d_1(P_123,Rational(3)/2,Rational(5)/2);
assert(!tmp.is_rational());
assert(tmp.polynomial()==P_123);
assert(tmp.low()==Rational(3)/2);
assert(tmp.high()==Rational(5)/2);
assert(tmp.sign_at_low()==P_123.sign_at(Rational(3)/2));
// refine
tmp = Algebraic_real_d_1(P_123,Rational(3)/2,Rational(5)/2);
tmp.refine();
assert(tmp.is_rational());
assert(tmp.rational()==Rational(2));
// tmp IS_GENERAL = sqrt 2
tmp = Algebraic_real_d_1(P_s2*P_3,Rational(1),Rational(2));
tmp.refine();
assert(tmp.low() >= Rational(1));
assert(tmp.high() <= Rational(3)/2);
// strong_refine
// tmp IS_GENERAL == 2;
tmp = Algebraic_real_d_1(P_123,Rational(3)/2,Rational(5)/2);
m = Rational(2);
tmp.strong_refine(m);
assert(tmp.is_rational());
assert(tmp.rational()==Rational(2));
// tmp IS_GENERAL = sqrt 2
tmp = Algebraic_real_d_1(P_s2*P_3,Rational(1),Rational(2));
m = Rational(3)/2;
tmp.strong_refine(m);
assert(tmp.low()!=m);
assert(tmp.high()!=m);
// refine_to(a,b)
// tmp IS_GENERAL = sqrt 2
tmp = Algebraic_real_d_1(P_s2*P_4,Rational(0),Rational(3));
assert(!tmp.is_rational());
tmp.refine_to(Rational(1), Rational(2));
assert(tmp.low() >= Rational(1));
assert(tmp.high() <= Rational(2));
// tmp IS_REAL = sqrt 2
tmp = Algebraic_real_d_1(P_s2,Rational(0),Rational(3));
assert(!tmp.is_rational());
tmp.refine_to(Rational(1), Rational(2));
assert(tmp.low() >= Rational(1));
assert(tmp.high() <= Rational(2));
// compare(rat)
// tmp IS_GENERAL = sqrt 2
tmp = Algebraic_real_d_1(P_s2*P_3,Rational(1),Rational(2));
m = Rational(1);
assert(tmp.compare(m)==1);
m = Rational(2);
assert(tmp.compare(m)==-1);
// tmp IS_GENERAL = 3
tmp = Algebraic_real_d_1(P_s2*P_3,Rational(2),Rational(4));
m = Rational(3);
assert(tmp.compare(m)==0);
assert(tmp.is_rational());
assert(tmp.rational()==Rational(3));
assert(CGAL::degree(tmp.polynomial()) == 1);
assert(tmp.polynomial().evaluate(Coefficient(3)) == Coefficient(0));
// compare_distinct()
tmp1 = Algebraic_real_d_1(P_s530, Rational(2), Rational(3)); // sqrt(5) = 2.236...
tmp2 = Algebraic_real_d_1(P_s530, Rational(5), Rational(6)); // sqrt(30) = 5.477...
assert(tmp1.compare_distinct(tmp2) == CGAL::SMALLER);
assert(tmp2.compare_distinct(tmp1) == CGAL::LARGER);
//member functions
// is_root_of
tmp1 = Algebraic_real_d_1(P_s2510,Rational(1)/2,Rational(3)/2);
assert(tmp1.is_root_of(P_s530*P_s2));
tmp1 = Algebraic_real_d_1(P_s2510,Rational(1)/2,Rational(3)/2);
assert(!tmp1.is_root_of(P_s530));
//rational_between
{
Rational r;
tmp1 = Algebraic_real_d_1(P_s2,Rational(1),Rational(2)); //sqrt2
tmp2 = Algebraic_real_d_1(P_s3,Rational(1),Rational(3)); //sqrt3
r = tmp1.rational_between(tmp2);
assert(tmp1.compare(r)==CGAL::SMALLER);
assert(tmp2.compare(r)==CGAL::LARGER);
r = tmp2.rational_between(tmp1);
assert(tmp1.compare(r)==CGAL::SMALLER);
assert(tmp2.compare(r)==CGAL::LARGER);
}
// to_double()
tmp = Algebraic_real_d_1(P_1*P_3*P_4, Rational(0), Rational(2));
assert(fabs(tmp.to_double() - 1.0) < 1e-10);
tmp = Algebraic_real_d_1(P_1*P_3, Rational(0), Rational(2));
assert(fabs(tmp.to_double() - 1.0) < 1e-10);
tmp = Algebraic_real_d_1(P_1, Rational(0), Rational(2));
assert(fabs(tmp.to_double() - 1.0) < 1e-10);
//IO tested in _test_algebraic_kernel_1.h
// test for Handle with union
{
typedef
CGAL::internal::Algebraic_real_d_1
<Coefficient,Rational,::CGAL::Handle_policy_union> Int;
Int i(5);
Int j(5);
Int k(6);
assert( ! i.identical( j));
assert( ! i.identical( k));
assert( ! j.identical( k));
assert( i == j);
assert( ! (i == k));
assert( i.identical( j));
assert( ! i.identical( k));
assert( ! j.identical( k));
// code coverage
assert( i == j);
}
// test for Handle without union
{
typedef
CGAL::internal::Algebraic_real_d_1
<Coefficient,Rational,::CGAL::Handle_policy_no_union> Int;
Int i(5);
Int j(5);
Int k(6);
assert( ! i.identical( j));
assert( ! i.identical( k));
assert( ! j.identical( k));
assert( i == j);
assert( ! (i == k));
assert( ! i.identical( j));
assert( ! i.identical( k));
assert( ! j.identical( k));
}
// to_interval
// {
// Algebraic_real_d_1 TMP;
// assert(CGAL::in(25.0,CGAL::to_interval(Algebraic_real_d_1(25))));
// assert(CGAL::in(sqrt(2),CGAL::to_interval(Algebraic_real_d_1(P_s2,1,2))));
// assert(CGAL::in(sqrt(2),CGAL::to_interval(Algebraic_real_d_1(P_s2510,1,2))));
// assert(CGAL::in(-sqrt(2),CGAL::to_interval(Algebraic_real_d_1(P_s2510,-2,-1))));
// assert(CGAL::in(sqrt(5),CGAL::to_interval(Algebraic_real_d_1(P_s2510,2,3))));
// assert(CGAL::in(-sqrt(5),CGAL::to_interval(Algebraic_real_d_1(P_s2510,-3,-2))));
// assert(CGAL::in(sqrt(10),CGAL::to_interval(Algebraic_real_d_1(P_s2510,3,4))));
// assert(CGAL::in(-sqrt(10),CGAL::to_interval(Algebraic_real_d_1(P_s2510,-4,-3))));
// }
//simplify
{
// just a synatx check
Algebraic_real_d_1(P_s2510,1,2).simplify();
}
}
/* ------------------------------------------------------------------------- */
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;
}
