bool
pcl::ihs::Integration::reconstructMesh (const CloudProcessedConstPtr& cloud_data,
const MeshPtr& mesh_model) const
{
if (!cloud_data->isOrganized ())
{
std::cerr << "ERROR in integration.cpp: Cloud is not organized\n";
return (false);
}
const uint32_t width = cloud_data->width;
const uint32_t height = cloud_data->height;
const size_t size = cloud_data->size ();
mesh_model->clear ();
mesh_model->reserveVertexes (size);
mesh_model->reserveFaces (2 * (width-1) * (height-1));
// Store which vertex is set at which position (initialized with invalid indexes)
VertexIndexes vertex_indexes (size, VertexIndex ());
// Convert to the model cloud type. This is actually not needed but avoids code duplication (see merge). And reconstructMesh is called only the first reconstruction step anyway.
// NOTE: The default constructor of PointModel has to initialize with NaNs!
CloudModelPtr cloud_model (new CloudModel ());
cloud_model->resize (size);
// Set the model points not reached by the main loop
for (uint32_t c=0; c<width; ++c)
{
const PointProcessed& pt_d = cloud_data->operator [] (c);
const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
if (pcl::isFinite (pt_d) && weight >= weight_min_)
{
cloud_model->operator [] (c) = PointModel (pt_d, weight);
}
}
for (uint32_t r=1; r<height; ++r)
{
for (uint32_t c=0; c<2; ++c)
{
const PointProcessed& pt_d = cloud_data->operator [] (r*width + c);
const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
if (pcl::isFinite (pt_d) && weight >= weight_min_)
{
cloud_model->operator [] (r*width + c) = PointModel (pt_d, weight);
}
}
}
// 4 2 - 1 //
// | | //
// * 3 - 0 //
// //
// 4 - 2 1 //
// \ \ //
// * 3 - 0 //
CloudProcessed::const_iterator it_d_0 = cloud_data->begin () + width + 2;
CloudModel::iterator it_m_0 = cloud_model->begin () + width + 2;
CloudModel::const_iterator it_m_1 = cloud_model->begin () + 2;
CloudModel::const_iterator it_m_2 = cloud_model->begin () + 1;
CloudModel::const_iterator it_m_3 = cloud_model->begin () + width + 1;
CloudModel::const_iterator it_m_4 = cloud_model->begin () ;
VertexIndexes::iterator it_vi_0 = vertex_indexes.begin () + width + 2;
VertexIndexes::iterator it_vi_1 = vertex_indexes.begin () + 2;
VertexIndexes::iterator it_vi_2 = vertex_indexes.begin () + 1;
VertexIndexes::iterator it_vi_3 = vertex_indexes.begin () + width + 1;
VertexIndexes::iterator it_vi_4 = vertex_indexes.begin () ;
for (uint32_t r=1; r<height; ++r)
{
for (uint32_t c=2; c<width; ++c)
{
const float weight = -it_d_0->normal_z; // weight = -dot (normal, [0; 0; 1])
if (pcl::isFinite(*it_d_0) && weight >= weight_min_)
{
*it_m_0 = PointModel (*it_d_0, weight);
}
this->addToMesh (it_m_0, it_m_1, it_m_2, it_m_3,
it_vi_0, it_vi_1, it_vi_2, it_vi_3,
mesh_model);
this->addToMesh (it_m_0, it_m_2, it_m_4, it_m_3,
it_vi_0, it_vi_2, it_vi_4, it_vi_3,
mesh_model);
++it_d_0;
++it_m_0; ++it_m_1; ++it_m_2; ++it_m_3; ++it_m_4;
++it_vi_0; ++it_vi_1; ++it_vi_2; ++it_vi_3; ++it_vi_4;
} // for (uint32_t c=2; c<width; ++c)
it_d_0 += 2;
it_m_0 += 2; it_m_1 += 2; it_m_2 += 2; it_m_3 += 2, it_m_4 += 2;
it_vi_0 += 2; it_vi_1 += 2; it_vi_2 += 2; it_vi_3 += 2, it_vi_4 += 2;
} // for (uint32_t r=1; r<height; ++r)
return (true);
}
void
pcl::ihs::OfflineIntegration::computationThread ()
{
std::vector <std::string> filenames;
if (!this->getFilesFromDirectory (path_dir_, ".pcd", filenames))
{
std::cerr << "ERROR in offline_integration.cpp: Could not get the files from the directory\n";
return;
}
// First cloud is reference model
std::cerr << "Processing file " << std::setw (5) << 1 << " / " << filenames.size () << std::endl;
CloudXYZRGBNormalPtr cloud_model (new CloudXYZRGBNormal ());
Eigen::Matrix4f T = Eigen::Matrix4f::Identity ();
if (!this->load (filenames [0], cloud_model, T))
{
std::cerr << "ERROR in offline_integration.cpp: Could not load the model cloud.\n";
return;
}
pcl::transformPointCloudWithNormals (*cloud_model, *cloud_model, T);
if (!integration_->reconstructMesh (cloud_model, mesh_model_))
{
std::cerr << "ERROR in offline_integration.cpp: Could not reconstruct the model mesh.\n";
return;
}
Base::setPivot ("model");
Base::addMesh (mesh_model_, "model");
if (filenames.size () < 1)
{
return;
}
for (unsigned int i=1; i<filenames.size (); ++i)
{
std::cerr << "Processing file " << std::setw (5) << i+1 << " / " << filenames.size () << std::endl;
{
boost::mutex::scoped_lock lock (mutex_);
if (destructor_called_) return;
}
boost::mutex::scoped_lock lock_quit (mutex_quit_);
CloudXYZRGBNormalPtr cloud_data (new CloudXYZRGBNormal ());
if (!this->load (filenames [i], cloud_data, T))
{
std::cerr << "ERROR in offline_integration.cpp: Could not load the data cloud.\n";
return;
}
if (!integration_->merge (cloud_data, mesh_model_, T))
{
std::cerr << "ERROR in offline_integration.cpp: merge failed.\n";
return;
}
integration_->age (mesh_model_);
{
lock_quit.unlock ();
boost::mutex::scoped_lock lock (mutex_);
if (destructor_called_) return;
Base::addMesh (mesh_model_, "model", Eigen::Isometry3d (T.inverse ().cast <double> ()));
Base::calcFPS (computation_fps_);
}
}
Base::setPivot ("model");
}
bool
pcl::ihs::Integration::reconstructMesh (const CloudXYZRGBNormalConstPtr& cloud_data,
MeshPtr& mesh_model) const
{
if (!cloud_data)
{
std::cerr << "ERROR in integration.cpp: Cloud pointer is invalid\n";
return (false);
}
if (!cloud_data->isOrganized ())
{
std::cerr << "ERROR in integration.cpp: Cloud is not organized\n";
return (false);
}
const int width = static_cast <int> (cloud_data->width);
const int height = static_cast <int> (cloud_data->height);
if (!mesh_model) mesh_model = MeshPtr (new Mesh ());
mesh_model->clear ();
mesh_model->reserveVertices (cloud_data->size ());
mesh_model->reserveEdges ((width-1) * height + width * (height-1) + (width-1) * (height-1));
mesh_model->reserveFaces (2 * (width-1) * (height-1));
// Store which vertex is set at which position (initialized with invalid indices)
VertexIndices vertex_indices (cloud_data->size (), VertexIndex ());
// Convert to the model cloud type. This is actually not needed but avoids code duplication (see merge). And reconstructMesh is called only the first reconstruction step anyway.
// NOTE: The default constructor of PointIHS has to initialize with NaNs!
CloudIHSPtr cloud_model (new CloudIHS ());
cloud_model->resize (cloud_data->size ());
// Set the model points not reached by the main loop
for (int c=0; c<width; ++c)
{
const PointXYZRGBNormal& pt_d = cloud_data->operator [] (c);
const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
if (!boost::math::isnan (pt_d.x) && weight > min_weight_)
{
cloud_model->operator [] (c) = PointIHS (pt_d, weight);
}
}
for (int r=1; r<height; ++r)
{
for (int c=0; c<2; ++c)
{
const PointXYZRGBNormal& pt_d = cloud_data->operator [] (r*width + c);
const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
if (!boost::math::isnan (pt_d.x) && weight > min_weight_)
{
cloud_model->operator [] (r*width + c) = PointIHS (pt_d, weight);
}
}
}
// 4 2 - 1 //
// | | //
// * 3 - 0 //
// //
// 4 - 2 1 //
// \ \ //
// * 3 - 0 //
const int offset_1 = -width;
const int offset_2 = -width - 1;
const int offset_3 = - 1;
const int offset_4 = -width - 2;
for (int r=1; r<height; ++r)
{
for (int c=2; c<width; ++c)
{
const int ind_0 = r*width + c;
const int ind_1 = ind_0 + offset_1;
const int ind_2 = ind_0 + offset_2;
const int ind_3 = ind_0 + offset_3;
const int ind_4 = ind_0 + offset_4;
assert (ind_0 >= 0 && ind_0 < static_cast <int> (cloud_data->size ()));
assert (ind_1 >= 0 && ind_1 < static_cast <int> (cloud_data->size ()));
assert (ind_2 >= 0 && ind_2 < static_cast <int> (cloud_data->size ()));
assert (ind_3 >= 0 && ind_3 < static_cast <int> (cloud_data->size ()));
assert (ind_4 >= 0 && ind_4 < static_cast <int> (cloud_data->size ()));
const PointXYZRGBNormal& pt_d_0 = cloud_data->operator [] (ind_0);
PointIHS& pt_m_0 = cloud_model->operator [] (ind_0);
const PointIHS& pt_m_1 = cloud_model->operator [] (ind_1);
const PointIHS& pt_m_2 = cloud_model->operator [] (ind_2);
const PointIHS& pt_m_3 = cloud_model->operator [] (ind_3);
const PointIHS& pt_m_4 = cloud_model->operator [] (ind_4);
VertexIndex& vi_0 = vertex_indices [ind_0];
VertexIndex& vi_1 = vertex_indices [ind_1];
VertexIndex& vi_2 = vertex_indices [ind_2];
VertexIndex& vi_3 = vertex_indices [ind_3];
VertexIndex& vi_4 = vertex_indices [ind_4];
const float weight = -pt_d_0.normal_z; // weight = -dot (normal, [0; 0; 1])
if (!boost::math::isnan (pt_d_0.x) && weight > min_weight_)
{
pt_m_0 = PointIHS (pt_d_0, weight);
}
this->addToMesh (pt_m_0,pt_m_1,pt_m_2,pt_m_3, vi_0,vi_1,vi_2,vi_3, mesh_model);
if (Mesh::IsManifold::value) // Only needed for the manifold mesh
{
this->addToMesh (pt_m_0,pt_m_2,pt_m_4,pt_m_3, vi_0,vi_2,vi_4,vi_3, mesh_model);
}
}
}
return (true);
}