int main()
{
CGAL::Random_points_in_sphere_3<Point_3> gen(100.0);
std::list<Point_3> points;
// generate 250 points randomly in a sphere of radius 100.0
// and insert them into the triangulation
CGAL::cpp11::copy_n(gen, 250, std::back_inserter(points) );
Delaunay T;
T.insert(points.begin(), points.end());
std::list<Vertex_handle> vertices;
T.incident_vertices(T.infinite_vertex(), std::back_inserter(vertices));
std::cout << "This convex hull of the 250 points has "
<< vertices.size() << " points on it." << std::endl;
// remove 25 of the input points
std::list<Vertex_handle>::iterator v_set_it = vertices.begin();
for (int i = 0; i < 25; i++)
{
T.remove(*v_set_it);
v_set_it++;
}
//copy the convex hull of points into a polyhedron and use it
//to get the number of points on the convex hull
Surface_mesh chull;
CGAL::convex_hull_3_to_face_graph(T, chull);
std::cout << "After removal of 25 points, there are "
<< num_vertices(chull) << " points on the convex hull." << std::endl;
return 0;
}
int main()
{
std::vector<Point> points;
points.push_back(Point(0,0));
points.push_back(Point(1,0));
points.push_back(Point(0,1));
points.push_back(Point(4,10));
points.push_back(Point(2,2));
points.push_back(Point(-1,0));
Delaunay T;
T.insert( boost::make_transform_iterator(points.begin(),Auto_count()),
boost::make_transform_iterator(points.end(), Auto_count() ) );
CGAL_assertion( T.number_of_vertices() == 6 );
// check that the info was correctly set.
Delaunay::Finite_vertices_iterator vit;
for (vit = T.finite_vertices_begin(); vit != T.finite_vertices_end(); ++vit)
if( points[ vit->info() ] != vit->point() ){
std::cerr << "Error different info" << std::endl;
exit(EXIT_FAILURE);
}
std::cout << "OK" << std::endl;
return 0;
}
void addNearest(Delaunay &T)
{
Delaunay::Finite_vertices_iterator vit;
for (vit = T.finite_vertices_begin(); vit != T.finite_vertices_end();++vit)
{
Delaunay::Vertex_circulator vit2, done;
std::vector <Vertex_handle> L;
vit2 = vit->incident_vertices();
done = vit2;
do {
if (T.is_infinite(vit2)) continue;
std::stringstream ss1, ss2;
int a, b;
ss1 << vit->info();
ss2 << vit2->info();
ss1 >> a; ss2 >> b;
if (b == a) continue;
G[a].push_back(b);
} while (++vit2 != done);
}
}
int main()
{
CGAL::Geomview_stream gv(CGAL::Bbox_3(-100, -100, -100, 600, 600, 600));
gv.set_line_width(4);
// gv.set_trace(true);
gv.set_bg_color(CGAL::Color(0, 200, 200));
// gv.clear();
Delaunay D;
Delaunay3d D3d;
Terrain T;
std::ifstream iFile("data/points3", std::ios::in);
Point3 p;
while ( iFile >> p )
{
D.insert( Point2(p.x(), p.y()) );
D3d.insert( p );
T.insert( p );
}
// use different colors, and put a few sleeps/clear.
gv << CGAL::BLUE;
std::cout << "Drawing 2D Delaunay triangulation in wired mode.\n";
gv.set_wired(true);
gv << D;
#if 1 // It's too slow ! Needs to use OFF for that.
gv << CGAL::RED;
std::cout << "Drawing its Voronoi diagram.\n";
gv.set_wired(true);
D.draw_dual(gv);
#endif
sleep(5);
gv.clear();
std::cout << "Drawing 2D Delaunay triangulation in non-wired mode.\n";
gv.set_wired(false);
gv << D;
sleep(5);
gv.clear();
std::cout << "Drawing 3D Delaunay triangulation in wired mode.\n";
gv.set_wired(true);
gv << D3d;
sleep(5);
gv.clear();
std::cout << "Drawing 3D Delaunay triangulation in non-wired mode.\n";
gv.set_wired(false);
gv << D3d;
sleep(5);
gv.clear();
std::cout << "Drawing Terrain in wired mode.\n";
gv.set_wired(true);
gv << T;
sleep(5);
gv.clear();
std::cout << "Drawing Terrain in non-wired mode.\n";
gv.set_wired(false);
gv << T;
std::cout << "Enter a key to finish" << std::endl;
char ch;
//std::cin >> ch;
std::cin.get();
return 0;
}
void asign_index_to_cells(std::map<Delaunay::Cell_handle,int> &index,Delaunay &T)
{
int N=0;
for(Delaunay::Finite_cells_iterator itr=T.finite_cells_begin();itr!=T.finite_cells_end();itr++)
{
index.insert(pair<const Delaunay::Finite_cells_iterator,int>(itr,N));
N++;
}
}
bool isEdgeElementary( const Delaunay & t,
const Vertex_handle & v1,
const Vertex_handle & v2 )
{
if ( t.is_infinite( v1 ) || t.is_infinite( v2 ) ) return false;
Z2i::Point a( toDGtal( v1->point())),
b(toDGtal( v2->point()));
return (b-a).norm( Z2i::Point::L_1 ) == 1;
}
int main()
{
Delaunay dt;
Delaunay::Vertex_handle vh;
vh = dt.insert(Point(0,0,0));
vh->info() = "Paris";
vh = dt.insert(Point(1,0,0.1));
vh->info() = "London";
vh = dt.insert(Point(0,1,0.2));
vh->info() = "New York";
return 0;
}
int main()
{
std::ifstream in("data/terrain.cin");
std::istream_iterator<Point> begin(in);
std::istream_iterator<Point> end;
Delaunay dt;
dt.insert(begin, end);
std::cout << dt.number_of_vertices() << std::endl;
std::ofstream fout_T;
fout_T.open("Tri.txt");
fout_T << dt;
fout_T.close();
return 0;
}
void draw_facets(Delaunay &Tr,std::vector<Facet> &facets,PointColor pcolors,CGAL::Geomview_stream &gv) {
if(! gv_on) return;
CGAL::Color colors[] = {CGAL::BLUE,CGAL::GREEN,CGAL::YELLOW,CGAL::DEEPBLUE,
CGAL::PURPLE,CGAL::VIOLET,CGAL::ORANGE,CGAL::RED};
if(pcolors.size() == 0)
return draw_facets(Tr,facets,gv);
// draw with color interpolation
for (std::vector<Facet>::iterator it = facets.begin();it != facets.end();it++) {
Facet f = *it;
Cell_handle c = f.first;
int j = f.second;
CGAL::Color vcolors[3];
int k = 0;
for(int i = 0;i < 4;i++)
if(i != j)
vcolors[k++] = pcolors[c->vertex(i)->info()];
int r = (vcolors[0].red() + vcolors[1].red() + vcolors[2].red()) / 3;
int g = (vcolors[0].green() + vcolors[1].green() + vcolors[2].green()) / 3;
int b = (vcolors[0].blue() + vcolors[1].blue() + vcolors[2].blue()) / 3;
gv << CGAL::Color(r,g,b);
// std::cout << "RGB " << r << " " << g << " " << b <<std::endl;
Triangle t = Tr.triangle(f);
gv << t;
}
}
bool
emptyLatticeTriangle( const Delaunay & t,
const Vertex_handle & v1,
const Vertex_handle & v2,
const Vertex_handle & v3 )
{
if ( t.is_infinite( v1 )
|| t.is_infinite( v2 )
|| t.is_infinite( v3 ) ) return false;
Z2i::Point a( toDGtal( v1->point())),
b(toDGtal( v2->point())),
c(toDGtal( v3->point()));
Z2i::Vector ab( b - a ), ac( c - a );
int d = ab[ 0 ] * ac[ 1 ] - ab[ 1 ] * ac[ 0 ];
return ( d == 1 ) || (d == -1 );
}
int
twiceNbLatticePointsInTriangle( const Delaunay & t,
const Vertex_handle & v1,
const Vertex_handle & v2,
const Vertex_handle & v3 )
{
if ( t.is_infinite( v1 )
|| t.is_infinite( v2 )
|| t.is_infinite( v3 ) ) return 10000000;
Z2i::Point a( toDGtal( v1->point())),
b(toDGtal( v2->point())),
c(toDGtal( v3->point()));
Z2i::Vector ab( b - a ), ac( c - a );
int d = ab[ 0 ] * ac[ 1 ] - ab[ 1 ] * ac[ 0 ];
d = (d >= 0) ? (d - 1) : (-d - 1);
return d;
}
void init_cells(Delaunay &T,int* cells)
{
int S=T.number_of_finite_cells();
int* ptr1=cells;
for(int i=0;i<S;i++)
{
*ptr1=0;
ptr1++;
}
}
void triangulate(Delaunay &T,std::vector<Point> &exterior)
{
for(int i=0;i<20;i++)
{
std::vector<Point>::iterator be=exterior.begin()+exterior.size()*i/20;
std::vector<Point>::iterator en=exterior.begin()+exterior.size()*(i+1)/20;
T.insert(be,en);
std::cout<<"*";
}
}
void draw_facets(Delaunay &Tr,std::vector<Facet> &facets,CGAL::Geomview_stream &gv) {
if(! gv_on) return;
CGAL::Color colors[] = {CGAL::BLUE,CGAL::GREEN,CGAL::YELLOW,CGAL::DEEPBLUE,
CGAL::PURPLE,CGAL::VIOLET,CGAL::ORANGE,CGAL::RED};
int k = 0;
for (std::vector<Facet>::iterator it = facets.begin();it != facets.end();it++,k++) {
gv << colors[k % 8];
Triangle t = Tr.triangle(*it);
gv << t;
}
}
bool
emptyLatticeTriangle( const Delaunay & t, const Face_handle & f )
{
if ( t.is_infinite( f ) ) return false;
Z2i::Point a( toDGtal(f->vertex(0)->point())),
b(toDGtal(f->vertex(1)->point())),
c(toDGtal(f->vertex(2)->point()));
Z2i::Vector ab( b - a ), ac( c - a );
int d = ab[ 0 ] * ac[ 1 ] - ab[ 1 ] * ac[ 0 ];
return ( d == 1 ) || (d == -1 );
}
int main () {
cin.sync_with_stdio(false);
cout.sync_with_stdio(false);
Delaunay d;
int n;
cin>>n;
for (int i=0;i<n;++i) {
double x,y;
cin>>x>>y;
d.add_point(x, y);
}
d.build();
auto t=d.get_triangles();
size_t cnt=t.size()*3;
cout<<cnt<<endl;
/*for (auto x : t) {
cout<<x.a->x<<' '<<x.a->y<<' '<<x.b->x<<' '<<x.b->y<<endl;
cout<<x.a->x<<' '<<x.a->y<<' '<<x.c->x<<' '<<x.c->y<<endl;
cout<<x.b->x<<' '<<x.b->y<<' '<<x.c->x<<' '<<x.c->y<<endl;
}*/
/*Delaunay d;
for (;;) {
double x,y;
if (!cin.good()) break;
cin>>x;
if (!cin.good()) break;
cin>>y;
d.add_point(x, y);
}
d.build();
double answer=0;
size_t ans=0;
vector< vector<const Delaunay::point *> > cells;
d.build_voronoi_cells(cells);
for (size_t i = 0; i < cells.size(); ++i)
if (cells[i].size())
answer += cells[i].size(), ++ans;
cout<<(ans==0 ? 0 : (answer/ans))<<endl;*/
return 0;
}
void SparseVectorField::triangulate()
{
Delaunay dt;
Delaunay::Finite_edges_iterator eIter;
Delaunay::Finite_faces_iterator fIter;
vector< Point >::const_iterator fpIter;
int i;
map< Delaunay::Vertex_handle, int > V;
map< Point, int > Vi;
Delaunay::Finite_vertices_iterator vIter;
dt.insert(startPoints_.begin(), startPoints_.end());
// Map each vertex to its index.
for(int i = 0; i < startPoints_.size(); i++)
Vi[startPoints_[i]] = i;
// Map vertices in the Delaunay triangulation to the original vertices.
for(vIter = dt.finite_vertices_begin(); vIter != dt.finite_vertices_end(); vIter++)
V[vIter] = Vi[Point(vIter->point().x(), vIter->point().y())];
// Retrieve vertex indices from the vertex map.
for(fIter = dt.finite_faces_begin(); fIter != dt.finite_faces_end(); fIter++)
{
Delaunay::Face f = *fIter;
triIndices_.push_back(V[f.vertex(0)]);
triIndices_.push_back(V[f.vertex(1)]);
triIndices_.push_back(V[f.vertex(2)]);
}
triangulationValid_ = true;
}
int main(int argc, char **argv)
{
int n=1000000;
int rep=100;
if (argc>=2)
n=atoi(argv[1]);
if (argc>=3)
rep=atoi(argv[2]);
std::vector<Point> points;
points.reserve(n);
CGAL::Random_points_in_disc_2<Point,Creator> g(1);
CGAL::copy_n( g, n, std::back_inserter(points));
Delaunay original;
original.insert(points.begin(),points.end());
double res=0;
for (int r=0;r<rep;++r){
Delaunay delaunay=original;
std::vector<Vertex_handle> vertices;
for(FVI fvi = delaunay.finite_vertices_begin(); fvi != delaunay.finite_vertices_end();++fvi){
vertices.push_back(fvi);
}
CGAL::Timer t;
t.start();
for (int k=0; k<vertices.size(); ++k)
delaunay.remove(vertices[k]);
t.stop();
res+=t.time();
if (delaunay.number_of_vertices()!=0){
std::cerr << "ERROR"<< std::endl;
return 1;
}
}
std::cout << res/rep << std::endl;
return 0;
}
void draw_cells_edges(Delaunay &Tr,std::vector<Cell_handle> &cells,CGAL::Geomview_stream &gv) {
if(! gv_on) return;
CGAL::Color colors[] = {CGAL::BLUE,CGAL::GREEN,CGAL::YELLOW};
int k = 0;
for (std::vector<Cell_handle>::iterator it = cells.begin();it != cells.end();it++,k++) {
Cell_handle c = *it;
gv << colors[k % 3];
for(int i = 0;i<4;i++)
for(int j = i+1;j < 4;j++) {
Segment e = Tr.segment(c,i,j);
gv << e;
}
}
}
void draw_tetra(Delaunay &Tr,std::vector<Cell_handle> &cells,CGAL::Geomview_stream &gv) {
if(! gv_on) return;
CGAL::Color colors[] = {CGAL::BLUE,CGAL::GREEN,CGAL::YELLOW,CGAL::DEEPBLUE,
CGAL::PURPLE,CGAL::VIOLET,CGAL::ORANGE,CGAL::RED};
int k = 0;
for (std::vector<Cell_handle>::iterator it = cells.begin();it != cells.end();it++,k++) {
Cell_handle c = *it;
gv << colors[k % 8];
Tetrahedron t = Tr.tetrahedron(c);
// Tetrahedron t(c->vertex(0)->point(),c->vertex(1)->point(),
// c->vertex(2)->point(),c->vertex(3)->point());
gv << t;
}
}
int main()
{
std::ifstream fin("terrain.pts"); // elevation ranges from 0 to 100
Delaunay dt;
dt.insert(std::istream_iterator<Point_3>(fin),
std::istream_iterator<Point_3>());
Interval_skip_list isl;
for(Finite_faces_iterator fh = dt.finite_faces_begin();
fh != dt.finite_faces_end();
++fh){
isl.insert(Interval(fh));
}
std::list<Interval> level;
isl.find_intervals(50, std::back_inserter(level));
for(std::list<Interval>::iterator it = level.begin();
it != level.end();
++it){
std::cout << dt.triangle(it->face_handle()) << std::endl;
}
return 0;
}
int main()
{
Delaunay T;
CGAL::Random_points_in_sphere_3<Point> rnd;
// First, make sure the triangulation is 3D.
T.insert(Point(0,0,0));
T.insert(Point(1,0,0));
T.insert(Point(0,1,0));
T.insert(Point(0,0,1));
assert(T.dimension() == 3);
// Inserts 100 random points if and only if their insertion
// in the Delaunay tetrahedralization conflicts with
// an even number of cells.
for (int i = 0; i != 100; ++i) {
Point p = *rnd++;
// Locate the point
Delaunay::Locate_type lt;
int li, lj;
Cell_handle c = T.locate(p, lt, li, lj);
if (lt == Delaunay::VERTEX)
continue; // Point already exists
// Get the cells that conflict with p in a vector V,
// and a facet on the boundary of this hole in f.
std::vector<Cell_handle> V;
Facet f;
T.find_conflicts(p, c,
CGAL::Oneset_iterator<Facet>(f), // Get one boundary facet
std::back_inserter(V)); // Conflict cells in V
if ((V.size() & 1) == 0) // Even number of conflict cells ?
T.insert_in_hole(p, V.begin(), V.end(), f.first, f.second);
}
std::cout << "Final triangulation has " << T.number_of_vertices()
<< " vertices." << std::endl;
return 0;
}
string DelaunayMesh::InsertSites(Delaunay & mesh, vector<Point2D *> & input)
{
string answer = "";
for(unsigned int i = 0; i < input.size(); i++)
{
// cerr << "insert " << i << "/" << input.size() << " : (" << input[i]->id << ", " << input[i]->x << ", " << input[i]->y << ")" << endl; // debug
switch(mesh.InsertSite(*input[i]))
{
case Delaunay::INSERT_OK:
// nothing else to do
break;
case Delaunay::INSERT_DUPLICATE:
input[i]->category = Point2D::ORPHAN;
break;
case Delaunay::INSERT_FAIL:
// answer = "cannot insert site";
input[i]->category = Point2D::ORPHAN;
break;
default:
throw Exception("LloydRelaxation::InsertSites(): unhandled insert site case");
}
}
#if 0
vector<Point2D *> output;
for(unsigned int i = 0; i < input.size(); i++)
{
if(input[i])
{
output.push_back(input[i]);
}
}
input = output;
#endif
return answer;
}
void VoronoiCgal_Patch::mark_domains(Delaunay& ct, Delaunay::Face_handle start,
int index, std::list<Delaunay::Edge>& border)
{
if (start->info().nesting_level != TRIANGLE_NOT_INIT)
{
return;
}
std::list<Delaunay::Face_handle> queue;
queue.push_back(start);
while (! queue.empty())
{
Delaunay::Face_handle fh = queue.front();
queue.pop_front();
if (fh->info().nesting_level == TRIANGLE_NOT_INIT)
{
fh->info().nesting_level = index;
for (int i = 0; i < 3; i++)
{
Delaunay::Edge e(fh, i);
Delaunay::Face_handle n = fh->neighbor(i);
if (n->info().nesting_level == TRIANGLE_NOT_INIT)
{
if (ct.is_constrained(e))
{
border.push_back(e);
}
else
{
queue.push_back(n);
}
}
}
}
}
}
int main()
{
Delaunay T;
CGAL::Random_points_in_cube_3<Point> rnd(0.5);
GT::Vector_3 v(0.5,0.5,0.5);
// First, make sure the triangulation is 3D.
T.insert(Point(0,0,0));
T.insert(Point(.1,0,0));
T.insert(Point(0,.1,0));
T.insert(Point(0,0,.1));
// Gets the conflict region of 100 random points
// in the Delaunay tetrahedralization
for (int i = 0; i != 100; ++i) {
Point p = (*rnd++)+v;
// Locate the point
Delaunay::Locate_type lt;
int li, lj;
Cell_handle c = T.locate(p, lt, li, lj);
if (lt == Delaunay::VERTEX)
continue; // Point already exists
// Get the cells that conflict with p in a vector V,
// and a facet on the boundary of this hole in f.
std::vector<Cell_handle> V;
Facet f;
T.find_conflicts(p, c,
CGAL::Oneset_iterator<Facet>(f), // Get one boundary facet
std::back_inserter(V)); // Conflict cells in V
}
std::cout << "Final triangulation has " << T.number_of_vertices()
<< " vertices." << std::endl;
return 0;
}
void TrPlaneStress2dXFEM :: XfemElementInterface_partitionElement(AList< Triangle > *answer, std :: vector< FloatArray > &together)
{
Delaunay dl;
dl.triangulate(together, answer);
return;
/*
// Two cases may occur when partitioning: we will get a subdomain
// with either 3 or 4 nodes.
if( together->giveSize() == 3 )
{
// 3 nodes
FloatArray *p1 = new FloatArray();
* p1 = * ( together->at(1) );
FloatArray *p2 = new FloatArray();
* p2 = * ( together->at(2) );
FloatArray *p3 = new FloatArray();
* p3 = * ( together->at(3) );
Triangle *triangle = new Triangle(p1, p2, p3);
if ( !triangle->isOrientedAnticlockwise() ) {
triangle->changeToAnticlockwise();
}
answer->put(1, triangle);
}
else
{
if( together->giveSize() == 4 )
{
// 4 nodes
// The array *together contains the four vertices of the area to be subdivided.
// The intersection points are located in the first two entries in the array and
// the vertex points are located in the last two entries in the array. We do not know
// if the last two entries are numbered such that the four vertices form a proper quad.
// We have to check if this is the case and if not, we swap the last two points.
int nodeMap[4] = {1, 2, 3, 4};
//////////////////////////////////////////////////////////////////////
// We have two options for the subdivision.
//
// Which option we choose will probably not matter in the end.
// However, trying to get triangles of good quality can't do
// any harm. Start by checking the mesh quality we
// will get for the two choices.
FloatArray *p1 = new FloatArray();
* p1 = * ( together->at(nodeMap[0]) );
FloatArray *p2 = new FloatArray();
* p2 = * ( together->at(nodeMap[1]) );
FloatArray *p3 = new FloatArray();
* p3 = * ( together->at(nodeMap[2]) );
FloatArray *p4 = new FloatArray();
* p4 = * ( together->at(nodeMap[3]) );
// bPoint2 b1_tmp( p1->at(1), p1->at(2) );
// bPoint2 b2_tmp( p2->at(1), p2->at(2) );
// bPoint2 b3_tmp( p3->at(1), p3->at(2) );
// bPoint2 b4_tmp( p4->at(1), p4->at(2) );
// bPoint2 cutLine( b2_tmp.x() - b1_tmp.x(), b2_tmp.y() - b1_tmp.y() );
FloatArray cutLine;
cutLine.beDifferenceOf(*p2, *p1);
// bPoint2 elLine( b4_tmp.x() - b3_tmp.x(), b4_tmp.y() - b3_tmp.y() );
FloatArray elLine;
elLine.beDifferenceOf(*p2, *p3);
// if( bDot(cutLine, elLine) > 0 )
if( cutLine.dotProduct(elLine) > 0.0 )
{
// printf("Permuting node map.\n");
nodeMap[2] = 4;
nodeMap[3] = 3;
}
* p1 = * ( together->at(nodeMap[0]) );
* p2 = * ( together->at(nodeMap[1]) );
* p3 = * ( together->at(nodeMap[2]) );
* p4 = * ( together->at(nodeMap[3]) );
// bPoint2 b1( p1->at(1), p1->at(2) );
// bPoint2 b2( p2->at(1), p2->at(2) );
// bPoint2 b3( p3->at(1), p3->at(2) );
// bPoint2 b4( p4->at(1), p4->at(2) );
// Subdivision alternative 1
// 1 - 2 - 4
// and
// 2 - 3 - 4
// Triangle 1
// double a1_1 = bDist( b1, b2 );
double a1_1 = p1->distance(*p2);
// double b1_1 = bDist( b2, b4 );
double b1_1 = p2->distance(*p4);
// double c1_1 = bDist( b4, b1 );
double c1_1 = p4->distance(*p1);
// bSeg2 AB1_1(b1, b2);
// double h1_1 = bDist(b4, AB1_1);
double h1_1 = p4->distance(*p1,*p2);
double A1_1 = 0.5*a1_1*h1_1;
double R1_1 = 0.25*a1_1*b1_1*c1_1/A1_1;
double sin_a1_1 = 0.5*a1_1/R1_1;
double sin_b1_1 = 0.5*b1_1/R1_1;
double sin_c1_1 = 0.5*c1_1/R1_1;
double rho1_1 = ( sin_a1_1 + sin_b1_1 + sin_c1_1 )/( 2.0*sin_a1_1*sin_b1_1*sin_c1_1 );
// printf("rho1_1: %e ", rho1_1);
// Triangle 2
// double a2_1 = bDist( b2, b3 );
double a2_1 = p2->distance(*p3);
// double b2_1 = bDist( b3, b4 );
double b2_1 = p3->distance(*p4);
// double c2_1 = bDist( b4, b2 );
double c2_1 = p4->distance(*p2);
// bSeg2 AB2_1(b2, b3);
// double h2_1 = bDist(b4, AB2_1);
double h2_1 = p4->distance(*p2,*p3);
double A2_1 = 0.5*a2_1*h2_1;
double R2_1 = 0.25*a2_1*b2_1*c2_1/A2_1;
double sin_a2_1 = 0.5*a2_1/R2_1;
double sin_b2_1 = 0.5*b2_1/R2_1;
double sin_c2_1 = 0.5*c2_1/R2_1;
double rho2_1 = ( sin_a2_1 + sin_b2_1 + sin_c2_1 )/( 2.0*sin_a2_1*sin_b2_1*sin_c2_1 );
// printf("rho2_1: %e\n", rho2_1);
double worst1 = max( fabs(rho1_1-2.0), fabs(rho2_1-2.0) );
// printf("worst1: %e\n", worst1 );
// Subdivision alternative 2
// 1 - 2 - 3
// and
// 1 - 3 - 4
// Triangle 1
// double a1_2 = bDist( b1, b2 );
double a1_2 = p1->distance(p2);
// double b1_2 = bDist( b2, b3 );
double b1_2 = p2->distance(*p3);
// double c1_2 = bDist( b3, b1 );
double c1_2 = p3->distance(*p1);
// bSeg2 AB1_2(b1, b2);
// double h1_2 = bDist(b3, AB1_2);
double h1_2 = p3->distance(*p1,*p2);
double A1_2 = 0.5*a1_2*h1_2;
double R1_2 = 0.25*a1_2*b1_2*c1_2/A1_2;
double sin_a1_2 = 0.5*a1_2/R1_2;
double sin_b1_2 = 0.5*b1_2/R1_2;
double sin_c1_2 = 0.5*c1_2/R1_2;
double rho1_2 = ( sin_a1_2 + sin_b1_2 + sin_c1_2 )/( 2.0*sin_a1_2*sin_b1_2*sin_c1_2 );
// printf("rho1_2: %e ", rho1_2);
// Triangle 2
// double a2_2 = bDist( b1, b3 );
double a2_2 = p1->distance(*p3);
// double b2_2 = bDist( b3, b4 );
double b2_2 = p3->distance(*p4);
// double c2_2 = bDist( b4, b1 );
double c2_2 = p4->distance(*p1);
// bSeg2 AB2_2(b1, b3);
// double h2_2 = bDist(b4, AB2_2);
double h2_2 = p4->distance(*p1,*p3);
double A2_2 = 0.5*a2_2*h2_2;
double R2_2 = 0.25*a2_2*b2_2*c2_2/A2_2;
double sin_a2_2 = 0.5*a2_2/R2_2;
double sin_b2_2 = 0.5*b2_2/R2_2;
double sin_c2_2 = 0.5*c2_2/R2_2;
double rho2_2 = ( sin_a2_2 + sin_b2_2 + sin_c2_2 )/( 2.0*sin_a2_2*sin_b2_2*sin_c2_2 );
// printf("rho2_2: %e\n", rho2_2);
double worst2 = max( fabs(rho1_2-2.0), fabs(rho2_2-2.0) );
// printf("worst2: %e\n", worst2 );
//////////////////////////////////////////////////////////////////////
if( worst1 < worst2 )
{
// Choose subdivision 1
// 1 - 2 - 4
// and
// 2 - 3 - 4
// printf("Choosing subdivision 1.\n");
FloatArray *p1A = new FloatArray();
* p1A = * ( together->at(nodeMap[0]) );
FloatArray *p2A = new FloatArray();
* p2A = * ( together->at(nodeMap[1]) );
FloatArray *p3A = new FloatArray();
* p3A = * ( together->at(nodeMap[3]) );
Triangle *triangle1 = new Triangle(p1A, p2A, p3A);
if ( !triangle1->isOrientedAnticlockwise() ) {
triangle1->changeToAnticlockwise();
}
answer->put(1, triangle1);
FloatArray *p1B = new FloatArray();
* p1B = * ( together->at(nodeMap[1]) );
FloatArray *p2B = new FloatArray();
* p2B = * ( together->at(nodeMap[2]) );
FloatArray *p3B = new FloatArray();
* p3B = * ( together->at(nodeMap[3]) );
Triangle *triangle2 = new Triangle(p1B, p2B, p3B);
if ( !triangle2->isOrientedAnticlockwise() ) {
triangle2->changeToAnticlockwise();
}
answer->put(2, triangle2);
}
else
{
// Choose subdivision 2
// 1 - 2 - 3
// and
// 1 - 3 - 4
// printf("Choosing subdivision 2.\n");
FloatArray *p1A = new FloatArray();
* p1A = * ( together->at(nodeMap[0]) );
FloatArray *p2A = new FloatArray();
* p2A = * ( together->at(nodeMap[1]) );
FloatArray *p3A = new FloatArray();
* p3A = * ( together->at(nodeMap[2]) );
Triangle *triangle1 = new Triangle(p1A, p2A, p3A);
if ( !triangle1->isOrientedAnticlockwise() ) {
triangle1->changeToAnticlockwise();
}
answer->put(1, triangle1);
FloatArray *p1B = new FloatArray();
* p1B = * ( together->at(nodeMap[0]) );
FloatArray *p2B = new FloatArray();
* p2B = * ( together->at(nodeMap[2]) );
FloatArray *p3B = new FloatArray();
* p3B = * ( together->at(nodeMap[3]) );
Triangle *triangle2 = new Triangle(p1B, p2B, p3B);
if ( !triangle2->isOrientedAnticlockwise() ) {
triangle2->changeToAnticlockwise();
}
answer->put(2, triangle2);
}
}
}
*/
}
void computeD(Delaunay &T)
{
T.insert(points.begin(), points.end());
G.resize(n);
}
void EKFOA::process(const double delta_t, cv::Mat & frame, Eigen::Vector3d & rW, Eigen::Vector4d & qWR, Eigen::Matrix3d & axes_orientation_and_confidence, std::vector<Point3d> (& XYZs)[3], Delaunay & triangulation, Point3d & closest_point){
double time_total;
std::vector<cv::Point2f> features_to_add;
std::vector<Features_extra> features_extra;
/*
* EKF prediction (state and measurement prediction)
*/
time_total = (double)cv::getTickCount();
double time_prediction = (double)cv::getTickCount();
filter.predict_state_and_covariance(delta_t);
filter.compute_features_h(cam, features_extra);
time_prediction = (double)cv::getTickCount() - time_prediction;
// std::cout << "predict = " << time_prediction/((double)cvGetTickFrequency()*1000.) << "ms" << std::endl;
/*
* Sense and map management (delete features from EKF)
*/
double time_tracker = (double)cv::getTickCount();
motion_tracker.process(frame, features_extra, features_to_add);
//TODO: Why is optical flow returning points outside the image???
time_tracker = (double)cv::getTickCount() - time_tracker;
time_total = time_total + time_tracker; //do not count the time spent by the tracker
//Delete no longer seen features from the state, covariance matrix and the features_extra:
double time_del = (double)cv::getTickCount();
filter.delete_features(features_extra);
time_del = (double)cv::getTickCount() - time_del;
// std::cout << "delete = " << time_del/((double)cvGetTickFrequency()*1000.) << "ms" << std::endl;
/*
* EKF Update step and map management (add new features to EKF)
*/
double time_update = (double)cv::getTickCount();
filter.update(cam, features_extra);
time_update = (double)cv::getTickCount() - time_update;
// std::cout << "update = " << time_update/((double)cvGetTickFrequency()*1000.) << "ms" << std::endl;
//Add new features
double time_add = (double)cv::getTickCount();
filter.add_features_inverse_depth(cam, features_to_add);
time_add = (double)cv::getTickCount() - time_add;
// std::cout << "add_fea = " << time_add/((double)cvGetTickFrequency()*1000.) << "ms" << std::endl;
/*
* Triangulation, surface and GUI data setting:
*/
double time_triangulation = (double)cv::getTickCount();
std::vector< std::pair<Point2d, size_t> > triangle_list;
std::list<Triangle> triangles_list_3d;
const Eigen::VectorXd & x_k_k = filter.x_k_k();
const Eigen::MatrixXd & p_k_k = filter.p_k_k();
//Set the position, so the GUI can draw it:
rW = x_k_k.segment<3>(0);//current position
//Set the axes orientation and confidence:
axes_orientation_and_confidence.setIdentity();//axes_orientation_and_confidence stores in each column one axis (X, Y, Z)
axes_orientation_and_confidence *= 5; //make the lines larger, so they are actually informative
//Apply rotation matrix:
Eigen::Matrix3d qWR_R;//Rotation matrix of current orientation quaternion
qWR = x_k_k.segment<4>(3);
MotionModel::quaternion_matrix(qWR, qWR_R);
axes_orientation_and_confidence.applyOnTheLeft(qWR_R); // == R * axes_orientation_and_confidence
for (int axis=0 ; axis<axes_orientation_and_confidence.cols() ; axis++){
//Set the length to be 3*sigma:
axes_orientation_and_confidence.col(axis) *= 3*std::sqrt(p_k_k(axis, axis)); //the first 3 positions of the cov matrix define the confidence for the position
//Translate origin:
axes_orientation_and_confidence.col(axis) += rW;
}
int num_features = (x_k_k.rows()-13)/6;
XYZs[0].resize(num_features);
XYZs[1].resize(num_features);
XYZs[2].resize(num_features);
//Compute the 3d positions and inverse depth variances of all the points in the state
int i=0; //Feature counter
for (int start_feature=13 ; start_feature<x_k_k.rows() ; start_feature+=6){
const int feature_inv_depth_index = start_feature + 5;
//As with any normal distribution, nearly all (99.73%) of the possible depths lie within three standard deviations of the mean!
const double sigma_3 = std::sqrt(p_k_k(feature_inv_depth_index, feature_inv_depth_index)); //sqrt(depth_variance)
const Eigen::VectorXd & yi = x_k_k.segment(start_feature, 6);
Eigen::VectorXd point_close(x_k_k.segment(start_feature, 6));
Eigen::VectorXd point_far(x_k_k.segment(start_feature, 6));
//Change the depth of the feature copy, so that it is possible to represent the range between -3*sigma and 3*sigma:
point_close(5) += sigma_3;
point_far(5) -= sigma_3;
Eigen::Vector3d XYZ_mu = (Feature::compute_cartesian(yi)); //mu (mean)
Eigen::Vector3d XYZ_close = (Feature::compute_cartesian(point_close)); //mean + 3*sigma. (since inverted signs are also inverted)
Eigen::Vector3d XYZ_far = (Feature::compute_cartesian(point_far)); //mean - 3*sigma
//The center of the model is ALWAYS the current position of the camera/robot, so have to 'cancel' the current orientation (R_inv) and translation (rWC = x_k_k.head(3)):
//Note: It is nicer to do this in the GUI class, as it is only a presention/perspective change. But due to the structure, it was easier to do it here.
XYZs[0][i] = Point3d(XYZ_mu(0), XYZ_mu(1), XYZ_mu(2)); //mu (mean)
XYZs[1][i] = Point3d(XYZ_close(0), XYZ_close(1), XYZ_close(2)); //mean + 3*sigma. (since inverted signs are also inverted)
XYZs[2][i] = Point3d(XYZ_far(0), XYZ_far(1), XYZ_far(2)); //mean - 3*sigma
//If the size that contains the 99.73% of the inverse depth distribution is smaller than the current inverse depth, add it to the surface:
const double size_sigma_3 = std::abs(1.0/(x_k_k(feature_inv_depth_index)-sigma_3) - 1.0/(x_k_k(feature_inv_depth_index)+sigma_3));
if (size_sigma_3 < 1/x_k_k(feature_inv_depth_index)){
triangle_list.push_back(std::make_pair(Point2d(features_extra[i].z(0), features_extra[i].z(1)), i));
}
if (x_k_k(feature_inv_depth_index) < 0 ){
std::cout << "feature behind the camera!!! : idx=" << i << ", value=" << x_k_k(feature_inv_depth_index) << std::endl;
}
i++;
}
triangulation.insert(triangle_list.begin(), triangle_list.end());
cv::Scalar delaunay_color = cv::Scalar(255, 0, 0); //blue
for(Delaunay::Finite_faces_iterator fit = triangulation.finite_faces_begin(); fit != triangulation.finite_faces_end(); ++fit) {
const Delaunay::Face_handle & face = fit;
//face->vertex(i)->info() = index of the point in the observation list.
line(frame, features_extra[face->vertex(0)->info()].z_cv, features_extra[face->vertex(1)->info()].z_cv, delaunay_color, 1);
line(frame, features_extra[face->vertex(1)->info()].z_cv, features_extra[face->vertex(2)->info()].z_cv, delaunay_color, 1);
line(frame, features_extra[face->vertex(2)->info()].z_cv, features_extra[face->vertex(0)->info()].z_cv, delaunay_color, 1);
//Add the face of the linked 3d points of this 2d triangle:
triangles_list_3d.push_back(Triangle(XYZs[1][face->vertex(0)->info()], XYZs[1][face->vertex(1)->info()], XYZs[1][face->vertex(2)->info()])); //XYZs[1] == close
}
// constructs AABB tree
Tree tree(triangles_list_3d.begin(), triangles_list_3d.end());
if (tree.size()>0){
// compute closest point and squared distance
Point3d point_query(rW[0], rW[1], rW[2]);
closest_point = tree.closest_point(point_query);
// FT sqd = tree.squared_distance(point_query);
Eigen::Vector3d last_displacement_vector = last_position - rW;
// double repealing_force = 0;
// if (std::sqrt(sqd) < 0.2){
// std::cout << "can crash! " << std::endl;
// repealing_force = 1/std::sqrt(sqd);
// }
// std::cout << "distance = [distance, " << std::sqrt(sqd) << "];" << std::endl;
// std::cout << "repealing_force = [repealing_force, " << repealing_force << "];" << std::endl;
}
//remember this position
last_position = rW;
// std::cout << "certaint= " << p_k_k.diagonal().sum() << std::endl;
time_triangulation = (double)cv::getTickCount() - time_triangulation;
// std::cout << "Triang = " << time_triangulation/((double)cvGetTickFrequency()*1000.) << "ms" << std::endl;
time_total = (double)cv::getTickCount() - time_total;
// std::cout << "EKF = " << time_total/((double)cvGetTickFrequency()*1000.) << "ms" << std::endl;
// std::cout << "tracker = " << time_tracker/((double)cvGetTickFrequency()*1000.) << "ms" << std::endl;
}
/**
* Performs the testing of dynamic convex hull of CGAL.
*/
int main(int argc, char** argv)
{
DEBUG_START;
if (argc != 2)
{
print_usage(argc, argv);
DEBUG_END;
return EXIT_FAILURE;
}
int num_points = 0;
if (sscanf(argv[1], "%d", &num_points) != 1)
{
print_usage(argc, argv);
DEBUG_END;
return EXIT_FAILURE;
}
CGAL::Random_points_on_sphere_3<Point_3> gen(100.0);
std::list<Point_3> points;
/*
* generate <num_points> points randomly on a sphere of radius 100.0
* and copy them to a std::vector
*/
CGAL::cpp11::copy_n(gen, num_points, std::back_inserter(points) );
/* begin time measurement */
TimeMeasurer timer;
timer.pushTimer();
Delaunay T;
T.insert(points.begin(), points.end());
std::list<Vertex_handle> vertices;
T.incident_vertices(T.infinite_vertex(), std::back_inserter(vertices));
/*
* copy the convex hull of points into a polyhedron and use it
* to get the number of points on the convex hull
*/
Polyhedron_3 chull0;
CGAL::convex_hull_3_to_polyhedron_3(T,chull0);
std::cout << "The convex hull contains " << chull0.size_of_vertices()
<< " vertices" << std::endl;
/* end time measurement */
double timeFirst = timer.popTimer();
printf("Time for initial construction of convex hull: %lf\n", timeFirst);
/* begin time measurement */
timer.pushTimer();
/* Remove 90% of points. */
float nReduced = (1. - 1. / (float) FACTOR_OF_VERTICES_REDUCTION) *
chull0.size_of_vertices();
std::list<Vertex_handle>::iterator v_set_it = vertices.begin();
for (int i = 0; i < nReduced; i++)
{
T.remove(*v_set_it);
v_set_it++;
}
/*
* copy the convex hull of points into a polyhedron and use it
* to get the number of points on the convex hull
*/
Polyhedron_3 chull;
CGAL::convex_hull_3_to_polyhedron_3(T,chull);
/* end time measurement */
double timeSecond = timer.popTimer();
printf("Time for recalculating of convex hull: %lf\n", timeSecond);
std::cout << "The convex hull contains " << chull.size_of_vertices()
<< " vertices" << std::endl;
DEBUG_END;
return 0;
}
void LFS::compute(const std::vector<vec3>& vertices,
double sliver_quality_degree /*= 0.01*/,
const bool* feature_label /*= 0*/) {
Delaunay* del = Delaunay::create("CGAL_spatial_sort") ;
del->set_vertices(vertices) ;
std::vector<int> tets ;
del->get_tetras(tets, false) ;
unsigned int nb_tets = (unsigned int)tets.size() / 4 ;
//compute tets' circumcenter and radius
std::vector<vec3> tet_circumcenter(nb_tets);
std::vector<double> tet_radius(nb_tets);
std::vector<bool> used_tet(nb_tets, false); // to avoid multiple insertion
int nv = (int)vertices.size();
poles.clear();
poles.reserve(nv*2);
std::vector<double> dis(nv, 0);
std::vector<int> tetid(nv, -1);
std::vector<bool> infinite_flag(nv, false);
std::vector<bool> infinite_tet(nb_tets, false);
N.clear();
N.resize(nv);
std::fill(N.begin(),N.end(), vec3(0,0,0)); // N x + d = 0 is a plane equation
std::vector<double> d(nv, 0);
std::vector<bool> good_tetra(nb_tets, true);
int start = 0;
int vertex_id[4];
vec3 normal;
double quality[6];
int bad_tetra = 0;
const double mPI = 3.1415926535897932384626433832795;
const double sliver_quality = sliver_quality_degree / 180. * mPI;
for(unsigned int i = 0; i < nb_tets; i++)
{
//set id
int inf_id = -1;
for (int j = 0; j < 4; j++)
{
vertex_id[j] = tets[start+j];
if (vertex_id[j] == -1)
{
infinite_tet[i] = true;
inf_id = j;
}
}
if (!infinite_tet[i])
{
bool suc = tetra_circumcenter_squaredradius(
vertices[tets[start]],
vertices[tets[start+1]],
vertices[tets[start+2]],
vertices[tets[start+3]],
tet_circumcenter[i], tet_radius[i], quality);
if ( suc == false ||
(
(quality[0] <= sliver_quality || quality[0] >= mPI - sliver_quality) &&
(quality[1] <= sliver_quality || quality[1] >= mPI - sliver_quality) &&
(quality[2] <= sliver_quality || quality[2] >= mPI - sliver_quality) &&
(quality[3] <= sliver_quality || quality[3] >= mPI - sliver_quality) &&
(quality[4] <= sliver_quality || quality[4] >= mPI - sliver_quality) &&
(quality[5] <= sliver_quality || quality[5] >= mPI - sliver_quality)
)
)
{
good_tetra[i] = false;
bad_tetra++;
}
//set dis
if (good_tetra[i])
{
for (int j = 0; j < 4; j++)
{
if ( tet_radius[i] > dis[ vertex_id[j] ] )
{
dis[ vertex_id[j] ] = tet_radius[i];
tetid[ vertex_id[j] ] = i;
}
}
}
}
else
{
const int &id1 = vertex_id[(inf_id+1)%4];
const int &id2 = vertex_id[(inf_id+2)%4];
const int &id3 = vertex_id[(inf_id+3)%4];
infinite_flag[ id1 ] = true;
infinite_flag[ id2 ] = true;
infinite_flag[ id3 ] = true;
normal = inf_id%2==0?
normalize(cross(vertices[id3]-vertices[id1], vertices[id2]-vertices[id1])):
normalize(cross(vertices[id2]-vertices[id1], vertices[id3]-vertices[id1]));
N[id1] += normal;
N[id2] += normal;
N[id3] += normal;
}
start += 4;
}
std::cerr <<"Total tetrahedrons (including infinite tetras): " << nb_tets << std::endl;
std::cerr <<"detected " << bad_tetra << " sliver tetrahedrons (min dihedral angle = " << sliver_quality_degree <<" degree" << std::endl;
//set poles, first round
for (int i = 0; i < nv; i++)
{
if (!infinite_flag[i])
{
if ( used_tet[tetid[i]] == false &&
(feature_label == 0 || feature_label[i] == false))
{
poles.push_back( tet_circumcenter[ tetid[i] ] );
used_tet[tetid[i]] = true;
}
N[i] = tet_circumcenter[ tetid[i] ] - vertices[i];
}
d[i] = -Geex::dot(N[i], vertices[i]);
}
//set poles, second round
std::fill(dis.begin(),dis.end(), 0);
std::fill(tetid.begin(),tetid.end(), -1);
start = 0;
for (unsigned int i = 0; i < nb_tets; i++)
{
if (!infinite_tet[i] && good_tetra[i])
{
for (int j = 0; j < 4; j++)
{
const int& vid = tets[start+j];
if ( tet_radius[i] > dis[ vid ] && Geex::dot(N[vid], tet_circumcenter[i])+ d[vid] < 0 )
{
dis[vid] = tet_radius[i];
tetid[vid] = i;
}
}
}
start += 4;
}
for (int i = 0; i < nv; i++)
{
if ( tetid[i] >= 0 && used_tet[tetid[i]] == false &&
(feature_label == 0 || feature_label[i] == false))
{
poles.push_back( tet_circumcenter[ tetid[i] ] );
used_tet[tetid[i]] = true;
}
}
delete del ;
create_kdtree();
}