40 #include <lvr/registration/ICPPointAlign.hpp>
41 #include <lvr/io/DataStruct.hpp>
45 camera_target_distance_(camera_target_distance), voxel_size_(voxel_size), options_(
options), fusion_count_(0),
46 slice_correction_(false)
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;
60 string mesh_notice = (
"#### B: Mesh Creation " + to_string(
mesh_count_) +
" ####");
66 fast_recon->getMesh(*meshPtr);
68 if(meshPtr->meshSize() == 0)
70 unordered_map<HMesh::VertexPtr, HMesh::VertexPtr> verts_map;
72 for(
auto cellPair : act_grid->m_old_fusion_cells)
75 for(
int edge_index = 0; edge_index < 12; edge_index++)
77 uint inter = box->m_intersections[edge_index];
79 if(inter != cFastBox::INVALID_INDEX)
81 for(
int i = 0; i < 3; i++)
83 auto current_neighbor = box->m_neighbors[
neighbor_table[edge_index][i]];
84 if(current_neighbor != 0)
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)
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])
100 current_neighbor->m_fusionNeighborBox =
false;
110 if(verts_map.size() > 0)
113 if(
slice_correction_ && (((
double)verts_map.size()/m->m_fusionVertices.size() < 0.5) || ((
double)misscount/verts_map.size() > 0.9)) )
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)
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;
126 for(
size_t i = 0; i < meshPtr->m_oldFusionVertices.size()*3; i+=3)
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;
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);
138 ICPPointAlign align(buffer, dataBuffer, transformation);
139 align.setMaxIterations(20);
140 align.setMaxMatchDistance(0.8);
141 Matrix4f correction = align.match();
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)
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())
154 vert->m_position.transform(trans);
156 map<size_t, HMesh::VertexPtr> kdFusionVertsMap;
158 data.create(cvSize(3,m->m_fusionVertices.size()), CV_32F);
159 for(
size_t i = 0; i < m->m_fusionVertices.size();i++)
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]));
166 map<size_t, HMesh::VertexPtr> kdOldFusionVertsMap;
168 query.create(cvSize(3,meshPtr->m_oldFusionVertices.size()), CV_32F);
169 for(
size_t i = 0; i < meshPtr->m_oldFusionVertices.size();i++)
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]));
179 const cvflann::SearchParams params(32);
180 cv::flann::GenericIndex< cvflann::L2<float> > *kdtrees;
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));
187 kdtrees->knnSearch(query, matches, distances, 1, cvflann::SearchParams(8));
191 for(
int i = 0; i < 10; i++) {
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]));
203 meshPtr->m_fusion_verts = verts_map;
216 getOutQueue()->Add(pair<pair<MeshPtr, bool>, vector<ImgPose*> >(
217 pair<MeshPtr, bool>(meshPtr, last_shift), grid_work.second));
227 for(
auto vert :
mesh->getVertices())
234 vert->m_position.x -= 1.5;
235 vert->m_position.y -= 1.5;
239 vert->m_position.x -= 150;
240 vert->m_position.y -= 150;
241 vert->m_position.z -= 150;