├── CMakeLists.txt
├── COPYRIGHT
├── README.md
├── examples
    ├── lab-recons.png
    ├── lab.png
    ├── srecons1.png
    └── srecons2.png
├── include
    └── SparseReconstruction.h
├── install_maxflow_lib.sh
├── maxflow_cmake.txt
└── src
    └── SparseReconstruction.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(sparse_reconstruction)
 3 | 
 4 | set(VISUALIZATION ON)
 5 | 
 6 | 
 7 | add_library( sparse_reconstruction SHARED
 8 |        maxflow/maxflow 
 9 |        maxflow/block.h 
10 |        maxflow/graph.h 
11 |        maxflow/instances.inc 
12 |        maxflow/graph.cpp 
13 |        maxflow/maxflow.cpp
14 |        src/SparseReconstruction.cpp
15 |        include/SparseReconstruction.h
16 |        )
17 | 
18 | include_directories(
19 |   include
20 |   maxflow
21 | ) 
22 | 
23 | 
24 | 
25 | set(GCC_CGAL_COMPILE_FLAGS "-frounding-math")
26 | set( CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} ${GCC_CGAL_COMPILE_FLAGS} -Wno-write-strings -o3" )
27 | 
28 | 
29 | set(SPARSE_RECONSTRUCTION_LIBRARIES gmp CGAL CGAL_Core)
30 | 
31 | if(VISUALIZATION)
32 |   add_definitions(-DGL_VISUALIZATION)
33 |   set(GL_LIBS GL glut)
34 | endif()
35 | 
36 | set_target_properties(sparse_reconstruction
37 |     PROPERTIES
38 |     ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/lib"
39 |     RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/lib"
40 |     LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/lib"
41 | )
42 | 
43 | target_link_libraries(sparse_reconstruction ${SPARSE_RECONSTRUCTION_LIBRARIES} ${GL_LIBS})
44 | 
45 | 


--------------------------------------------------------------------------------
/COPYRIGHT:
--------------------------------------------------------------------------------
 1 | Copyright (C) 2016 Seyed Abbas Sadat, Autonomy Lab, Simon Fraser University
 2 | You can contact the author at <asadat at autonomylab dot org>
 3 | 
 4 | Licensed under the Apache License, Version 2.0 (the "License");
 5 | you may not use this file except in compliance with the License.
 6 | You may obtain a copy of the License at
 7 | 
 8 |     http://www.apache.org/licenses/LICENSE-2.0
 9 | 
10 | Unless required by applicable law or agreed to in writing, software
11 | distributed under the License is distributed on an "AS IS" BASIS,
12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 | See the License for the specific language governing permissions and
14 | limitations under the License.
15 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | sparse_reconstruction
 2 | ==========================
 3 | 
 4 | ## Descritopn
 5 | A library to produce a consistent mesh from the generated point cloud of a key-frame based Visual SLAM (or Structure-From-Motion).
 6 | (example reconstruction: https://github.com/asadat/sparse_reconstruction/wiki )
 7 | 
 8 | ## dependencies
 9 | Required libraries:
10 | 	- CGAL (www.cgal.org)
11 | 	- opengl
12 | 	- gmp (www.gmplib.org)
13 | 	- boost
14 | 	- maxflow solver (run install_maxflow_lib.sh to install this library)
15 | 
16 | 
17 | Comment/uncomment the following line in CMakeLists.txt to disable/enable the methods for mesh visualization:
18 | set(VISUALIZATION ON)
19 | 
20 | 
21 | ## Method
22 | We use a simplified version of the approach introduced in the following paper (only the visibility constraints are used in the optimization):
23 | 
24 | Vu, Hoang-Hiep, et al. "High accuracy and visibility-consistent dense multiview stereo." IEEE transactions on pattern analysis and machine intelligence 34.5 (2012): 889-901.
25 | 
26 | ## License
27 | The source code is released under the Apache 2.0 license
28 | 


--------------------------------------------------------------------------------
/examples/lab-recons.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/asadat/sparse_reconstruction/6b19cb67f83f0a64776f8b392fe5f9b37085aebf/examples/lab-recons.png


--------------------------------------------------------------------------------
/examples/lab.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/asadat/sparse_reconstruction/6b19cb67f83f0a64776f8b392fe5f9b37085aebf/examples/lab.png


--------------------------------------------------------------------------------
/examples/srecons1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/asadat/sparse_reconstruction/6b19cb67f83f0a64776f8b392fe5f9b37085aebf/examples/srecons1.png


--------------------------------------------------------------------------------
/examples/srecons2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/asadat/sparse_reconstruction/6b19cb67f83f0a64776f8b392fe5f9b37085aebf/examples/srecons2.png


--------------------------------------------------------------------------------
/include/SparseReconstruction.h:
--------------------------------------------------------------------------------
  1 | 
  2 | //-*- SparseReconstruction Class -*-
  3 | // Copyright 2016 Seyed Abbas Sadat (asadat@autonomylab.org)
  4 | 
  5 | #ifndef __SPARSE_RECONSTRUCTION_H
  6 | #define __SPARSE_RECONSTRUCTION_H
  7 | 
  8 | #include <TooN/TooN.h>
  9 | #include <TooN/se3.h>
 10 | #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
 11 | #include <CGAL/Delaunay_triangulation_3.h>
 12 | #include <CGAL/Triangulation_vertex_base_with_info_3.h>
 13 | #include <CGAL/Triangulation_cell_base_with_info_3.h>
 14 | 
 15 | #include <boost/graph/graph_traits.hpp>
 16 | #include <boost/graph/adjacency_matrix.hpp>
 17 | #include <boost/graph/metric_tsp_approx.hpp>
 18 | 
 19 | class Map;
 20 | class MapPoint;
 21 | 
 22 | template<typename A,typename B,typename C> class Graph;
 23 | 
 24 | //this is the graph type used by the maxflow library
 25 | typedef Graph<double,double,double> GraphType;
 26 | 
 27 | using namespace TooN;
 28 | 
 29 | class SparseReconstruction
 30 | {
 31 | private:
 32 | 
 33 |     /*
 34 |      * stores a triangle and properties that indicate confidence
 35 |      */
 36 |     struct Triangle3D;
 37 | 
 38 |     public:
 39 | 
 40 |         class MapPoint_KeyFrames
 41 |         {
 42 |         public:
 43 |             // the 3D position of the map point
 44 |             Vector<3> p;
 45 | 
 46 |             // the 3D position of the viewpoints that
 47 |             // the map point was observed from
 48 |             std::vector<Vector<3> > keyframes_p;
 49 |         };
 50 | 
 51 |         class CellInfo{
 52 |         public:
 53 |             int id;
 54 |             static const int NO_ID = -1;
 55 |             std::set<int> collisionData;// the neighbor cells with which
 56 |                                            // this cell makes a surface boundary
 57 |             std::vector< Triangle3D* > surfaceTris; //the surface triangles that are on a face of this cell.
 58 |             CellInfo();
 59 | 
 60 |             //returns the surface triangle shared by this cell and the cell with the supplied ID.
 61 |             //A NULL pointer is returned if the two cells don't share a surface triangle.
 62 |             Triangle3D* getCommonTriangle3D( int cell_id );
 63 |         };
 64 | 
 65 | 
 66 |         unsigned int nextCellId; //a global counter for assigning cell IDs
 67 | 
 68 |         class VertexInfo{
 69 |         private:
 70 |             static unsigned long nextVertexId; //a global counter for assigning vertex IDs
 71 |         public:
 72 |             long id;
 73 |             static const int NO_ID = -1;
 74 |             std::vector< Vector<3> > camera_locations; //the camera positions that saw this vertex
 75 |             std::vector< Triangle3D* > surfaceTris; //the surface triangles that have this vertex as a vertex
 76 |             VertexInfo();
 77 |         };
 78 | 
 79 | 
 80 |         typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
 81 |         typedef CGAL::Triangulation_cell_base_with_info_3<CellInfo,K>       InfoCell;
 82 |         typedef CGAL::Triangulation_vertex_base_with_info_3<VertexInfo, K>  InfoVert;
 83 | 
 84 |         typedef CGAL::Triangulation_data_structure_3<InfoVert, InfoCell>         Tds;
 85 |         typedef CGAL::Delaunay_triangulation_3<K, Tds>                      Delaunay;
 86 | 
 87 |         typedef Delaunay::Point   Point;
 88 | 
 89 | 
 90 |         ~SparseReconstruction();
 91 |         static SparseReconstruction * GetInstance()
 92 |         {
 93 |             if(instance == NULL)
 94 |             {
 95 |                 instance = new SparseReconstruction();
 96 |             }
 97 | 
 98 |             return instance;
 99 |         }
100 | 
101 | 
102 | #ifdef GL_VISUALIZATION
103 |         void glDraw();
104 | #endif
105 | 
106 |         void Triangulate(std::vector<MapPoint_KeyFrames*> &mp_kf);
107 | 
108 |         void Reset();
109 | 
110 |         //bool saveMesh(const std::string& fileName);
111 | 
112 |         //bool savePoints(const std::string& file);
113 | 
114 |         //set the maximum distance a triangle can be from the collision triangle and still be considered a part of the surface estimation
115 |         static const double SURFACE_EST_MAX_DIST = 1.0;
116 | 
117 |     private:
118 | 
119 |         struct Triangle3D{
120 | 
121 |             int id;
122 |             Delaunay::Cell_handle c1,c2;
123 |             int KeyframeTex;
124 |             Vector<2> textureCord[3];
125 |             Matrix<3,3> tri;
126 |             std::vector<Delaunay::Vertex_handle> vertices;
127 |             Delaunay::Vertex_handle outside_vh; //vertex not on this triangle, but on the cell in the OUTSIDE direction
128 | 
129 |             bool unexplored;
130 |             Vector<3> unit_normal_towards_outside;
131 |         };
132 | 
133 |         SparseReconstruction();
134 |         static SparseReconstruction* instance;
135 | 
136 |         void GenerateGraph();
137 | 
138 | #ifdef GL_VISUALIZATION
139 |         void DrawCell(Delaunay::Cell_handle ch);
140 | #endif
141 | 
142 |         Delaunay::Cell_handle GetCameraCell(const Vector<3> start, const Vector<3> end, Vector<3> & cpoint);
143 | 
144 |         Delaunay::Cell_handle FindCollisionFromVertex(const Vector<3> start, const Vector<3> end, const Delaunay::Cell_handle & cell,
145 |                                                       Vector<3> & collisionPoint, Delaunay::Cell_handle & lastCell);
146 | 
147 |         Delaunay::Cell_handle NextCell(const Vector<3> start, const Vector<3> end, const Delaunay::Cell_handle & curCell,
148 |                                        Vector<3> & cpoint1, Vector<3> & cpoint2);
149 | 
150 |         bool ExistsVertex(const Delaunay::Cell_handle &ch, const Vector<3> v);
151 |         void copy(Vector<3> &v, SparseReconstruction::Delaunay::Point& p);
152 |         int AssignIdToCell(const Delaunay::Cell_handle& cell);
153 |         void IncreaseCost(const Delaunay::Cell_handle &cell1, const Delaunay::Cell_handle &cell2, double resetVal=-1);
154 |         void IncreaseCost(int cell1, int cell2, double resetVal=-1);
155 |         void AddSourceLink(int cellId);
156 |         void AddSinkLink(int cellId);
157 |         int GetGraphNodesCount() const;
158 |         bool GetCommonTriangle(const Delaunay::Cell_handle &cell1, const Delaunay::Cell_handle &cell2,
159 |                                Matrix<3,3> &triangle,bool noCameraNeighbor,
160 |                                std::vector<Delaunay::Vertex_handle> &vertices, Delaunay::Vertex_handle &outside_vh);
161 | 
162 |         double MinDistToVertex(Delaunay & t, Point p);
163 | 
164 |         TooN::Vector<3> sf;
165 | 
166 |         Delaunay delaunayMesh;
167 | 
168 |         Vector<3> rstart;
169 |         Vector<3> rend;
170 |         std::vector<Delaunay::Cell_handle> collidedCells;
171 | 
172 |         double maxCost;
173 |         std::map<std::pair<int,int>, double> edgeCost;
174 |         std::map<int,Delaunay::Cell_handle> id2cell;
175 |         std::map<int, std::pair<double, double> > sourceSinkCost;
176 |         std::vector<Delaunay::Vertex_handle> cameraVertices;
177 |         //Surface Triangle
178 |         std::vector< Triangle3D* > surfaceTriangles;
179 | 
180 |         //min cut result
181 |         std::vector< std::pair<Delaunay::Cell_handle,Delaunay::Cell_handle> > neighbor_cells;
182 |         std::vector< Delaunay::Cell_handle > sourceNeighbors;
183 |         std::vector< Delaunay::Cell_handle > sinkNeighbors;
184 | 
185 |         //graph functions
186 |         void get_links_in_min_cut(int num_nodes, std::map< std::pair<int,int>, double > linksToCosts,
187 |                                                    std::map<int, std::pair<double,double> > terminal_costs,
188 |                                                    std::vector< std::pair<int,int> > &neighbor_links,
189 |                                                    std::vector< int > &source_links_in_min_cut,
190 |                                                    std::vector< int > &sink_links_in_min_cut);
191 |         int add_node_if_necessary(int n, std::map<int, int> &node_id_map, GraphType *g);
192 | 
193 |         typedef boost::adjacency_matrix<boost::undirectedS, boost::no_property,
194 |                 boost::property <boost::edge_weight_t, double,
195 |                 boost::property<boost::edge_index_t, int> > > Boost_Graph;
196 | 
197 | 
198 | 
199 |         Vector<3> cgal2ToonVect( K::Vector_3 vect);
200 | 
201 |         inline Vector<3> cgalVect2ToonVect( K::Vector_3 vect){
202 |             return makeVector( vect[0], vect[1], vect[2]);
203 |         }
204 | 
205 |         inline Delaunay::Point toonVect2cgalPoint( Vector<3> vect){
206 |             return Delaunay::Point( vect[0], vect[1], vect[2]);
207 |         }
208 | 
209 |         inline Vector<3> cgalPoint2ToonVect( K::Point_3 p){
210 |             return makeVector( p[0], p[1], p[2]);
211 |         }
212 |         inline K::Triangle_3 toonMat2CgalTri( Matrix<3,3> tri_mat){
213 |             K::Point_3 p1 = K::Point_3(tri_mat[0][0], tri_mat[0][1], tri_mat[0][2]);
214 |             K::Point_3 p2 = K::Point_3(tri_mat[1][0], tri_mat[1][1], tri_mat[1][2]);
215 |             K::Point_3 p3 = K::Point_3(tri_mat[2][0], tri_mat[2][1], tri_mat[2][2]);
216 |             return K::Triangle_3(p1,p2,p3);
217 |         }
218 |         inline Vector<3> get_triangle_centroid( Matrix<3,3> tri_mat ){
219 |             return makeVector( (tri_mat[0][0]+tri_mat[1][0]+tri_mat[2][0])/3,
220 |                                (tri_mat[0][1]+tri_mat[1][1]+tri_mat[2][1])/3,
221 |                                (tri_mat[0][2]+tri_mat[1][2]+tri_mat[2][2])/3);
222 |         }
223 |         inline double euclidean_distance_squared(Vector<3> first, Vector<3> second){
224 |             Vector<3> diff = first - second;
225 |             return diff*diff;
226 |         }
227 | 
228 |         //for getting shortest path through triangles
229 |         template<typename VertexListGraph, typename Vertex>
230 |         std::map<Vertex, Triangle3D> map_vertices_to_triangles(std::vector<Triangle3D> tris, VertexListGraph &g);
231 | 
232 |         //adds edges between all vetrices having weights equal to the eclidean distance between the centers
233 |         //of the triangles represented by the vertices.
234 |         template<typename WeightMap, typename Vertex, typename VertexListGraph>
235 |         void create_connected_graph(VertexListGraph &g, WeightMap wmap, std::map<Vertex,Triangle3D> vmap);
236 | 
237 | 
238 |         Vector<3> get_norm_toward_outside(Triangle3D* tri);
239 | 
240 |         int intersect3D_RayTriangle( Vector<3> r0, Vector<3> r1 , Vector<3> v0, Vector<3> v1, Vector<3> v2, Vector<3> &I);
241 | };
242 | 
243 | 
244 | 
245 | #endif
246 | 


--------------------------------------------------------------------------------
/install_maxflow_lib.sh:
--------------------------------------------------------------------------------
1 | if [ ! -d "maxflow" ]; then
2 | 	wget http://vision.csd.uwo.ca/code/maxflow-v3.01.zip
3 | 	mkdir maxflow 
4 | 	unzip maxflow-v3.01.zip -d maxflow/.
5 | 	#cp maxflow_cmake.txt maxflow/CMakeLists.txt
6 | fi
7 | 


--------------------------------------------------------------------------------
/maxflow_cmake.txt:
--------------------------------------------------------------------------------
1 | add_library (maxflow block.h graph.h instances.inc graph.cpp maxflow.cpp)
2 | target_include_directories (maxflow PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
3 | 
4 | 


--------------------------------------------------------------------------------
/src/SparseReconstruction.cpp:
--------------------------------------------------------------------------------
   1 | 
   2 | #include "SparseReconstruction.h"
   3 | 
   4 | #include <CGAL/Triangle_3.h>
   5 | #include <CGAL/Vector_3.h>
   6 | #include <CGAL/Point_2.h>
   7 | #include <CGAL/Line_3.h>
   8 | #include <CGAL/linear_least_squares_fitting_3.h>
   9 | #include <CGAL/ch_melkman.h>
  10 | #include <CGAL/ch_graham_andrew.h>
  11 | #include <CGAL/Dimension.h>
  12 | 
  13 | #include "graph.h"
  14 | 
  15 | #ifdef GL_VISUALIZATION
  16 | #include "GL/glut.h"
  17 | #endif
  18 | 
  19 | #define SOURCE_COST 10000
  20 | #define SINK_COST   0.1
  21 | #define SMALLVALUE  0.00001
  22 | #define SMALL_NUM   0.00000001
  23 | #define VIS_SCORE_RAYS  1
  24 | 
  25 | 
  26 | SparseReconstruction * SparseReconstruction::instance = NULL;
  27 | 
  28 | SparseReconstruction::SparseReconstruction()
  29 | {
  30 | 
  31 | }
  32 | 
  33 | 
  34 | SparseReconstruction::~SparseReconstruction()
  35 | {    
  36 | }
  37 | 
  38 | void SparseReconstruction::Reset()
  39 | {
  40 |     delaunayMesh.clear();
  41 |     cameraVertices.clear();
  42 |     id2cell.clear();
  43 | 
  44 |     neighbor_cells.clear();
  45 |     sourceNeighbors.clear();
  46 |     sinkNeighbors.clear();
  47 | 
  48 |     //collisionData.clear();
  49 |     for(unsigned int i=0; i<surfaceTriangles.size(); i++)
  50 |         delete surfaceTriangles[i];
  51 | 
  52 |     surfaceTriangles.clear();
  53 | 
  54 | }
  55 | 
  56 | void SparseReconstruction::Triangulate(std::vector<MapPoint_KeyFrames*> &mp_kf)
  57 | {
  58 |     delaunayMesh.clear();
  59 |     cameraVertices.clear();
  60 |     nextCellId = 0;
  61 | 
  62 | //    for(unsigned int j=0; j< map->vpKeyFrames.size();j++)
  63 | //    {
  64 | //        Vector<3> v = map->vpKeyFrames[j]->se3CfromW.inverse().get_translation();
  65 | //        Point p(v[0],v[1],v[2]);
  66 | //        cameraVertices.push_back(t.insert(p));
  67 | //    }
  68 | 
  69 |     //copy the map to the delaunay mesh
  70 |         // This becomes an "unsafe" copy in the sense that some points may not be correct due to
  71 |         // a bundle adjustment moving the points during the copy.  This will cause the mesh to
  72 |         // be slightly wrong for this triangulation.  However, bundle adjustments dont move points
  73 |         // very far, and so the error in the triangulation should be minimal.  The mesh will still
  74 |         // be correct in the sense that no part will disappear, but may have graphical bugs from
  75 |         // some points being moved while reading and not others.
  76 |         // to make it safe,
  77 |     // for each point in the map
  78 | 
  79 | 
  80 |     for(size_t i=0; i<mp_kf.size(); i++)
  81 |     {
  82 |         // get the position
  83 |         Vector<3> v = mp_kf[i]->p;
  84 |         // make a delanay point
  85 |         Point p(v[0],v[1],v[2]);
  86 | 
  87 |         //ROS_INFO("here1");
  88 |         double distSq = MinDistToVertex(delaunayMesh, p);
  89 |         if(distSq < 0.001)
  90 |             continue;
  91 | 
  92 |         //ROS_INFO("here2");
  93 | 
  94 |         // add it to the mesh and get the handle
  95 |         Delaunay::Vertex_handle vh = delaunayMesh.insert(p);
  96 | 
  97 |         std::copy(mp_kf[i]->keyframes_p.begin(), mp_kf[i]->keyframes_p.end(), std::back_inserter(vh->info().camera_locations));
  98 | 
  99 |     }
 100 | 
 101 | 
 102 | 
 103 |     GenerateGraph();
 104 | 
 105 |     id2cell.clear();
 106 | 
 107 |     Delaunay::Cell_iterator itci;
 108 |     for(itci = delaunayMesh.all_cells_begin(); itci != delaunayMesh.all_cells_end(); ++itci)
 109 |     {
 110 |         if ( itci->info().id != CellInfo::NO_ID ){
 111 |             id2cell[itci->info().id] = itci;
 112 |         }
 113 |     }
 114 | 
 115 |     neighbor_cells.clear();
 116 |     sourceNeighbors.clear();
 117 |     sinkNeighbors.clear();
 118 | 
 119 |     std::vector< std::pair<int,int> > neighbor_cells_i;
 120 |     std::vector< int > sourceNeighbors_i;
 121 |     std::vector< int > sinkNeighbors_i;
 122 | 
 123 |     this->get_links_in_min_cut(GetGraphNodesCount(),edgeCost,sourceSinkCost,neighbor_cells_i,sourceNeighbors_i,sinkNeighbors_i);
 124 | 
 125 |     for(unsigned int i=0; i<sinkNeighbors_i.size();i++)
 126 |     {
 127 |         sinkNeighbors.push_back(id2cell[sinkNeighbors_i[i]]);
 128 |     }
 129 | 
 130 |     for(unsigned int i=0; i<neighbor_cells_i.size(); i++)
 131 |     {
 132 |         std::pair<Delaunay::Cell_handle,Delaunay::Cell_handle> edge;
 133 | 
 134 |         edge.first  = id2cell[neighbor_cells_i[i].first];
 135 |         edge.second = id2cell[neighbor_cells_i[i].second];
 136 |         neighbor_cells.push_back(edge);
 137 | 
 138 |     }
 139 | 
 140 | 
 141 |     //collisionData.clear();
 142 |     for(unsigned int i=0; i<surfaceTriangles.size(); i++)
 143 |         delete surfaceTriangles[i];
 144 |     surfaceTriangles.clear();
 145 |     int tri_id = 0;
 146 |     for(unsigned int i=0; i < neighbor_cells.size(); i++)
 147 |     {
 148 |         Matrix<3,3> tr;
 149 |         Triangle3D *tri = new Triangle3D();
 150 | 
 151 |         if(GetCommonTriangle(neighbor_cells[i].first, neighbor_cells[i].second, tr, true, tri->vertices, tri->outside_vh))
 152 |         {
 153 |             //Add Collision data
 154 |             neighbor_cells[i].first->info().collisionData.insert(neighbor_cells[i].second->info().id);
 155 | 
 156 | 
 157 |             tri->id = tri_id;
 158 |             tri->tri = tr;
 159 |             tri->KeyframeTex = -1;//VisibilityServer::GetInstance()->GetBestKeyFrame(tr[0], tr[1], tr[2]);
 160 | 
 161 |             if(tri->KeyframeTex >= 0)
 162 |             {
 163 |                 //tri->textureCord[0] = VisibilityServer::GetInstance()->GetImageCoord(tr[0], tri->KeyframeTex);
 164 |                 //tri->textureCord[1] = VisibilityServer::GetInstance()->GetImageCoord(tr[1], tri->KeyframeTex);
 165 |                 //tri->textureCord[2] = VisibilityServer::GetInstance()->GetImageCoord(tr[2], tri->KeyframeTex);
 166 |             }
 167 |             else
 168 |             {
 169 |                 tri->textureCord[0] = makeVector(0,0);
 170 |                 tri->textureCord[1] = makeVector(0,0);
 171 |                 tri->textureCord[2] = makeVector(0,0);
 172 |             }
 173 | 
 174 |             tri->c1 = neighbor_cells[i].first;
 175 |             tri->c2 = neighbor_cells[i].second;
 176 | 
 177 |             tri->c1->info().surfaceTris.push_back(tri);
 178 |             tri->c2->info().surfaceTris.push_back(tri);
 179 | 
 180 |             //associate this triangle3d with each of its vertices
 181 |             for (std::vector<Delaunay::Vertex_handle>::iterator vit = tri->vertices.begin(); vit != tri->vertices.end(); vit++ )
 182 |             {
 183 |                 (*vit)->info().surfaceTris.push_back( tri );
 184 |             }
 185 | 
 186 |             tri->unit_normal_towards_outside = get_norm_toward_outside( tri );
 187 |             TooN::normalize( tri->unit_normal_towards_outside );
 188 | 
 189 |             tri->unexplored = (tri->vertices[0]->info().camera_locations.size() <= 0) || (tri->vertices[1]->info().camera_locations.size() <= 0)
 190 |                     || (tri->vertices[2]->info().camera_locations.size() <= 0);
 191 | 
 192 |             assert( tri->outside_vh!=NULL ); //set by getCommonTriangle
 193 |             surfaceTriangles.push_back(tri);
 194 | 
 195 |             tri_id++;
 196 |         }
 197 |     }
 198 | }
 199 | 
 200 | void SparseReconstruction::copy(Vector<3> &v, Delaunay::Point &p)
 201 | {
 202 |     v[0] = p.x();
 203 |     v[1] = p.y();
 204 |     v[2] = p.z();
 205 | }
 206 | 
 207 | int SparseReconstruction::intersect3D_RayTriangle( Vector<3> r0, Vector<3> r1 , Vector<3> v0, Vector<3> v1, Vector<3> v2, Vector<3> &I)
 208 | {
 209 |     Vector<3>    u, v, n;              // triangle vectors
 210 |     Vector<3>    dir, w0, w;           // ray vectors
 211 |     float     r, a, b;              // params to calc ray-plane intersect
 212 | 
 213 |     // get triangle edge vectors and plane normal
 214 |     u = v1 - v0;
 215 |     v = v2 - v0;
 216 |     n = u ^ v;              // cross product
 217 |     if (n == (TooN::makeVector(0,0,0)))             // triangle is degenerate
 218 |         return -1;                  // do not deal with this case
 219 | 
 220 |     dir = r1 - r0;              // ray direction vector
 221 |     w0 = r0 - v0;
 222 |     a = -(n*w0);
 223 |     b = (n*dir);
 224 |     if (fabs(b) < SMALL_NUM) {     // ray is  parallel to triangle plane
 225 |         if (a == 0)                 // ray lies in triangle plane
 226 |             return 2;
 227 |         else return 0;              // ray disjoint from plane
 228 |     }
 229 | 
 230 |     // get intersect point of ray with triangle plane
 231 |     r = a / b;
 232 |     if (r < 0.0)                    // ray goes away from triangle
 233 |         return 0;                   // => no intersect
 234 |     // for a segment, also test if (r > 1.0) => no intersect
 235 | 
 236 |     I = r0 + r * dir;            // intersect point of ray and plane
 237 | 
 238 |     // is I inside T?
 239 |     float    uu, uv, vv, wu, wv, D;
 240 |     uu = (u*u);
 241 |     uv = (u*v);
 242 |     vv = (v*v);
 243 |     w = I - v0;
 244 |     wu = (w*u);
 245 |     wv = (w*v);
 246 |     D = (uv) *  uv - (uu) * vv;
 247 | 
 248 |     // get and test parametric coords
 249 |     float s, t;
 250 |     s = (uv * wv - vv * wu) / D;
 251 |     if (s < 0.0 || s > 1.0)         // I is outside T
 252 |         return 0;
 253 |     t = (uv * wu - uu * wv) / D;
 254 |     if (t < 0.0 || (s + t) > 1.0)  // I is outside T
 255 |         return 0;
 256 | 
 257 |     return 1;                       // I is in T
 258 | }
 259 | 
 260 | Vector<3> SparseReconstruction::get_norm_toward_outside(Triangle3D *tri)
 261 | {
 262 |     //find normal in direction of OUTSIDE
 263 |     Vector<3> v0 = cgalPoint2ToonVect( tri->vertices[0]->point() );
 264 |     Vector<3> v1 = cgalPoint2ToonVect( tri->vertices[1]->point() );
 265 |     Vector<3> v2 = cgalPoint2ToonVect( tri->vertices[2]->point() );
 266 | 
 267 |     Vector<3> triNorm = (v0-v1)^(v0-v2);
 268 | 
 269 |     Vector<3> vOutside = cgalPoint2ToonVect( tri->outside_vh->point() );
 270 |     Vector<3> center =  0.333 * (v0 + v1 + v2);
 271 | 
 272 |     //choose triangle normal that points towards the outside vertex
 273 |     if(triNorm * (vOutside - center) < 0)
 274 |         triNorm = -1 * triNorm;
 275 | 
 276 |     return triNorm;
 277 | }
 278 | 
 279 | bool SparseReconstruction::ExistsVertex(const Delaunay::Cell_handle &ch, const Vector<3> v)
 280 | {
 281 |     Delaunay::Point p(v[0], v[1], v[2]);
 282 |     Delaunay::Vertex_handle vh;
 283 |     if(delaunayMesh.is_vertex(p, vh))
 284 |     {
 285 |         if(ch->has_vertex(vh))
 286 |             return true;
 287 |         else
 288 |             return false;
 289 |     }
 290 |     else
 291 |     {
 292 |         //ROS_INFO("Feature point not found in the mesh vertices.");
 293 |         return false;
 294 |     }
 295 | }
 296 | 
 297 | SparseReconstruction::Delaunay::Cell_handle SparseReconstruction::GetCameraCell(Vector<3> cameraPos, Vector<3> featurePose, Vector<3> & cpoint)
 298 | {
 299 |     Vector<3> near = featurePose-cameraPos;
 300 |     normalize(near);
 301 |     near = cameraPos + SMALLVALUE * near;
 302 |     cpoint = near;
 303 |     Delaunay::Point p(near[0],near[1],near[2]);
 304 |     Delaunay::Cell_handle cell = delaunayMesh.locate(p);
 305 |     return cell;
 306 | }
 307 | 
 308 | SparseReconstruction::Delaunay::Cell_handle SparseReconstruction::NextCell(const Vector<3> start, const Vector<3> end, const Delaunay::Cell_handle &curCell, Vector<3> &cpoint1, Vector<3> &cpoint2)
 309 | {
 310 |     Delaunay::Cell_handle ch;
 311 |     return ch;
 312 | }
 313 | 
 314 | SparseReconstruction::Delaunay::Cell_handle SparseReconstruction::FindCollisionFromVertex(const Vector<3> start, const Vector<3> end, const Delaunay::Cell_handle &cell,
 315 |                                                                                 Vector<3> &cpoint, Delaunay::Cell_handle & lastCell)
 316 | {
 317 | 
 318 |     Delaunay::Vertex_handle vh[4];
 319 |     Vector<3> vp[4];
 320 |     for(int i=0; i<4; i++)
 321 |     {
 322 |         vh[i] = cell->vertex(i);
 323 |         copy(vp[i], vh[i]->point());
 324 |     }
 325 | 
 326 |     int res[] = {100,100,100,100};
 327 |     int checks[] = {0,0,0,0};
 328 |     //int ii=-1, jj=-1, kk=-1;
 329 | 
 330 |     Delaunay::Cell_handle nextCell = NULL;
 331 |     //conds[0] = (blackList.find(vh[0]) == blackList.end()) || (blackList.find(vh[1]) == blackList.end()) || (blackList.find(vh[2]) == blackList.end());
 332 |     ///conds[1] = (blackList.find(vh[0]) == blackList.end()) || (blackList.find(vh[1]) == blackList.end()) || (blackList.find(vh[3]) == blackList.end());
 333 |     ///conds[2] = (blackList.find(vh[0]) == blackList.end()) || (blackList.find(vh[2]) == blackList.end()) || (blackList.find(vh[3]) == blackList.end());
 334 |     ///conds[3] = (blackList.find(vh[1]) == blackList.end()) || (blackList.find(vh[2]) == blackList.end()) || (blackList.find(vh[3]) == blackList.end());
 335 | 
 336 |     bool foundCollision = false;
 337 | 
 338 | 
 339 | 
 340 |     {       
 341 |         res[0] = intersect3D_RayTriangle(start, end, vp[0], vp[1], vp[2], cpoint);
 342 | 
 343 | //        if(res[0] == 1)
 344 | //            collisionPoints.push_back(cpoint);
 345 | 
 346 | //        checks[0] += (cell->neighbor(3) != lastCell)?1:0;
 347 | //        checks[0] += (!ExistsVertex(cell,cpoint))?5:0;
 348 | 
 349 |         if(res[0] == 1 && cell->neighbor(3) != lastCell && !ExistsVertex(cell,cpoint))
 350 |         {
 351 |             nextCell = cell->neighbor(3);
 352 | //            ii=0; jj=1; kk=2;
 353 | //            blackList.clear();
 354 | //            blackList.insert(vh[0]);
 355 | //            blackList.insert(vh[1]);
 356 | //            blackList.insert(vh[2]);
 357 |             foundCollision = true;
 358 |         }
 359 |     }
 360 | 
 361 |     if(!foundCollision)
 362 |     {
 363 |         res[1] = intersect3D_RayTriangle(start, end, vp[0], vp[1], vp[3], cpoint);
 364 | 
 365 | //        if(res[1] == 1)
 366 | //            collisionPoints.push_back(cpoint);
 367 | 
 368 | //        checks[1] += (cell->neighbor(2) != lastCell)?1:0;
 369 | //        checks[1] += (!ExistsVertex(cell,cpoint))?5:0;
 370 | 
 371 |         if(res[1]==1 && cell->neighbor(2) != lastCell && !ExistsVertex(cell,cpoint))
 372 |         {
 373 |             nextCell = cell->neighbor(2);
 374 | //            ii=0; jj=1; kk=3;
 375 | //            blackList.clear();
 376 | //            blackList.insert(vh[0]);
 377 | //            blackList.insert(vh[1]);
 378 | //            blackList.insert(vh[3]);
 379 |             foundCollision = true;
 380 |         }
 381 | 
 382 |     }
 383 | 
 384 |     if(!foundCollision)
 385 |     {
 386 |         res[2] = intersect3D_RayTriangle(start, end, vp[0], vp[2], vp[3], cpoint);
 387 | 
 388 | //        if(res[2] == 1)
 389 | //            collisionPoints.push_back(cpoint);
 390 | 
 391 | //        checks[2] += (cell->neighbor(1) != lastCell)?1:0;
 392 | //        checks[2] += (!ExistsVertex(cell,cpoint))?5:0;
 393 | 
 394 |         if(res[2] == 1 && cell->neighbor(1) != lastCell && !ExistsVertex(cell,cpoint))
 395 |         {
 396 |             nextCell = cell->neighbor(1);
 397 | //            ii=0; jj=2; kk=3;
 398 | //            blackList.clear();
 399 | //            blackList.insert(vh[0]);
 400 | //            blackList.insert(vh[2]);
 401 | //            blackList.insert(vh[3]);
 402 |             foundCollision = true;
 403 |         }
 404 |     }
 405 | 
 406 |     if(!foundCollision)
 407 |     {
 408 |         res[3] = intersect3D_RayTriangle(start, end, vp[1], vp[2], vp[3], cpoint);
 409 | 
 410 | //        if(res[3] == 1)
 411 | //            collisionPoints.push_back(cpoint);
 412 | 
 413 | //        checks[3] += (cell->neighbor(0) != lastCell)?1:0;
 414 | //        checks[3] += (!ExistsVertex(cell,cpoint))?5:0;
 415 | 
 416 |         if(res[3] == 1 && cell->neighbor(0) != lastCell && !ExistsVertex(cell,cpoint))
 417 |         {
 418 |             nextCell = cell->neighbor(0);
 419 | //            ii=1; jj=2; kk=3;
 420 | //            blackList.clear();
 421 | //            blackList.insert(vh[1]);
 422 | //            blackList.insert(vh[2]);
 423 | //            blackList.insert(vh[3]);
 424 |             foundCollision = true;
 425 |         }
 426 |     }
 427 | 
 428 | 
 429 | 
 430 | /*
 431 |     if(ii>=0 && jj >=0 && kk>= 0)
 432 |     {
 433 |         for(int j=0; j<4; j++)
 434 |         {
 435 |             Delaunay::Cell_handle chn = cell->neighbor(j);
 436 |             if(chn->has_vertex(vh[ii]) && chn->has_vertex(vh[jj]) && chn->has_vertex(vh[kk]))
 437 |             {
 438 |                 //if(!t.is_infinite(chn))
 439 |                 {
 440 |                     nextCell = chn;
 441 | 
 442 |                 }
 443 | //                else
 444 | //                {
 445 | //                    ROS_INFO("hitting infinite cell!!!!");
 446 | //                }
 447 |             }
 448 |         }
 449 |     }
 450 |     else
 451 |     {
 452 |         ROS_INFO("No Collision with the sides of he cell!!!! res: %d\t%d\t%d\t%d", res[0], res[1], res[2], res[3]);
 453 |     }
 454 | */
 455 |     if(nextCell == NULL)
 456 |     {
 457 | 
 458 |         collidedCells.clear();
 459 |         collidedCells.push_back(lastCell);
 460 |         collidedCells.push_back(cell);
 461 |         rstart = start;
 462 |         rend = end;        
 463 |     }
 464 | 
 465 |     return nextCell;
 466 | }
 467 | 
 468 | int SparseReconstruction::AssignIdToCell(const Delaunay::Cell_handle &cell)
 469 | {
 470 |     if (cell->info().id == CellInfo::NO_ID)
 471 |     {
 472 |         cell->info().id  = nextCellId++;
 473 |     }
 474 |     return cell->info().id;
 475 | 
 476 | //    std::map<Delaunay::Cell_handle,int>::iterator it;
 477 | //    int s = cell2id.size();
 478 | //    //int curId
 479 | //    it = cell2id.find(cell);
 480 | //    if( it != cell2id.end())
 481 | //    {
 482 | //        return it->second;
 483 | //    }
 484 | //    else
 485 | //    {
 486 | //        cell2id[cell] = s;
 487 | //        return s;
 488 | //    }
 489 | 
 490 |   /*  bool flag = true;
 491 |     for(int i=0; i<collidedCells.size(); i++)
 492 |     {
 493 |         if(collidedCells[i] == cell)
 494 |         {
 495 |             flag = false;
 496 |             break;
 497 |         }
 498 |     }
 499 | 
 500 |     if(flag)
 501 |         collidedCells.push_back(cell);
 502 |         */
 503 | }
 504 | 
 505 | void SparseReconstruction::IncreaseCost(const Delaunay::Cell_handle &cell1, const Delaunay::Cell_handle &cell2, double resetVal)
 506 | {
 507 |     int cell1id = AssignIdToCell(cell1);
 508 |     int cell2id = AssignIdToCell(cell2);
 509 | 
 510 |     IncreaseCost(cell1id, cell2id, resetVal);
 511 | }
 512 | 
 513 | void SparseReconstruction::IncreaseCost(int cell1, int cell2, double resetVal)
 514 | {
 515 |     std::pair<int,int> edge;
 516 |     edge.first = cell1;
 517 |     edge.second = cell2;
 518 | 
 519 |     std::map<std::pair<int,int>, double>::iterator it;
 520 |     it = edgeCost.find(edge);
 521 |     double cost;
 522 | 
 523 | //    if(it == edgeCost.end())
 524 | //    {
 525 | //        edge.first = cell2;
 526 | //        edge.second = cell1;
 527 | //        it = edgeCost.find(edge);
 528 | //    }
 529 | 
 530 |     if(it != edgeCost.end())
 531 |     {
 532 |         if(resetVal>=0)
 533 |             it->second = resetVal;
 534 |         else
 535 |             it->second += 0.1;
 536 | 
 537 |         cost = it->second;
 538 |     }
 539 |     else
 540 |     {
 541 |         if(resetVal>=0)
 542 |             it->second = resetVal;
 543 |         else
 544 |             edgeCost[edge] = 0.1;
 545 | 
 546 |         cost = edgeCost[edge];
 547 |     }
 548 | 
 549 |     if(cost > maxCost && cost < SINK_COST)
 550 |         maxCost = cost;
 551 | }
 552 | 
 553 | void SparseReconstruction::AddSourceLink(int cellId)
 554 | {
 555 |     std::map<int, std::pair<double,double> >::iterator it;
 556 |     it = sourceSinkCost.find(cellId);
 557 |     if(it != sourceSinkCost.end())
 558 |     {
 559 |         it->second.first = SOURCE_COST;
 560 |     }
 561 |     else
 562 |     {
 563 |         std::pair<double,double> cost;
 564 |         cost.first = SOURCE_COST;
 565 |         cost.second = 0;
 566 |         //it->second = cost;
 567 |         sourceSinkCost[cellId] = cost;
 568 |     }
 569 | 
 570 | }
 571 | 
 572 | int SparseReconstruction::GetGraphNodesCount() const
 573 | {
 574 |     return nextCellId;
 575 | }
 576 | 
 577 | void SparseReconstruction::AddSinkLink(int cellId)
 578 | {
 579 |     std::map<int, std::pair<double,double> >::iterator it;
 580 |     it = sourceSinkCost.find(cellId);
 581 |     if(it != sourceSinkCost.end())
 582 |     {
 583 |         it->second.second += SINK_COST;
 584 |     }
 585 |     else
 586 |     {
 587 |         std::pair<double,double> cost;
 588 |         cost.second = SINK_COST;
 589 |         cost.first = 0;
 590 |         sourceSinkCost[cellId] = cost;
 591 |     }
 592 | }
 593 | 
 594 | void SparseReconstruction::GenerateGraph()
 595 | {
 596 |     maxCost = 0;
 597 |     edgeCost.clear();
 598 |     //cell2id.clear();
 599 |     nextCellId = 0;
 600 | 
 601 |     sourceSinkCost.clear();
 602 |     collidedCells.clear();
 603 | 
 604 |     //for(unsigned int i=0; i< map->vpKeyFrames.size(); i++)
 605 |     // for each point in the map
 606 |     Delaunay::Finite_vertices_iterator vhIT;
 607 |     for(vhIT = delaunayMesh.finite_vertices_begin(); vhIT != delaunayMesh.finite_vertices_end(); ++vhIT)
 608 |     {
 609 |         // this point has enough keyframes already
 610 |         Delaunay::Vertex_handle featureHandle = vhIT;
 611 |         Vector<3> featurePos;
 612 |         // is there a better way to do this?
 613 |         featurePos[0] = featureHandle->point()[0];
 614 |         featurePos[1] = featureHandle->point()[1];
 615 |         featurePos[2] = featureHandle->point()[2];
 616 | 
 617 |         // for each keyframe of the point
 618 |         std::vector<Vector<3> >::iterator camIT;
 619 |         for(camIT = featureHandle->info().camera_locations.begin(); camIT != featureHandle->info().camera_locations.end(); ++camIT)
 620 |         {
 621 | 
 622 |             // get the position of the camera
 623 |             Vector<3> CameraPos  = *camIT;
 624 | 
 625 |             // get the cell the camera is in
 626 |             Delaunay::Cell_handle cameraCell;
 627 |             Delaunay::Point dummy1(CameraPos[0], CameraPos[1], CameraPos[2]);
 628 |             cameraCell = delaunayMesh.locate(dummy1);
 629 | 
 630 | 
 631 |             // The cell behind the feature
 632 |             Vector<3> c2fNorm = featurePos - CameraPos;
 633 |             normalize(c2fNorm);
 634 |             Vector<3> near = featurePos + SMALLVALUE * c2fNorm;
 635 | 
 636 | /*            // get the cell behind the feature
 637 |             Vector<3> near = featurePos - CameraPos;
 638 |             normalize(near);
 639 |             near = featurePos + SMALLVALUE * near;
 640 | */
 641 |             Delaunay::Point dummy2(near[0], near[1],near[2]);
 642 |             Delaunay::Cell_handle backCell = delaunayMesh.locate(dummy2);
 643 |             if(delaunayMesh.is_infinite(backCell))
 644 |             {
 645 | 
 646 | //                std::vector<Delaunay::Cell_handle> cells;
 647 | //                delaunayMesh.incident_cells(featureHandle,std::back_inserter(cells));
 648 | 
 649 | //                std::vector<Delaunay::Cell_handle> infneib;
 650 | 
 651 | //                for(int i=0; i<cells.size(); i++)
 652 | //                {
 653 | //                    if(delaunayMesh.is_infinite(cells[i]))
 654 | //                        infneib.push_back(cells[i]);
 655 | //                }
 656 | 
 657 | //                for(int i=0; i<cells.size(); i++)
 658 | //                {
 659 | //                    if(!delaunayMesh.is_infinite(cells[i]))
 660 | //                    {
 661 | //                        for(int j=0; j<infneib.size();j++)
 662 | //                        {
 663 | //                            if(cells[i]->has_neighbor(infneib[j]))
 664 | //                            {
 665 | //                                AddSinkLink(AssignIdToCell(infneib[j]));
 666 | //                                IncreaseCost(cells[i],infneib[j]);
 667 | //                            }
 668 | //                        }
 669 | //                    }
 670 | //                }
 671 | 
 672 |             }
 673 |             else
 674 |             {
 675 |                 //if(ParamsAccess::varParams->DrawRRTTree)
 676 |                     AddSinkLink(AssignIdToCell(backCell));
 677 |             }
 678 | 
 679 |             Delaunay::Cell_handle curCell;
 680 |             int curId;
 681 | 
 682 |             // Find the cell in front of the feature
 683 |             Vector<3> f2c = -c2fNorm;
 684 | 
 685 |             near = featurePos + SMALLVALUE * f2c;
 686 |             Delaunay::Point dummy3(near[0], near[1],near[2]);
 687 |             curCell = delaunayMesh.locate(dummy3);
 688 | 
 689 |             curId = AssignIdToCell(curCell);
 690 |             Vector<3> cpoint = near;
 691 | 
 692 |             if(delaunayMesh.is_infinite(curCell))
 693 |             {
 694 |                 AddSourceLink(curId);
 695 |                 continue;
 696 |             }
 697 | 
 698 |             Delaunay::Cell_handle lastCell = backCell;
 699 |             int loopn = 0;
 700 |             while(true)
 701 |             {
 702 |                 if(loopn++ > 1000)
 703 |                 {
 704 |                     return;
 705 |                 }
 706 | //                if(std::find(collidedCells.begin(),collidedCells.end(),curCell) == collidedCells.end())
 707 | //                    collidedCells.push_back(curCell);
 708 | //                else
 709 | //                {
 710 | //                    rstart = featurePos;
 711 | //                    rend = CameraPos;
 712 | //                    return;
 713 | //                }
 714 | 
 715 |                 Delaunay::Cell_handle nextCell;
 716 |                 int nextId;
 717 | 
 718 |                 near += SMALLVALUE*f2c;
 719 | 
 720 |                 nextCell = FindCollisionFromVertex( featurePos + SMALLVALUE * f2c, CameraPos, curCell, cpoint, lastCell);
 721 | 
 722 |                 if(nextCell == NULL)
 723 |                     break;
 724 | 
 725 |                 nextId = AssignIdToCell(nextCell);
 726 |                 //ROS_INFO("Graph generation.. Next Id = %d", nextId);
 727 | 
 728 |                 near = cpoint;
 729 | 
 730 |                 IncreaseCost(nextId, curId);
 731 | 
 732 |                 if(nextCell == cameraCell || delaunayMesh.is_infinite(nextCell))
 733 |                 {
 734 |                     //ROS_INFO("-------********");
 735 |                     AddSourceLink(nextId);
 736 |                     break;
 737 |                 }
 738 |                 else
 739 |                 {
 740 |                     //ROS_INFO("------->>>>>");
 741 |                     lastCell = curCell;
 742 |                     curCell = nextCell;
 743 |                     curId   = nextId;
 744 |                 }
 745 | 
 746 |                 //ROS_INFO("3");
 747 |             }
 748 | 
 749 | //            Vector<3> cameraPos = (*it)->se3CfromW.inverse().get_translation();
 750 | 
 751 | //            Vector<3> cp = cameraPos;
 752 | //            sf = fp;
 753 | 
 754 | //            Delaunay::Vertex_handle vh;
 755 | //            Delaunay::Point dummy(fp[0], fp[1], fp[2]);
 756 | //            if(!t.is_vertex(dummy,vh))
 757 | //            {
 758 | //                //ROS_INFO("??");
 759 | //                continue;
 760 | //            }
 761 | 
 762 | //            //start from a vertice (1) camera cell (2) a vertex on the ray to another vertex
 763 | //            Vector<3> cpoint;
 764 | //            Delaunay::Cell_handle curCell = GetCameraCell(cp,fp, cpoint);
 765 | //            if(t.is_infinite(curCell))
 766 | //                continue;
 767 | 
 768 | //            if(isVertex(curCell,fp))
 769 | //            {
 770 | //                //int curId;
 771 | //                //int nextId;
 772 | //                //curId = AssignIdToCell(curCell);
 773 | //                //AddSourceLink(curId);
 774 | //                continue;
 775 | //            }
 776 | 
 777 | //            int curId;
 778 | //            int nextId;
 779 | //            curId = AssignIdToCell(curCell);
 780 | //            AddSourceLink(curId);
 781 | //           // collidedCells.push_back(curCell);
 782 | 
 783 | //            Vector<3> collisionPoint = cp;
 784 | 
 785 | //            while(true)
 786 | //            {
 787 | //                Vector<3> near = fp-collisionPoint;
 788 | //                normalize(near);
 789 | //                near = SMALLVALUE*near + collisionPoint;
 790 | 
 791 | //                Delaunay::Cell_handle nextCell = FindCollisionFromVertex(near,fp,curCell,collisionPoint);
 792 | //                if(nextCell == NULL)
 793 | //                {
 794 | //                    //ROS_INFO("No Next Cell Found.");
 795 | //                    break;
 796 | //                }
 797 | 
 798 | //                nextId = AssignIdToCell(nextCell);
 799 | //                IncreaseCost(curId, nextId);
 800 | 
 801 | //                if(isVertex(nextCell,fp))
 802 | //                {
 803 | //                    Vector<3> near = fp-cameraPos;
 804 | //                    normalize(near);
 805 | //                    near = fp + SMALLVALUE * near;
 806 | //                    cpoint = near;
 807 | //                    Delaunay::Point p(near[0],near[1],near[2]);
 808 | //                    Delaunay::Cell_handle cell = t.locate(p);
 809 | //                    if(!t.is_infinite(cell))
 810 | //                    {
 811 | //                       int lastcellid = AssignIdToCell(cell);
 812 | //                       AddSinkLink(lastcellid);
 813 | //                    }
 814 | //                    else
 815 | //                    {
 816 | //                        AddSinkLink(nextId);
 817 | //                    }
 818 | 
 819 | //                    //ROS_INFO("At the end of the ray.");
 820 | //                    break;
 821 | //                }
 822 | 
 823 | //                curCell = nextCell;
 824 | //                curId = nextId;
 825 | 
 826 | //            }
 827 | 
 828 | 
 829 |         }
 830 | 
 831 |     }
 832 | 
 833 | //    // add all the other const weighted edges
 834 |     Delaunay::Finite_cells_iterator fci;
 835 |     for(fci = delaunayMesh.finite_cells_begin(); fci != delaunayMesh.finite_cells_end(); ++fci)
 836 |     {
 837 |         Delaunay::Cell_handle ch = fci;
 838 |         int cid = AssignIdToCell(ch);
 839 | 
 840 |         Delaunay::Cell_handle cn;
 841 |         for(int i=0; i<4; i++)
 842 |         {
 843 |             cn = ch->neighbor(i);
 844 | 
 845 |             if(delaunayMesh.is_infinite(cn))
 846 |                 continue;
 847 | 
 848 |             int nid = AssignIdToCell(cn);
 849 | 
 850 |             std::pair<int,int> e;
 851 |             e.first = cid;
 852 |             e.second = nid;
 853 |             //Matrix<3,3> t;
 854 |             //Triangle3D tmp;
 855 |             //GetCommonTriangle(ch, cn, t, false, tmp.vertices, tmp.outside_vh);
 856 |             //double area = triangle_area_squared(t);
 857 |             double regFac = 0.2;//PtamParameters::varparams().MeshRegularizer /*ParamsAccess::varParams->MeshRegularizationFac*/;
 858 |             if(edgeCost.find(e) == edgeCost.end())
 859 |             {               
 860 |                 edgeCost[e] = regFac;
 861 |             }
 862 |             else
 863 |             {
 864 |                 edgeCost[e] += regFac;
 865 |             }
 866 | 
 867 |         }
 868 |     }
 869 | 
 870 | 
 871 | }
 872 | 
 873 | double SparseReconstruction::MinDistToVertex(Delaunay & t,  Point p)
 874 | {
 875 |     double minDist = 99999;
 876 | 
 877 |     if(t.number_of_vertices() < 5)
 878 |         return minDist;
 879 | 
 880 |     Delaunay::Vertex_handle v = t.nearest_vertex(p);
 881 |     Vector<3> pt = cgalPoint2ToonVect(p);
 882 |     if(!t.is_infinite(v))
 883 |     {
 884 |         Vector<3> vt = cgalPoint2ToonVect(v->point());
 885 |         minDist = (pt-vt)*(pt-vt);
 886 |     }
 887 | 
 888 |     return minDist;
 889 | 
 890 | }
 891 | 
 892 | bool SparseReconstruction::GetCommonTriangle(const Delaunay::Cell_handle &cell1, const Delaunay::Cell_handle &cell2,
 893 |                                         Matrix<3,3> &triangle, bool noCameraNeighbor,
 894 |                                         std::vector<Delaunay::Vertex_handle> &vertices, Delaunay::Vertex_handle &outside_vh)
 895 | {
 896 |     int n=0;
 897 | 
 898 |     for(int i=0; i<4; i++)
 899 |     {
 900 |         Delaunay::Vertex_handle v = cell1->vertex(i);
 901 |         if(cell2->has_vertex(v) && (!noCameraNeighbor || std::find(cameraVertices.begin(), cameraVertices.end(), v) == cameraVertices.end()))
 902 |         {
 903 |             vertices.push_back( v );
 904 |             Delaunay::Point p = v->point();
 905 |             triangle[n][0] = p.x();
 906 |             triangle[n][1] = p.y();
 907 |             triangle[n][2] = p.z();
 908 |             n++;
 909 |         }
 910 |         else if ( !cell2->has_vertex(v) )
 911 |         {
 912 |             outside_vh = v; //the vertex that belongs ONLY to C1 is towards the outside
 913 |         }
 914 | 
 915 |     }
 916 | 
 917 |     if(n == 3)
 918 |         return true;
 919 |     else
 920 |         return false;
 921 | }
 922 | 
 923 | void SparseReconstruction::get_links_in_min_cut(int num_nodes, std::map< std::pair<int,int>, double > linksToCosts,
 924 |                                            std::map<int, std::pair<double,double> > terminal_costs,
 925 |                                            std::vector< std::pair<int,int> > &neighbor_links,
 926 |                                            std::vector< int > &source_links_in_min_cut,
 927 |                                            std::vector< int > &sink_links_in_min_cut)
 928 | {
 929 | 
 930 |     std::map<int, GraphType::node_id> node_id_map; //maps ids supplied as ints to "node_id"s used by the flow lib
 931 | 
 932 |     printf("Connecting %d nodes with %lu edges between non-terminal nodes.\n", num_nodes, linksToCosts.size());
 933 | 
 934 |     //populate graph
 935 |     GraphType *g = new GraphType( num_nodes, /*estimated # of edges*/ linksToCosts.size());
 936 | 
 937 | #ifdef _VERIFY_FLOW
 938 |     //flow verification can't be done if some edges were deleted from the supplied map
 939 |     //so we'll store them in this second structure
 940 |     std::map< std::pair<int,int>, double > revLinksToCosts;
 941 | #endif
 942 | 
 943 |     //add links between non-terminal nodes.
 944 |     for(std::map< std::pair<int,int>,double >::iterator mit = linksToCosts.begin(); mit != linksToCosts.end(); mit++) {
 945 | 
 946 |         int n1 = mit->first.first;
 947 |         int n2 = mit->first.second;
 948 | 
 949 |         double forward_cost = mit->second;
 950 | 
 951 |         //add both nodes to the graph if necessary
 952 |         GraphType::node_id n1_id = add_node_if_necessary(n1, node_id_map, g);
 953 |         GraphType::node_id n2_id = add_node_if_necessary(n2, node_id_map, g);
 954 | 
 955 |         //see if the link in the opposite direction exists
 956 |         std::pair<int,int> rev_pair(n2,n1);
 957 |         double reverse_cost=0.0;
 958 |         if ( linksToCosts.find(rev_pair) != linksToCosts.end() ){
 959 |             reverse_cost = linksToCosts[ rev_pair ];
 960 | 
 961 |             linksToCosts.erase( rev_pair );//we don't want to consider this link again, so we delete it
 962 | #ifdef _VERIFY_FLOW
 963 |             assert( revLinksToCosts.find(rev_pair)==revLinksToCosts.end() );
 964 |             revLinksToCosts[rev_pair] = reverse_cost; //only used to verify
 965 | #endif
 966 |         }
 967 | 
 968 |         g->add_edge(n1_id, n2_id, forward_cost, reverse_cost);
 969 |     }
 970 | 
 971 |     //specify costs between terminal and non-terminal nodes
 972 | 
 973 |     for(std::map<int, std::pair<double,double> >::iterator terms_it = terminal_costs.begin(); terms_it != terminal_costs.end(); terms_it++){
 974 | 
 975 |         //nodes that have no non-terminal neighbours will have to be added
 976 |         GraphType::node_id id = add_node_if_necessary(terms_it->first, node_id_map, g);
 977 | 
 978 |         double costFromSource = terms_it->second.first;
 979 |         double costToSink = terms_it->second.second;
 980 | 
 981 |         g -> add_tweights( id, costFromSource, costToSink );
 982 |     }
 983 | 
 984 |     //calc flow
 985 |     double flow = g -> maxflow();
 986 | 
 987 |  #ifdef _VERIFY_FLOW
 988 |     double min_cut_capacity=0.0;
 989 |  #endif
 990 | 
 991 | 
 992 |     //we now find which edges have one end in the sink set and another in the source set
 993 |     for(std::map< std::pair<int,int>,double >::iterator mit = linksToCosts.begin(); mit != linksToCosts.end(); mit++)
 994 |     {
 995 |         int n1 = mit->first.first;
 996 |         int n2 = mit->first.second;
 997 | 
 998 |         //get IDs
 999 |         GraphType::node_id n1_id = node_id_map[n1];
1000 |         GraphType::node_id n2_id = node_id_map[n2];
1001 | 
1002 | #ifdef _VERIFY_FLOW
1003 |         std::pair<int,int> rev_pair(n2, n1);
1004 |         if (g->what_segment(n1_id)==GraphType::SINK   && g->what_segment(n2_id)==GraphType::SOURCE){
1005 |             //neighbor_links.push_back( mit->first ); //this edge IS NOT in the min cut!!!! (but the reverse edge is, if it exists)
1006 | 
1007 |             //the min cut considers only edges from the source set to the sink set
1008 |             if ( revLinksToCosts.find( rev_pair ) != revLinksToCosts.end() ){
1009 |                 min_cut_capacity += revLinksToCosts[rev_pair];
1010 |                 neighbor_links.push_back( rev_pair );
1011 |             }
1012 | 
1013 |         } else if (g->what_segment(n1_id)==GraphType::SOURCE && g->what_segment(n2_id)==GraphType::SINK) {
1014 |             neighbor_links.push_back( mit->first );
1015 | 
1016 |             min_cut_capacity += mit->second;
1017 |         }
1018 | #else
1019 |         if (   (g->what_segment(n1_id)==GraphType::SINK   && g->what_segment(n2_id)==GraphType::SOURCE)
1020 |             || (g->what_segment(n1_id)==GraphType::SOURCE && g->what_segment(n2_id)==GraphType::SINK) )
1021 |         {
1022 |             neighbor_links.push_back( mit->first );
1023 |         }
1024 |  #endif
1025 |     }
1026 | 
1027 |     //put nodes that have links to the terminal nodes
1028 |     for(std::map<int, std::pair<double,double> >::iterator t_it = terminal_costs.begin(); t_it != terminal_costs.end(); t_it++)
1029 |     {
1030 |         int n = t_it->first;
1031 |         GraphType::node_id id = node_id_map[n];
1032 | 
1033 |         double costFromSource = t_it->second.first;
1034 |         double costToSink = t_it->second.second;
1035 | 
1036 |         /*          2
1037 |          * (source)---->(id)
1038 |          * If the (id) node is in the sink set, then this link is in the cut
1039 |          */
1040 | 
1041 |         if (g->what_segment(id)==GraphType::SINK && costFromSource>0)
1042 |         {
1043 |             source_links_in_min_cut.push_back(n);
1044 | #ifdef _VERIFY_FLOW
1045 |             min_cut_capacity += costFromSource;
1046 | #endif
1047 |         }
1048 |         else if ( g->what_segment(id)==GraphType::SOURCE && costToSink>0)
1049 |         {
1050 |             sink_links_in_min_cut.push_back(n);
1051 | #ifdef _VERIFY_FLOW
1052 |             min_cut_capacity += costToSink;
1053 | #endif
1054 |         }
1055 |     }
1056 | 
1057 | #ifdef _VERIFY_FLOW
1058 | 
1059 |     printf("There are %lu links with one end in the sink set and the other in the source set, "
1060 |            "\n\t %lu links connected to the source in the min cut, "
1061 |            "\n\t %lu links connected to the sink in the min cut\n", neighbor_links.size(), source_links_in_min_cut.size(), sink_links_in_min_cut.size());
1062 |     printf("Calculated flow to be %f and summed edges in min cut to be %f.\n", flow, min_cut_capacity);
1063 |     assert(fabs(flow-min_cut_capacity)<0.0001);
1064 | #endif
1065 | 
1066 |     delete g;
1067 | }
1068 | 
1069 | int SparseReconstruction::add_node_if_necessary(int n, std::map<int, int> &node_id_map, GraphType *g)
1070 | {
1071 |     GraphType::node_id  id;
1072 |     if ( node_id_map.find(n) != node_id_map.end() )
1073 |     {
1074 |         id = node_id_map[ n ];
1075 |     }
1076 |     else
1077 |     {
1078 |         id = g->add_node();
1079 |         node_id_map[ n ] = id;
1080 |     }
1081 |     return id;
1082 | }
1083 | 
1084 | template<typename VertexListGraph, typename Vertex>
1085 | std::map<Vertex, SparseReconstruction::Triangle3D> SparseReconstruction::map_vertices_to_triangles(std::vector<Triangle3D> tris, VertexListGraph &g)
1086 | {
1087 |     using namespace boost;
1088 |     using namespace std;
1089 | 
1090 |     typedef typename graph_traits<VertexListGraph>::vertex_iterator VItr;
1091 | 
1092 |     std::map<Vertex, Triangle3D> v_pmap;
1093 | 
1094 |     VItr vi, ve;
1095 |     int idx(-1);
1096 |     for (boost::tie(vi, ve) = vertices(g); vi != ve; ++vi)
1097 |     {
1098 |         Vertex v(*vi);
1099 |         if (idx != -1) //we leave the first vertex unmapped, since it's a dummy vertex
1100 |         {
1101 |             v_pmap[v] = tris[idx];
1102 |         }
1103 | 
1104 |         idx++;
1105 |     }
1106 | 
1107 |     return v_pmap;
1108 | }
1109 | 
1110 | template<typename WeightMap, typename Vertex, typename VertexListGraph>
1111 | void SparseReconstruction::create_connected_graph(VertexListGraph &g, WeightMap wmap, std::map<Vertex,Triangle3D> vmap)
1112 | {
1113 |     using namespace std;
1114 |     using namespace boost;
1115 |     typedef typename graph_traits<VertexListGraph>::edge_descriptor Edge;
1116 |     typedef typename graph_traits<VertexListGraph>::vertex_iterator VItr;
1117 | 
1118 |     Edge e;
1119 |     bool inserted;
1120 | 
1121 |     pair<VItr, VItr> verts(vertices(g));
1122 |     for (VItr src(verts.first); src != verts.second; src++)
1123 |     {
1124 |         for (VItr dest(src); dest != verts.second; dest++)
1125 |         {
1126 |             if (dest != src)
1127 |             {
1128 |                 double weight = 0.0;
1129 | 
1130 |                 //the weights to and from the dummy vertex (indexes NULL) will be left as zero
1131 |                 if (vmap.find(*src) != vmap.end() && vmap.find(*dest) != vmap.end()){
1132 |                     Vector<3> src_center = get_triangle_centroid( vmap[*src].tri );
1133 |                     Vector<3> dest_center = get_triangle_centroid( vmap[*dest].tri );
1134 | 
1135 |                     weight = euclidean_distance_squared(src_center, dest_center);
1136 |                 }
1137 | 
1138 |                 boost::tie(e, inserted) = add_edge(*src, *dest, g);
1139 | 
1140 |                 wmap[e] = weight;
1141 |             }
1142 | 
1143 | 
1144 |         }
1145 |     }
1146 |     ROS_INFO("Found %lu vertices and %lu edges\n", num_vertices(g), num_edges(g));
1147 | }
1148 | 
1149 | 
1150 | SparseReconstruction::CellInfo::CellInfo() : id(CellInfo::NO_ID) {}
1151 | 
1152 | SparseReconstruction::Triangle3D* SparseReconstruction::CellInfo::getCommonTriangle3D( int cell_id )
1153 | {
1154 |     for (unsigned int i=0; i < surfaceTris.size(); i++)
1155 |     {
1156 |         if (surfaceTris[i]->c1->info().id == cell_id
1157 |                 || surfaceTris[i]->c2->info().id == cell_id )
1158 |             return surfaceTris[i];
1159 |     }
1160 |     return NULL;
1161 | }
1162 | 
1163 | unsigned long SparseReconstruction::VertexInfo::nextVertexId(0);
1164 | 
1165 | SparseReconstruction::VertexInfo::VertexInfo()
1166 | {
1167 |     id = nextVertexId++;
1168 | }
1169 | 
1170 | 
1171 | 
1172 | //bool SparseReconstruction::saveMesh(const std::string &dir)
1173 | //{
1174 | //    std::string dirName = dir;
1175 | //    if(*dirName.rbegin() != '/')
1176 | //        dirName += '/';
1177 | //    std::set<int> framesUsed;
1178 | //    //char curDIR[256];
1179 | //    //getcwd(&curDIR, 256);
1180 | //    std::string xmlFile = dirName + "mapdata.xml";
1181 | 
1182 | //    ROS_INFO("Saving map to %s", dirName.c_str());
1183 | 
1184 | //    struct stat dirStatus;
1185 | //    if(!(stat(dirName.c_str(),&dirStatus) == 0 && (dirStatus.st_mode & S_IFDIR) != 0))
1186 | //    {
1187 | //        ROS_INFO("Directory '%s' doesn't exist.  Creating...", dirName.c_str());
1188 | //        // doesn't exist, make it
1189 | //        if(mkdir(dirName.c_str(), S_IRWXU | S_IRWXG | S_IRWXO ) != 0)
1190 | //        {
1191 | //            ROS_WARN("Failed to create %s! Aborting!", dirName.c_str());
1192 | //            return false;
1193 | //        }
1194 | //    }else
1195 | //    {
1196 | //        ROS_WARN("Directory already '%s' exists!  Aborting!", dirName.c_str());
1197 | //        return false;
1198 | //        struct dirent *fileHandle;
1199 | //        DIR *dirHandle;
1200 | 
1201 | //        dirHandle = opendir(dirName.c_str());
1202 | 
1203 | //        while ( (fileHandle = readdir(dirHandle)) )
1204 | //        {
1205 | //            if(fileHandle->d_type != DT_DIR)
1206 | //            {
1207 | //                // build the full path for each file in the folder
1208 | //                std::string fName(fileHandle->d_name);
1209 | //                fName = dirName + fName;
1210 | //                //ROS_INFO("pretending to remove %s", fName.c_str());
1211 | //                remove(fName.c_str());
1212 | //            }
1213 | //        }
1214 | //    }
1215 | 
1216 | //    std::ofstream xmlf(xmlFile.c_str(), std::ofstream::binary | std::ofstream::trunc);
1217 | //    if(!xmlf || !xmlf.is_open())
1218 | //        return false;
1219 | 
1220 | //    ROS_INFO("Writing XML...");
1221 | //    // NOTE: faking XML as there is little reason to use an xml library if this is only going to be replaced eventually
1222 | 
1223 | //    /* format:
1224 | //     *      <map>
1225 | //     *          <version='0.0.0'/>
1226 | //     *          <triangles>
1227 | //     *              ...
1228 | //     *          </triangles>
1229 | //     *          <frames>
1230 | //     *              ...
1231 | //     *          </frames>
1232 | //     *      </map>*/
1233 | 
1234 | //    // writing header
1235 | 
1236 | //    xmlf << "<map>\n";
1237 | //    xmlf << "\t<version value='0.0.0'/>\n";
1238 | //    xmlf << "\t<triangles count='" << surfaceTriangles.size() <<"'>\n";
1239 | 
1240 | //    // then for each triangle in the mesh
1241 | //    for(unsigned int ii=0; ii<surfaceTriangles.size(); ii++)
1242 | //    {
1243 | //        // get the vertexes of the triangle
1244 | //        Matrix<3,3> tri = surfaceTriangles[ii]->tri;
1245 | 
1246 | //        // and the texture id for the texture to use
1247 | //        int bestKeyFrame = surfaceTriangles[ii]->KeyframeTex;
1248 | 
1249 | //        // add to used frames
1250 | //        framesUsed.insert(bestKeyFrame);
1251 | 
1252 | //        // write triangle out to file
1253 | //        /* format:
1254 | //         *          <tri frame = bestKeyFrame>
1255 | //         *              <vertex>
1256 | //         *                  <pos x='x' y='y' z='z'/>
1257 | //         *                  <texture x='x' y='y'/>
1258 | //         *              </vertex>
1259 | //         *              <vertex> ... x2
1260 | //         *              </vertex>
1261 | //         *          </tri>*/
1262 | 
1263 | //        xmlf << "\t\t<tri frame='" << ((bestKeyFrame<0)?0:bestKeyFrame) << "'>\n";
1264 | 
1265 | //        // then for each vertex in triangle
1266 | //        for(unsigned int i=0; i<3; i++)
1267 | //        {
1268 | //            xmlf << "\t\t\t<vertex>\n";
1269 | //            // get the texture coordinate
1270 | //            Vector<2> crd = surfaceTriangles[ii]->textureCord[i];
1271 | //            // and the vertex pos
1272 | //            Vector<3> vPos = tri[i];
1273 | 
1274 | //            xmlf << "\t\t\t\t<pos x='" << vPos[0] << "' y='" << vPos[1] << "' z='" << vPos[2] << "'/>\n";
1275 | //            xmlf << "\t\t\t\t<texture x='" << crd[0] << "' y='" << crd[1] << "'/>\n";
1276 | 
1277 | //            xmlf << "\t\t\t</vertex>\n";
1278 | //        }
1279 | 
1280 | //        xmlf << "\t\t</tri>\n";
1281 | //    }
1282 | 
1283 | //    // closing triangles
1284 | //    xmlf << "\t</triangles>\n";
1285 | //    // openning frames
1286 | //    xmlf << "\t<frames count='"<< framesUsed.size() <<"'>\n";
1287 | 
1288 | //    ROS_INFO("Writing keyframes...");
1289 | 
1290 | //    // Serialize textures in file
1291 | //    for(std::set<int>::iterator I = framesUsed.begin(); I != framesUsed.end(); ++I)
1292 | //    {
1293 | //        int kf = *I;
1294 | //        if(kf < 0) continue;
1295 | 
1296 | //        std::stringstream ss;
1297 | //        ss << kf;
1298 | //        std::string frameName = "frame" + ss.str() + ".jpg";
1299 | //        std::string frameFile = dirName + frameName;
1300 | //        // going to save images outside of the xml with ( http://www.edwardrosten.com/cvd/cvd/html/group__gImageIO.html#ga7b74b54ba166c9f3901e5eec9fa07bfe )
1301 | //        // then link the xml to the image name for loading purposes
1302 | 
1303 | //        std::ofstream frameF(frameFile.c_str(), std::ofstream::binary | std::ofstream::trunc);
1304 | //        if(!frameF || !frameF.is_open())
1305 | //        {
1306 | //            // close out and fail
1307 | //            xmlf.close();
1308 | //            return false;
1309 | //        }
1310 | //        // otherwise write the frame to disk
1311 | //        // TODO, write to disk
1312 | 
1313 | //        CVD::img_save(map->vpKeyFrames[kf]->imColor, frameF, CVD::ImageType::JPEG );
1314 | 
1315 | 
1316 | //        // done writing image
1317 | //        frameF.close();
1318 | //        // write frame info to xml.
1319 | //        /* format:
1320 | //         *      <frame id ='kf' file='file'/>
1321 | //         */
1322 | 
1323 | //        xmlf << "\t\t<frame id='" << kf << "' file='" << frameName << "'/>\n";
1324 | //    }
1325 | 
1326 | //    // closing textures and opening vertexs
1327 | //    xmlf << "\t</frames>\n";
1328 | //    // and closing root node
1329 | //    xmlf << "</map>\n";
1330 | 
1331 | //    xmlf.close();
1332 | //    return true;
1333 | //}
1334 | 
1335 | //bool SparseReconstruction::savePoints(const std::string &file)
1336 | //{
1337 | //    std::string xmlFile = file + ".xml";
1338 | 
1339 | //    ROS_INFO("Saving points to %s", xmlFile.c_str());
1340 | 
1341 | //    std::ofstream xmlf(xmlFile.c_str(), std::ofstream::binary | std::ofstream::trunc);
1342 | //    if(!xmlf || !xmlf.is_open())
1343 | //        return false;
1344 | 
1345 | //    long unsigned int keyFramePointSize = 0;
1346 | 
1347 | //    ROS_INFO("Writing XML...");
1348 | //    // NOTE: faking XML as there is little reason to use an xml library if this is only going to be replaced eventually
1349 | 
1350 | //    /* format:
1351 | //     *      <pointmap>
1352 | //     *          <version='0.0.0'/>
1353 | //     *          <points count=..>
1354 | //     *              <point x=.. y=.. z=..>
1355 | //     *                  <kfs count=..>
1356 | //     *                      <kf id=../>
1357 | //     *                      ...
1358 | //     *                  </kfs>
1359 | //     *              </point>
1360 | //     *              ...
1361 | //     *          </points>
1362 | //     *          <keyframes count=..>
1363 | //     *              <kf id=.. x=.. y=.. z=.. />
1364 | //     *          </keyframes>
1365 | //     *      </map>*/
1366 | 
1367 | //    // writing header
1368 | 
1369 | //    xmlf << std::fixed << std::setprecision(10);
1370 | 
1371 | //    xmlf << "<pointmap>\n";
1372 | //    xmlf << "\t<version value='0.0.0'/>\n";
1373 | //    xmlf << "\t<points count='" << map->vpPoints.size() <<"'>\n";
1374 | 
1375 | //    std::map<KeyFrame::Ptr, unsigned int> kf2id;
1376 | //    unsigned int kfidcount = 0;
1377 | 
1378 | //    // then for each point in the map
1379 | //    for(unsigned int ii=0; ii < map->vpPoints.size(); ii++)
1380 | //    {
1381 | //        boost::shared_ptr<MapPoint> cur = map->vpPoints[ii];
1382 | //        // get the position of the point
1383 | //        Vector<3> pos = cur->v3WorldPos;
1384 | 
1385 | //        xmlf << "\t\t<point x='" << pos[0] << "' y='" << pos[1] << "' z='" << pos[2] << "'>\n";
1386 | 
1387 | //        // get the number of keyframes that saw it
1388 | //        unsigned int kfc = cur->pMMData->sMeasurementKFs.size();
1389 | 
1390 | //        xmlf << "\t\t\t<kfs count='" << kfc << "'>\n";
1391 | 
1392 | //        // for each keyframe
1393 | //        for(std::set<KeyFrame::Ptr>::iterator iitt = cur->pMMData->sMeasurementKFs.begin(); iitt != cur->pMMData->sMeasurementKFs.end(); ++iitt)
1394 | //        {
1395 | //            boost::shared_ptr<KeyFrame> curkf = *iitt;
1396 | //            if(kf2id.find(curkf) == kf2id.end())
1397 | //            {
1398 | //                // assign a new id
1399 | //                kf2id[curkf] = ++kfidcount;
1400 | //            }
1401 | 
1402 | //            xmlf << "\t\t\t\t<kf id='" << kf2id[curkf] << "'/>\n";
1403 | //            keyFramePointSize++;
1404 | //        }
1405 | 
1406 | //        // closing keyframes
1407 | //        xmlf << "\t\t\t</kfs>\n";
1408 | 
1409 | //        // closing point
1410 | //        xmlf << "\t\t</point>\n";
1411 | //    }
1412 | 
1413 | //    ROS_INFO("Saved %lu points", map->vpPoints.size());
1414 | 
1415 | //    ROS_INFO("Saved %lu unique keyframe-point pairs", keyFramePointSize);
1416 | 
1417 | //    // closing points
1418 | //    xmlf << "\t</points>\n";
1419 | 
1420 | //    xmlf << "\t<keyframes count='" << kfidcount << "'>\n";
1421 | 
1422 | //    // for each keyframe
1423 | //    for(std::map<KeyFrame::Ptr, unsigned int>::iterator iitt = kf2id.begin(); iitt != kf2id.end(); ++iitt)
1424 | //    {
1425 | //        KeyFrame::Ptr curkf = (*iitt).first;
1426 | //        unsigned int kfid = (*iitt).second;
1427 | //        Vector<3> kfpos  = curkf->se3CfromW.inverse().get_translation();
1428 | 
1429 | //        xmlf << "\t\t<kf id='" << kfid << "' x='" << kfpos[0] << "' y='" <<kfpos[1] << "' z='" << kfpos[2] << "'/>\n";
1430 | //    }
1431 | 
1432 | //    ROS_INFO("Saved %lu keyframes", kf2id.size());
1433 | 
1434 | //    // closing keyframes
1435 | //    xmlf << "\t</keyframes>\n";
1436 | 
1437 | //    // closing map
1438 | //    xmlf << "</pointmap>\n";
1439 | 
1440 | //    long unsigned int fileSize = static_cast<unsigned int>(xmlf.tellp());
1441 | //    ROS_INFO("Saved %lu bytes", fileSize);
1442 | 
1443 | //    xmlf.close();
1444 | //    return true;
1445 | //}
1446 | 
1447 | 
1448 | #ifdef GL_VISUALIZATION
1449 | 
1450 | void SparseReconstruction::DrawCell(Delaunay::Cell_handle ch)
1451 | {
1452 |     Vector<3> v[4];
1453 |     int n=0;
1454 | 
1455 |     if(delaunayMesh.is_infinite(ch))
1456 |     {
1457 |         return;
1458 |         for(int i=0; i<4;i++)
1459 |         {
1460 |             if(!delaunayMesh.is_infinite(ch->vertex(i)))
1461 |             {
1462 |                 n++;
1463 |                 copy(v[i], ch->vertex(i)->point());
1464 |             }
1465 |         }
1466 |     }
1467 |     else
1468 |     {
1469 |         for(int i=0; i<4;i++)
1470 |         {
1471 |             copy(v[i], ch->vertex(i)->point());
1472 |         }
1473 |         n = 4;
1474 |     }
1475 | 
1476 | }
1477 | 
1478 | 
1479 | void SparseReconstruction::glDraw()
1480 | {
1481 |     //ROS_INFO("check 1");
1482 | 
1483 |     //load the textures
1484 | //    for(int kf = 0; kf < map->vpKeyFrames.size(); kf++)
1485 | //    {
1486 | //        if(!map->vpKeyFrames[kf]->TextureLoaded)
1487 | //        {
1488 | //            //ROS_INFO("Loading Texture for KeyFrame#: %d", kf);
1489 | //            map->vpKeyFrames[kf]->TextureLoaded = true;
1490 | //            glEnable(GL_TEXTURE_2D);
1491 | //            glGenTextures(1, &map->vpKeyFrames[kf]->mnFrameTex);
1492 | //            glBindTexture(GL_TEXTURE_2D, map->vpKeyFrames[kf]->mnFrameTex);
1493 | //            glTexImage2D(GL_TEXTURE_2D,
1494 | //            0, GL_RGB,
1495 | //                            map->vpKeyFrames[kf]->imColor.size().x, map->vpKeyFrames[kf]->imColor.size().y,
1496 | //            0,GL_RGB,
1497 | //            GL_UNSIGNED_BYTE,
1498 | //            map->vpKeyFrames[kf]->imColor.data());
1499 | //            glTexParameteri(GL_TEXTURE_2D , GL_TEXTURE_MIN_FILTER , GL_NEAREST);
1500 | //            glTexParameteri(GL_TEXTURE_2D , GL_TEXTURE_MAG_FILTER , GL_NEAREST);
1501 | //        }
1502 | //    }
1503 | 
1504 | 
1505 |     if(false)
1506 |     {
1507 |         Delaunay::Finite_cells_iterator cit = delaunayMesh.finite_cells_begin();
1508 | 
1509 |         glLineWidth(1);
1510 | 
1511 |         glBegin(GL_LINES);
1512 | 
1513 |         for(;cit != delaunayMesh.finite_cells_end(); ++cit)
1514 |         {
1515 |             Delaunay::Vertex_handle v[4];
1516 | 
1517 |             v[0] = cit->vertex(0);
1518 |             v[1] = cit->vertex(1);
1519 |             v[2] = cit->vertex(2);
1520 |             v[3] = cit->vertex(3);
1521 | 
1522 |             Vector<3> p[4];
1523 | 
1524 |             copy(p[0],v[0]->point());
1525 |             copy(p[1],v[1]->point());
1526 |             copy(p[2],v[2]->point());
1527 |             copy(p[3],v[3]->point());
1528 | 
1529 | 
1530 | //            for(int i=0; i<4; i++)
1531 | //                for(int j=0; j<4; j++)
1532 | //                {
1533 | //                    CVD::glVertex(p[i]);
1534 | //                    CVD::glVertex(p[j]);
1535 | //                }
1536 |         }
1537 | 
1538 | 
1539 |         glEnd();
1540 |     }
1541 | 
1542 | 
1543 | 
1544 |     if(/*ParamsAccess::varParams->DrawInsideCells*/false)
1545 |     {
1546 |         for(unsigned int ii=0; ii<surfaceTriangles.size(); ii++)
1547 |         {
1548 | 
1549 |             double c = ((double)(ii%255))/255.0;
1550 | 
1551 |             glColor4f(1-c,c,1,1);
1552 |             glLineWidth(2);
1553 | 
1554 |             glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
1555 |             glBegin(GL_TRIANGLES);
1556 | 
1557 |             if(surfaceTriangles[ii]->c2 != NULL)
1558 |                 DrawCell(surfaceTriangles[ii]->c2);
1559 | 
1560 |             glEnd();
1561 |         }
1562 | 
1563 |     }
1564 | 
1565 |     if(/*ParamsAccess::varParams->DrawSurfaceMesh*/true)
1566 |     {
1567 | 
1568 |         for(unsigned int ii=0; ii<surfaceTriangles.size(); ii++)
1569 |         {
1570 |             Matrix<3,3> tri = surfaceTriangles[ii]->tri;
1571 | 
1572 | 
1573 |             glColor3f(1,1,1);
1574 |             glLineWidth(3);
1575 | 
1576 |             glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
1577 |             glBegin(GL_TRIANGLES);
1578 |             for(int i=0; i<3; i++)
1579 |             {
1580 |                 glVertex3d(tri[i][0], tri[i][1], tri[i][2]);
1581 |             }
1582 |             glEnd();
1583 |         }
1584 | 
1585 |     }
1586 | 
1587 | 
1588 | 
1589 | //    if(/*ParamsAccess::varParams->DrawTexturedMesh*/true)
1590 | //    {
1591 | //        glColor3f(0,1,0);
1592 | //        glLineWidth(2);
1593 | 
1594 | 
1595 | //        //glPolygonMode(GL_FRONT, GL_FILL);
1596 | //        //glBegin(GL_LINES);
1597 | //        for(unsigned int ii=0; ii<surfaceTriangles.size(); ii++)
1598 | //        {
1599 | //            Matrix<3,3> tri = surfaceTriangles[ii]->tri;
1600 | 
1601 | //            int bestKeyframe = surfaceTriangles[ii]->KeyframeTex; //VisibilityServer::GetInstance()->GetBestKeyFrame(tri[0], tri[1], tri[2]);
1602 | 
1603 | //            if(bestKeyframe>=0)
1604 | //            {
1605 | //                glEnable(GL_TEXTURE_2D);
1606 | //                glBindTexture(GL_TEXTURE_2D, map->vpKeyFrames[bestKeyframe]->mnFrameTex);
1607 | //            }
1608 | 
1609 | //            glPolygonMode(GL_FRONT_AND_BACK , GL_FILL);
1610 | 
1611 | //            glBegin(GL_TRIANGLES);
1612 | 
1613 | //           // bool tx = TriangleNormalInCamera(map->vpKeyFrames[f[ff]]->se3CfromW, tri);
1614 | //            for(int i=0; i<3; i++)
1615 | //            {
1616 | //                glColor3f(1, 1, 1);
1617 | //                Vector<2> crd = surfaceTriangles[ii]->textureCord[i];//VisibilityServer::GetInstance()->GetImageCoord(tri[i], bestKeyframe);
1618 | //              //  if(tx)
1619 | //                {
1620 | //                    if (/*ParamsAccess::varParams->DrawTriangleValidityTrace*/false){
1621 | //                        continue;
1622 | //                        /* Here is the colour key:
1623 | //                         *      yellow triangles: too_far
1624 | //                         *      red triangles: too_big && !too_far
1625 | //                         *      green triangles: too_big && validated_by_angle && !too_far
1626 | //                         */
1627 | //                        if (surfaceTriangles[ii]->too_far)
1628 | //                        {
1629 | //                            glColor3f(0.8, 0.8, 0); //draw yellow triangle if it's too far
1630 | //                        }
1631 | //                        else
1632 | //                        {
1633 | //                            if ( surfaceTriangles[ii]->occluded )
1634 | //                            {
1635 | //                                //back facing => blue
1636 | //                                glColor3f(0, 0, 1);
1637 | //                            }
1638 | //                            else if ( surfaceTriangles[ii]->too_big )
1639 | //                            {
1640 | //                                //big & not validated => red
1641 | //                                glColor3f(1, 0, 0);
1642 | //                            }
1643 | //                            else
1644 | //                            {
1645 | //                                //not too big, not occluded and not too far => just draw the texture
1646 | //                                glTexCoord2f(crd[0], crd[1]);
1647 | //                            }
1648 | //                        }
1649 | 
1650 | //                    }
1651 | //                    else
1652 | //                    {
1653 | //                        if(surfaceTriangles[ii]->unexplored || bestKeyframe<0)
1654 | //                            glColor3f(0.5,0.5,0.5);
1655 | //                        else
1656 | //                            glTexCoord2f(crd[0], crd[1]);
1657 | //                    }
1658 | //                    glVertex3d(tri[i][0], tri[i][1], tri[i][2]);
1659 | //                }
1660 | //            }
1661 | //            glEnd();
1662 | //            glDisable(GL_TEXTURE_2D);
1663 | 
1664 | 
1665 | //        }
1666 | 
1667 | //    }
1668 | 
1669 | }
1670 | 
1671 | #endif
1672 | 


--------------------------------------------------------------------------------