void
pcl::ihs::Integration::age (const MeshPtr& mesh, const bool cleanup) const
{
for (Mesh::VertexIterator it = mesh->beginVertexes (); it!=mesh->endVertexes (); ++it)
{
if(it->age < age_max_)
{
// Point survives
++it->age;
}
else if(it->age == age_max_) // Judgement Day
{
if(it->visconf.getValue () < visconf_min_)
{
// Point dies (no need to transform it)
mesh->deleteVertex (*it);
}
else
{
// Point becomes immortal
it->age = std::numeric_limits <unsigned int>::max ();
}
}
}
if (cleanup)
{
mesh->cleanUp ();
}
}
bool
pcl::ihs::Integration::merge (const CloudProcessedConstPtr& cloud_data,
const MeshPtr& mesh_model,
const Transformation& T) const
{
if (!cloud_data->isOrganized ())
{
std::cerr << "ERROR in integration.cpp: Data 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 ();
if (!mesh_model->sizeVertexes ())
{
std::cerr << "ERROR in integration.cpp: Model mesh is empty\n";
return (false);
}
// Nearest neighbor search
// TODO: remove this unnecessary copy
CloudXYZPtr xyz_model (new CloudXYZ ());
xyz_model->reserve (mesh_model->sizeVertexes ());
for (VertexConstIterator it=mesh_model->beginVertexes (); it!=mesh_model->endVertexes (); ++it)
{
xyz_model->push_back (PointXYZ (it->x, it->y, it->z));
}
kd_tree_->setInputCloud (xyz_model);
std::vector <int> index (1);
std::vector <float> squared_distance (1);
mesh_model->reserveVertexes (mesh_model->sizeVertexes () + cloud_data->size ());
mesh_model->reserveFaces (mesh_model->sizeFaces () + 2 * (width-1) * (height-1));
// Data cloud in model coordinates (this does not change the connectivity information) and weights
CloudModelPtr cloud_data_transformed (new CloudModel ());
cloud_data_transformed->resize (size);
// Sensor position in model coordinates
const Eigen::Vector4f& sensor_eye = T * Eigen::Vector4f (0.f, 0.f, 0.f, 1.f);
// Store which vertex is set at which position (initialized with invalid indexes)
VertexIndexes vertex_indexes (size, VertexIndex ());
// Set the transformed 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_)
{
PointModel& pt_d_t = cloud_data_transformed->operator [] (c);
pt_d_t = PointModel (pt_d, weight);
pt_d_t.getVector4fMap () = T * pt_d_t.getVector4fMap ();
pt_d_t.getNormalVector4fMap () = T * pt_d_t.getNormalVector4fMap ();
}
}
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_)
{
PointModel& pt_d_t = cloud_data_transformed->operator [] (r*width + c);
pt_d_t = PointModel (pt_d, weight);
pt_d_t.getVector4fMap () = T * pt_d_t.getVector4fMap ();
pt_d_t.getNormalVector4fMap () = T * pt_d_t.getNormalVector4fMap ();
}
}
}
// 4 2 - 1 //
// | | //
// * 3 - 0 //
// //
// 4 - 2 1 //
// \ \ //
// * 3 - 0 //
CloudProcessed::const_iterator it_d_0 = cloud_data->begin () + width + 2;
CloudModel::iterator it_d_t_0 = cloud_data_transformed->begin () + width + 2;
CloudModel::const_iterator it_d_t_1 = cloud_data_transformed->begin () + 2;
CloudModel::const_iterator it_d_t_2 = cloud_data_transformed->begin () + 1;
CloudModel::const_iterator it_d_t_3 = cloud_data_transformed->begin () + width + 1;
CloudModel::const_iterator it_d_t_4 = cloud_data_transformed->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_d_t_0 = PointModel (*it_d_0, weight);
it_d_t_0->getVector4fMap () = T * it_d_t_0->getVector4fMap ();
it_d_t_0->getNormalVector4fMap () = T * it_d_t_0->getNormalVector4fMap ();
// NN search
if (!kd_tree_->nearestKSearchT (*it_d_t_0, 1, index, squared_distance))
{
std::cerr << "ERROR in integration.cpp: nearestKSearch failed!\n";
return (false);
}
// Average out corresponding points
if (squared_distance[0] <= squared_distance_max_)
{
Vertex& v_m = mesh_model->getElement (VertexIndex (index[0])); // Non-const reference!
if (v_m.getNormalVector4fMap ().dot (it_d_t_0->getNormalVector4fMap ()) >= dot_normal_min_)
{
*it_vi_0 = VertexIndex (index[0]);
const float W = v_m.weight; // Old accumulated weight
const float w = it_d_t_0->weight; // Weight of new point
const float WW = v_m.weight = W + w; // New accumulated weight
const float r_m = static_cast <float> (v_m.r);
const float g_m = static_cast <float> (v_m.g);
const float b_m = static_cast <float> (v_m.b);
const float r_d = static_cast <float> (it_d_t_0->r);
const float g_d = static_cast <float> (it_d_t_0->g);
const float b_d = static_cast <float> (it_d_t_0->b);
v_m.getVector4fMap () = ( W*v_m.getVector4fMap () + w*it_d_t_0->getVector4fMap ()) / WW;
v_m.getNormalVector4fMap () = ((W*v_m.getNormalVector4fMap () + w*it_d_t_0->getNormalVector4fMap ()) / WW).normalized ();
v_m.r = this->trimRGB ((W*r_m + w*r_d) / WW);
v_m.g = this->trimRGB ((W*g_m + w*g_d) / WW);
v_m.b = this->trimRGB ((W*b_m + w*b_d) / WW);
// Point has been observed again -> give it some extra time to live
v_m.age = 0;
// add a direction to the visibility confidence
v_m.visconf.addDirection (v_m.getNormalVector4fMap (), sensor_eye-v_m.getVector4fMap (), w);
} // dot normals
} // squared distance
} // isfinite && min weight
// Connect
// 4 2 - 1 //
// | | //
// * 3 - 0 //
// //
// 4 - 2 1 //
// \ \ //
// * 3 - 0 //
this->addToMesh (it_d_t_0, it_d_t_1, it_d_t_2, it_d_t_3,
it_vi_0, it_vi_1, it_vi_2, it_vi_3,
mesh_model);
this->addToMesh (it_d_t_0, it_d_t_2, it_d_t_4, it_d_t_3,
it_vi_0, it_vi_2, it_vi_4, it_vi_3,
mesh_model);
++it_d_0;
++it_d_t_0; ++it_d_t_1; ++it_d_t_2; ++it_d_t_3; ++it_d_t_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_d_t_0 += 2; it_d_t_1 += 2; it_d_t_2 += 2; it_d_t_3 += 2, it_d_t_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);
}