MeshStage.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above
11  * copyright notice, this list of conditions and the following
12  * disclaimer in the documentation and/or other materials provided
13  * with the distribution.
14  * * Neither the name of Willow Garage, Inc. nor the names of its
15  * contributors may be used to endorse or promote products derived
16  * from this software without specific prior written permission.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
21  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
22  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
23  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
28  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  *
31  */
32 /*
33  * MeshStage.cpp
34  *
35  * @date 13.11.2015
36  * @author Tristan Igelbrink (Tristan@Igelbrink.com)
37  */
38 
39 #include <kfusion/MeshStage.hpp>
40 #include <lvr/registration/ICPPointAlign.hpp>
41 #include <lvr/io/DataStruct.hpp>
42 
43 // default constructor
44 MeshStage::MeshStage(double camera_target_distance, double voxel_size, Options* options) : AbstractStage(),
45  camera_target_distance_(camera_target_distance), voxel_size_(voxel_size), options_(options), fusion_count_(0),
46  slice_correction_(false)
47 {
48  mesh_count_ = 0;
49  timestamp.setQuiet(!options->verbose());
50 }
51 
52 void MeshStage::firstStep() { /* skip */ };
53 
54 void MeshStage::step()
55 {
56  auto grid_work = boost::any_cast<pair<pair<TGrid*, bool>, vector<ImgPose*> > >(getInQueue()->Take());
57  TGrid* act_grid = grid_work.first.first;
58  bool last_shift = grid_work.first.second;
59  MeshPtr meshPtr = new HMesh();
60  string mesh_notice = ("#### B: Mesh Creation " + to_string(mesh_count_) + " ####");
61  ScopeTime* cube_time = new ScopeTime(mesh_notice.c_str());
62 
63  cFastReconstruction* fast_recon = new cFastReconstruction(act_grid);
64  timestamp.setQuiet(!options_->verbose());
65  // Create an empty mesh
66  fast_recon->getMesh(*meshPtr);
67  transformMeshBack(meshPtr);
68  if(meshPtr->meshSize() == 0)
69  return;
70  unordered_map<HMesh::VertexPtr, HMesh::VertexPtr> verts_map;
71  size_t misscount = 0;
72  for(auto cellPair : act_grid->m_old_fusion_cells)
73  {
74  cFastBox* box = cellPair.second;
75  for( int edge_index = 0; edge_index < 12; edge_index++)
76  {
77  uint inter = box->m_intersections[edge_index];
78  uint inter2 = -1;
79  if(inter != cFastBox::INVALID_INDEX)
80  {
81  for(int i = 0; i < 3; i++)
82  {
83  auto current_neighbor = box->m_neighbors[neighbor_table[edge_index][i]];
84  if(current_neighbor != 0)
85  {
86  uint in2 = current_neighbor->m_intersections[neighbor_vertex_table[edge_index][i]];
87  HMesh::VertexPtr old_vert = last_mesh_queue_.front()->getVertices()[inter];
88  auto vert_it = verts_map.find(old_vert);
89  if(vert_it == verts_map.end() && in2 != cFastBox::INVALID_INDEX && in2 != 0 && in2 != inter && current_neighbor->m_fusionNeighborBox)
90  {
91  inter2 = in2;
92  HMesh::VertexPtr act_vert = meshPtr->getVertices()[inter2];
93  verts_map.insert(pair<HMesh::VertexPtr, HMesh::VertexPtr>(old_vert, act_vert));
94  meshPtr->setOldFusionVertex(inter2);
95  if(act_vert->m_position[0] != old_vert->m_position[0] || act_vert->m_position[1] != old_vert->m_position[1]
96  || act_vert->m_position[2] != old_vert->m_position[2])
97  {
98  misscount++;
99  }
100  current_neighbor->m_fusionNeighborBox = false;
101  }
102  }
103  }
104  }
105  }
106  }
107  if(last_mesh_queue_.size() > 0)
108  {
109  auto m = last_mesh_queue_.front();
110  if(verts_map.size() > 0)
111  {
112 
113  if(slice_correction_ && (((double)verts_map.size()/m->m_fusionVertices.size() < 0.5) || ((double)misscount/verts_map.size() > 0.9)) )
114  {
115  float euler[6];
116  PointBufferPtr buffer(new PointBuffer());
117  PointBufferPtr dataBuffer(new PointBuffer());
118  floatArr vertexBuffer( new float[3 * m->m_fusionVertices.size()] );
119  floatArr dataVertexBuffer( new float[3 * meshPtr->m_oldFusionVertices.size()] );
120  for(size_t i = 0; i < m->m_fusionVertices.size()*3; i+=3)
121  {
122  vertexBuffer[i] = -m->m_fusionVertices[i/3]->m_position.x * 100;
123  vertexBuffer[i + 1] = -m->m_fusionVertices[i/3]->m_position.y * 100;
124  vertexBuffer[i + 2] = m->m_fusionVertices[i/3]->m_position.z * 100;
125  }
126  for(size_t i = 0; i < meshPtr->m_oldFusionVertices.size()*3; i+=3)
127  {
128  dataVertexBuffer[i] = -meshPtr->m_oldFusionVertices[i/3]->m_position.x * 100;
129  dataVertexBuffer[i + 1] = -meshPtr->m_oldFusionVertices[i/3]->m_position.y * 100;
130  dataVertexBuffer[i + 2] = meshPtr->m_oldFusionVertices[i/3]->m_position.z * 100;
131  }
132  buffer->setPointArray(vertexBuffer, m->m_fusionVertices.size());
133  dataBuffer->setPointArray(dataVertexBuffer, meshPtr->m_oldFusionVertices.size());
134  Vertexf position(0, 0, 0);
135  Vertexf angle(0, 0, 0);
136  Matrix4f transformation(position, angle);
137 
138  ICPPointAlign align(buffer, dataBuffer, transformation);
139  align.setMaxIterations(20);
140  align.setMaxMatchDistance(0.8);
141  Matrix4f correction = align.match();
142  Matrix4f trans;
143  trans.set(12, -correction[12]/100.0);
144  trans.set(13, -correction[13]/100.0);
145  trans.set(14, correction[14]/100.0);
146  trans.toPostionAngle(euler);
147  double correction_value = sqrt(pow(trans[12],2) + pow(trans[13],2) + pow(trans[14],2));
148  if(correction_value < 0.08)
149  {
150  cout << "Applieng ICP Pose " << endl;
151  cout << "Pose: " << correction[12] << " " << correction[13] << " " << correction[14] << " " << euler[3] << " " << euler[4] << " " << euler[5] << endl;
152  for(auto vert : meshPtr->getVertices())
153  {
154  vert->m_position.transform(trans);
155  }
156  map<size_t, HMesh::VertexPtr> kdFusionVertsMap;
157  cv::Mat data;
158  data.create(cvSize(3,m->m_fusionVertices.size()), CV_32F); // The set A
159  for(size_t i = 0; i < m->m_fusionVertices.size();i++)
160  {
161  data.at<float>(i,0) = m->m_fusionVertices[i]->m_position.x;
162  data.at<float>(i,1) = m->m_fusionVertices[i]->m_position.y;
163  data.at<float>(i,2) = m->m_fusionVertices[i]->m_position.z;
164  kdFusionVertsMap.insert(pair<size_t, HMesh::VertexPtr>(i,m->m_fusionVertices[i]));
165  }
166  map<size_t, HMesh::VertexPtr> kdOldFusionVertsMap;
167  cv::Mat query;
168  query.create(cvSize(3,meshPtr->m_oldFusionVertices.size()), CV_32F); // The set A
169  for(size_t i = 0; i < meshPtr->m_oldFusionVertices.size();i++)
170  {
171  query.at<float>(i,0) = meshPtr->m_oldFusionVertices[i]->m_position.x;
172  query.at<float>(i,1) = meshPtr->m_oldFusionVertices[i]->m_position.y;
173  query.at<float>(i,2) = meshPtr->m_oldFusionVertices[i]->m_position.z;
174  kdOldFusionVertsMap.insert(pair<size_t, HMesh::VertexPtr>(i, meshPtr->m_oldFusionVertices[i]));
175  }
176  cv::Mat matches; //This mat will contain the index of nearest neighbour as returned by Kd-tree
177  cv::Mat distances; //In this mat Kd-Tree return the distances for each nearest neighbour
178  //This set B
179  const cvflann::SearchParams params(32); //How many leaves to search in a tree
180  cv::flann::GenericIndex< cvflann::L2<float> > *kdtrees; // The flann searching tree
181 
182  // Create matrices
183  matches.create(cvSize(1,meshPtr->m_oldFusionVertices.size()), CV_32SC1);
184  distances.create(cvSize(1,meshPtr->m_oldFusionVertices.size()), CV_32FC1);
185  kdtrees = new cv::flann::GenericIndex< cvflann::L2<float> >(data, cvflann::KDTreeIndexParams(4)); // a 4 k-d tree
186  // Search KdTree
187  kdtrees->knnSearch(query, matches, distances, 1, cvflann::SearchParams(8));
188  int NN_index;
189  float dist;
190  verts_map.clear();
191  for(int i = 0; i < 10; i++) {
192 
193  NN_index = matches.at<int>(i,0);
194  dist = distances.at<float>(i, 0);
195  verts_map.insert(pair<HMesh::VertexPtr, HMesh::VertexPtr>(kdFusionVertsMap[NN_index], kdOldFusionVertsMap[i]));
196  }
197  delete kdtrees;
198  global_correction_ *= trans;
199  }
200 
201  }
202  }
203  meshPtr->m_fusion_verts = verts_map;
204  }
205  if(last_mesh_queue_.size() > 0)
206  {
207  //delete last_grid_queue_.front();
208  last_mesh_queue_.pop();
209  }
210  last_mesh_queue_.push(meshPtr);
211 
212  mesh_count_++;
213 
214  delete cube_time;
215  delete fast_recon;
216  getOutQueue()->Add(pair<pair<MeshPtr, bool>, vector<ImgPose*> >(
217  pair<MeshPtr, bool>(meshPtr, last_shift), grid_work.second));
218  if(last_shift)
219  done(true);
220 }
221 
222 void MeshStage::lastStep() { /* skip */ }
223 
224 
225 void MeshStage::transformMeshBack(MeshPtr mesh)
226 {
227  for(auto vert : mesh->getVertices())
228  {
229  // calc in voxel
230  vert->m_position.x *= voxel_size_;
231  vert->m_position.y *= voxel_size_;
232  vert->m_position.z *= voxel_size_;
233  //offset for cube coord to center coord
234  vert->m_position.x -= 1.5;
235  vert->m_position.y -= 1.5;
236  vert->m_position.z -= 1.5 - camera_target_distance_;
237 
238  //offset for cube coord to center coord
239  vert->m_position.x -= 150;
240  vert->m_position.y -= 150;
241  vert->m_position.z -= 150;
242  vert->m_position.transform(global_correction_);
243  }
244 }
cFastBox
FastKinFuBox< ColorVertex< float, unsigned char >, lvr::Normal< float > > cFastBox
Definition: GridStage.hpp:64
VertexPtr
lvr::HalfEdgeVertex< cVertex, lvr::Normal< float > > * VertexPtr
Definition: KinFuApp.hpp:14
lvr2::Matrix4f
Eigen::Matrix4f Matrix4f
Eigen 4x4 matrix, single precision.
Definition: MatrixTypes.hpp:147
mesh
HalfEdgeMesh< Vec > mesh
Definition: src/tools/lvr2_gs_reconstruction/Main.cpp:26
options
const kaboom::Options * options
Definition: src/tools/lvr2_kaboom/Main.cpp:45
MeshStage::camera_target_distance_
double camera_target_distance_
Definition: MeshStage.hpp:88
MeshStage::MeshStage
MeshStage(double camera_target_distance, double voxel_size, Options *options)
Definition: MeshStage.cpp:44
MeshStage::transformMeshBack
void transformMeshBack(MeshPtr mesh)
Definition: MeshStage.cpp:225
lvr2::timestamp
static Timestamp timestamp
A global time stamp object for program runtime measurement.
Definition: Timestamp.hpp:116
HMesh
HalfEdgeKinFuMesh< cVertex, lvr::Normal< float > > HMesh
Definition: FusionStage.hpp:62
kfusion::Options
A class to parse the program options for the reconstruction executable.
Definition: ext/kintinuous/kfusion/include/kfusion/Options.hpp:51
MeshStage::lastStep
virtual void lastStep()
Definition: MeshStage.cpp:222
MeshStage::global_correction_
lvr::Matrix4f global_correction_
Definition: MeshStage.hpp:93
lvr2::PointBufferPtr
std::shared_ptr< PointBuffer > PointBufferPtr
Definition: PointBuffer.hpp:130
MeshStage::step
virtual void step()
Definition: MeshStage.cpp:54
AbstractStage::getInQueue
boost::shared_ptr< BlockingQueue > getInQueue() const
Definition: AbstractStage.hpp:88
MeshStage::options_
Options * options_
Definition: MeshStage.hpp:91
AbstractStage::done
bool done() const
Definition: AbstractStage.hpp:91
AbstractStage::getOutQueue
boost::shared_ptr< BlockingQueue > getOutQueue() const
Definition: AbstractStage.hpp:89
MeshStage::slice_correction_
bool slice_correction_
Definition: MeshStage.hpp:92
lvr2::floatArr
boost::shared_array< float > floatArr
Definition: DataStruct.hpp:133
MeshStage::mesh_count_
size_t mesh_count_
Definition: MeshStage.hpp:90
MeshPtr
HMesh * MeshPtr
Definition: FusionStage.hpp:63
lvr2::neighbor_table
static const int neighbor_table[12][3]
Definition: FastBoxTables.hpp:41
lvr2::neighbor_vertex_table
static const int neighbor_vertex_table[12][3]
Definition: FastBoxTables.hpp:56
MeshStage::firstStep
virtual void firstStep()
Definition: MeshStage.cpp:52
uint
unsigned int uint
Definition: Model.hpp:46
TGrid
TsdfGrid< cVertex, cFastBox, kfusion::Point > TGrid
Definition: GridStage.hpp:65
lvr2::Timestamp::setQuiet
void setQuiet(bool quiet)
Definition: Timestamp.hpp:87
cFastReconstruction
FastReconstruction< ColorVertex< float, unsigned char >, lvr::Normal< float >, cFastBox > cFastReconstruction
Definition: GridStage.hpp:66
MeshStage::voxel_size_
double voxel_size_
Definition: MeshStage.hpp:89
MeshStage::last_mesh_queue_
queue< MeshPtr > last_mesh_queue_
Definition: MeshStage.hpp:87

lvr2
Author(s): Thomas Wiemann , Sebastian Pütz , Alexander Mock , Lars Kiesow , Lukas Kalbertodt , Tristan Igelbrink , Johan M. von Behren , Dominik Feldschnieders , Alexander Löhr
autogenerated on Mon Feb 28 2022 22:46:08