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);
}
bool
pcl::ihs::Integration::merge (const CloudXYZRGBNormalConstPtr& cloud_data,
MeshPtr& mesh_model,
const Eigen::Matrix4f& T) 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: Data cloud is not organized\n";
return (false);
}
if (!mesh_model)
{
std::cerr << "ERROR in integration.cpp: Mesh pointer is invalid\n";
return (false);
}
if (!mesh_model->sizeVertices ())
{
std::cerr << "ERROR in integration.cpp: Model mesh is empty\n";
return (false);
}
const int width = static_cast <int> (cloud_data->width);
const int height = static_cast <int> (cloud_data->height);
// Nearest neighbor search
CloudXYZPtr xyz_model (new CloudXYZ ());
xyz_model->reserve (mesh_model->sizeVertices ());
for (unsigned int i=0; i<mesh_model->sizeVertices (); ++i)
{
const PointIHS& pt = mesh_model->getVertexDataCloud () [i];
xyz_model->push_back (PointXYZ (pt.x, pt.y, pt.z));
}
kd_tree_->setInputCloud (xyz_model);
std::vector <int> index (1);
std::vector <float> squared_distance (1);
mesh_model->reserveVertices (mesh_model->sizeVertices () + cloud_data->size ());
mesh_model->reserveEdges (mesh_model->sizeEdges () + (width-1) * height + width * (height-1) + (width-1) * (height-1));
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
CloudIHSPtr cloud_data_transformed (new CloudIHS ());
cloud_data_transformed->resize (cloud_data->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 indices)
VertexIndices vertex_indices (cloud_data->size (), VertexIndex ());
// Set the transformed 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_)
{
PointIHS& pt_d_t = cloud_data_transformed->operator [] (c);
pt_d_t = PointIHS (pt_d, weight);
pt_d_t.getVector4fMap () = T * pt_d_t.getVector4fMap ();
pt_d_t.getNormalVector4fMap () = T * pt_d_t.getNormalVector4fMap ();
}
}
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_)
{
PointIHS& pt_d_t = cloud_data_transformed->operator [] (r*width + c);
pt_d_t = PointIHS (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 //
const int offset_1 = -width;
const int offset_2 = -width - 1;
const int offset_3 = - 1;
const int offset_4 = -width - 2;
const float dot_min = std::cos (max_angle_ * 17.45329252e-3); // deg to rad
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_d_t_0 = cloud_data_transformed->operator [] (ind_0);
const PointIHS& pt_d_t_1 = cloud_data_transformed->operator [] (ind_1);
const PointIHS& pt_d_t_2 = cloud_data_transformed->operator [] (ind_2);
const PointIHS& pt_d_t_3 = cloud_data_transformed->operator [] (ind_3);
const PointIHS& pt_d_t_4 = cloud_data_transformed->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_d_t_0 = PointIHS (pt_d_0, weight);
pt_d_t_0.getVector4fMap () = T * pt_d_t_0.getVector4fMap ();
pt_d_t_0.getNormalVector4fMap () = T * pt_d_t_0.getNormalVector4fMap ();
pcl::PointXYZ tmp; tmp.getVector4fMap () = pt_d_t_0.getVector4fMap ();
// NN search
if (!kd_tree_->nearestKSearch (tmp, 1, index, squared_distance))
{
std::cerr << "ERROR in integration.cpp: nearestKSearch failed!\n";
return (false);
}
// Average out corresponding points
if (squared_distance [0] <= max_squared_distance_)
{
PointIHS& pt_m = mesh_model->getVertexDataCloud () [index [0]]; // Non-const reference!
if (pt_m.getNormalVector4fMap ().dot (pt_d_t_0.getNormalVector4fMap ()) >= dot_min)
{
vi_0 = VertexIndex (index [0]);
const float W = pt_m.weight; // Old accumulated weight
const float w = pt_d_t_0.weight; // Weight of new point
const float WW = pt_m.weight = W + w; // New accumulated weight
const float r_m = static_cast <float> (pt_m.r);
const float g_m = static_cast <float> (pt_m.g);
const float b_m = static_cast <float> (pt_m.b);
const float r_d = static_cast <float> (pt_d_t_0.r);
const float g_d = static_cast <float> (pt_d_t_0.g);
const float b_d = static_cast <float> (pt_d_t_0.b);
pt_m.getVector4fMap () = ( W*pt_m.getVector4fMap () + w*pt_d_t_0.getVector4fMap ()) / WW;
pt_m.getNormalVector4fMap () = ((W*pt_m.getNormalVector4fMap () + w*pt_d_t_0.getNormalVector4fMap ()) / WW).normalized ();
pt_m.r = this->trimRGB ((W*r_m + w*r_d) / WW);
pt_m.g = this->trimRGB ((W*g_m + w*g_d) / WW);
pt_m.b = this->trimRGB ((W*b_m + w*b_d) / WW);
// Point has been observed again -> give it some extra time to live
pt_m.age = 0;
// Add a direction
pcl::ihs::addDirection (pt_m.getNormalVector4fMap (), sensor_eye-pt_m.getVector4fMap (), pt_m.directions);
} // dot normals
} // squared distance
} // !isnan && min weight
// Connect
// 4 2 - 1 //
// | | //
// * 3 - 0 //
// //
// 4 - 2 1 //
// \ \ //
// * 3 - 0 //
this->addToMesh (pt_d_t_0,pt_d_t_1,pt_d_t_2,pt_d_t_3, vi_0,vi_1,vi_2,vi_3, mesh_model);
if (Mesh::IsManifold::value) // Only needed for the manifold mesh
{
this->addToMesh (pt_d_t_0,pt_d_t_2,pt_d_t_4,pt_d_t_3, vi_0,vi_2,vi_4,vi_3, mesh_model);
}
}
}
return (true);
}