bool
pcl::ihs::OfflineIntegration::load (const std::string& filename,
CloudXYZRGBNormalPtr& cloud,
Eigen::Matrix4f& T) const
{
if (!cloud)
{
cloud = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ());
}
// Load the cloud.
CloudXYZRGBAPtr cloud_input (new CloudXYZRGBA ());
pcl::PCDReader reader;
if (reader.read (filename, *cloud_input) < 0)
{
std::cerr << "ERROR in offline_integration.cpp: Could not read the pcd file.\n";
return (false);
}
// Normal estimation.
normal_estimation_->setInputCloud (cloud_input);
normal_estimation_->compute (*cloud);
pcl::copyPointCloud (*cloud_input, *cloud);
// Change the file extension of the file.
// Load the transformation.
std::string fn_transform = filename;
size_t pos = fn_transform.find_last_of (".");
if (pos == std::string::npos || pos == (fn_transform.size () - 1))
{
std::cerr << "ERROR in offline_integration.cpp: No file extension\n";
return (false);
}
fn_transform.replace (pos, std::string::npos, ".transform");
if (!this->loadTransform (fn_transform, T))
{
std::cerr << "ERROR in offline_integration.cpp: Could not load the transformation.\n";
return (false);
}
return (true);
}
void
pcl::ihs::InHandScanner::newDataCallback (const CloudXYZRGBAConstPtr& cloud_in)
{
Base::calcFPS (computation_fps_); // Must come before the lock!
boost::mutex::scoped_lock lock (mutex_);
if (destructor_called_) return;
pcl::StopWatch sw;
// Input data processing
CloudXYZRGBNormalPtr cloud_data;
CloudXYZRGBNormalPtr cloud_discarded;
if (running_mode_ == RM_SHOW_MODEL)
{
cloud_data = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ());
}
else if (running_mode_ == RM_UNPROCESSED)
{
if (!input_data_processing_->calculateNormals (cloud_in, cloud_data))
return;
}
else if (running_mode_ >= RM_PROCESSED)
{
if (!input_data_processing_->segment (cloud_in, cloud_data, cloud_discarded))
return;
}
double time_input_data_processing = sw.getTime ();
// Registration & integration
if (running_mode_ >= RM_REGISTRATION_CONT)
{
std::cerr << "\nGlobal iteration " << iteration_ << "\n";
std::cerr << "Input data processing:\n"
<< " - time : "
<< std::setw (8) << std::right << time_input_data_processing << " ms\n";
if (iteration_ == 0)
{
transformation_ = Eigen::Matrix4f::Identity ();
sw.reset ();
integration_->reconstructMesh (cloud_data, mesh_model_);
std::cerr << "Integration:\n"
<< " - time reconstruct mesh : "
<< std::setw (8) << std::right << sw.getTime () << " ms\n";
cloud_data = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ());
++iteration_;
}
else
{
Eigen::Matrix4f T_final = Eigen::Matrix4f::Identity ();
if (icp_->findTransformation (mesh_model_, cloud_data, transformation_, T_final))
{
transformation_ = T_final;
sw.reset ();
integration_->merge (cloud_data, mesh_model_, transformation_);
std::cerr << "Integration:\n"
<< " - time merge : "
<< std::setw (8) << std::right << sw.getTime () << " ms\n";
sw.reset ();
integration_->age (mesh_model_);
std::cerr << " - time age : "
<< std::setw (8) << std::right << sw.getTime () << " ms\n";
sw.reset ();
std::vector <Mesh::HalfEdgeIndices> boundary_collection;
pcl::geometry::getBoundBoundaryHalfEdges (*mesh_model_, boundary_collection, 1000);
std::cerr << " - time compute boundary : "
<< std::setw (8) << std::right << sw.getTime () << " ms\n";
sw.reset ();
mesh_processing_->processBoundary (*mesh_model_, boundary_collection);
std::cerr << " - time mesh processing : "
<< std::setw (8) << std::right << sw.getTime () << " ms\n";
cloud_data = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ());
++iteration_;
}
}
}
// Visualization & copy back some variables
double time_model = 0;
double time_data = 0;
if (mesh_model_->empty ()) Base::setPivot ("data");
else Base::setPivot ("model");
sw.reset ();
Base::addMesh (mesh_model_, "model", Eigen::Isometry3d (transformation_.inverse ().cast <double> ()));
time_model = sw.getTime ();
sw.reset ();
Base::addMesh (cloud_data , "data"); // Converts to a mesh for visualization
if (running_mode_ < RM_REGISTRATION_CONT && cloud_discarded)
{
Base::addMesh (cloud_discarded, "cloud_discarded");
}
else
{
Base::removeMesh ("cloud_discarded");
}
time_data = sw.getTime ();
if (running_mode_ >= RM_REGISTRATION_CONT)
{
std::cerr << "Copy to visualization thread:\n"
<< " - time model : "
<< std::setw (8) << std::right << time_model << " ms\n"
<< " - time data : "
<< std::setw (8) << std::right << time_data << " ms\n";
}
if (running_mode_ == RM_REGISTRATION_SINGLE)
{
lock.unlock ();
this->showProcessedData ();
}
}
