void QMapWidget::fillWithPclPointCloud(const pcl::PointCloud<PointT>& cloud, const QString &name)
{
PointCloud::Ptr mapCloud = getCloud(name);
mapCloud->points.clear();
for(const auto& p : cloud)
mapCloud->points.push_back(PointCloud::PointT(p.x,p.y,p.z));
cameraToClouds();
}
QList<GraphicNetCloud*> NetworkGraphics::getClouds(QList<NetNode*> nodes){
QList<GraphicNetCloud*> clouds;
for(int i=0; i<nodes.size(); i++){
clouds.push_back(getCloud(nodes[i]));
}
return clouds;
}
clams::Cloud::ConstPtr clams::StreamSequenceBase::operator[] (size_t idx) const
{
// With no cache, we just return the cloud
if (cache_size_ == 0)
return getCloud (idx);
// Otherwise we update cache -- not threadsafe
if (!pcds_cache_[idx])
{
#pragma omp critical
{
if (!pcds_cache_[idx])
{
clams::Cloud::ConstPtr cloud = getCloud (idx);
addCloudToCache (idx, cloud);
}
}
}
return pcds_cache_[idx];
}
PointCloud<PointNormal>::Ptr
Scanner::getCloudCached(float theta, float phi, Model &model)
{
Scan scan = { theta, phi };
std::stringstream ss;
ss << "scan_" << model.id << "_" << deg(scan.theta) << "_" << deg(scan.phi) << ".pcd";
std::string name = ss.str();
if (ifstream(name.c_str())) {
PointCloud<PointNormal>::Ptr cloud (new PointCloud<PointNormal>());
io::loadPCDFile(name, *cloud);
return cloud;
} else {
PointCloud<PointNormal>::Ptr cloud = getCloud(scan, model);
io::savePCDFileBinary(name, *cloud);
return cloud;
}
}
//--------------------------------------------------------------
void ArmCalibration::storeCloud()
{
PCPtr cloud = getCloud(4);
storedClouds.push_back(MotorRotationsCloud(arduino.motorAngles(), cloud));
}
void cData::deleteCloud(int idCloud)
{
GlCloud* cloud = getCloud(idCloud);
if(cloud)
delete cloud;
}
void
v4r::Registration::MultiSessionModelling<PointT>::compute()
{
for(size_t a=0; a < reg_algos_.size(); a++)
{
reg_algos_[a]->setMSM(this);
reg_algos_[a]->initialize(session_ranges_);
}
std::vector< std::vector< std::vector<EdgeBetweenPartialModels> > > edges;
edges.resize(session_ranges_.size());
for(size_t i=0; i < session_ranges_.size(); i++)
{
edges[i].resize(session_ranges_.size());
}
//for each session pair, call the class that registers two partial models and returns a set of poses
for(size_t i=0; i < session_ranges_.size(); i++)
{
std::pair<int, int> pair_i = session_ranges_[i];
for(size_t j=(i+1); j < session_ranges_.size(); j++)
{
std::pair<int, int> pair_j = session_ranges_[j];
for(size_t a=0; a < reg_algos_.size(); a++)
{
reg_algos_[a]->setSessions(pair_i, pair_j);
reg_algos_[a]->compute(i,j);
//poses transform the RF of pair_j to the RF of pair_i
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > poses;
reg_algos_[a]->getPoses(poses);
std::cout << "poses between " << j << " and " << i << " :" << poses.size() << std::endl;
for(size_t p=0; p < poses.size(); p++)
{
EdgeBetweenPartialModels ebpm;
ebpm.transformation_ = poses[p];
ebpm.cost_ = -1;
ebpm.i_ = i;
ebpm.j_ = j;
edges[i][j].push_back(ebpm);
}
}
}
}
//for each edge in the graph, compute a score quantifying quality of alignment
//overlap? number of outliers? fsv average?
for(size_t i=0; i < session_ranges_.size(); i++)
{
for(size_t j=(i+1); j < session_ranges_.size(); j++)
{
if(edges[i][j].size() > 0)
{
std::cout << "Number of edges:" << edges[i][j].size() << std::endl;
//iterate over the edges and compute cost_ (the higher, the worse)
#pragma omp parallel for schedule(dynamic, 1) num_threads(4)
for(size_t k=0; k < edges[i][j].size(); k++)
{
computeCost(edges[i][j][k]);
}
}
}
}
//final alignment between partial models? MST
int final_alignment = 0; //0-MST, 1-something fancy?
switch(final_alignment)
{
case 0:
{
std::vector<typename pcl::PointCloud<PointT>::Ptr> partial_model_clouds(session_ranges_.size());
for(size_t i=0; i < session_ranges_.size(); i++)
{
partial_model_clouds[i].reset(new pcl::PointCloud<PointT>);
for(int t=session_ranges_[i].first; t <= session_ranges_[i].second; t++)
{
typename pcl::PointCloud<PointT>::Ptr transformed(new pcl::PointCloud<PointT>);
pcl::copyPointCloud(*getCloud(static_cast<size_t>(t)), getIndices(static_cast<size_t>(t)), *transformed);
Eigen::Matrix4f pose_inv = getPose(static_cast<size_t>(t));
pcl::transformPointCloud(*transformed, *transformed, pose_inv);
*partial_model_clouds[i] += *transformed;
}
}
Graph G;
for(size_t i=0; i < session_ranges_.size(); i++)
{
boost::add_vertex((int)i, G);
}
for(size_t i=0; i < session_ranges_.size(); i++)
{
for(size_t j=(i+1); j < session_ranges_.size(); j++)
{
if(edges[i][j].size() > 0)
{
float min_cost = std::numeric_limits<float>::infinity();
int min_k = -1;
for(size_t k=0; k < edges[i][j].size(); k++)
{
//std::cout << "cost:" << edges[i][j][k].cost_ << std::endl;
if(edges[i][j][k].cost_ < min_cost)
{
min_cost = edges[i][j][k].cost_;
min_k = static_cast<int>(k);
}
}
std::cout << j << " => " << i << " " << min_cost << " " << min_k << std::endl;
//add edge to graph... transformation maps from j to i (otherwise invert)
myEdge e;
e.edge_weight = edges[i][j][min_k].cost_;
e.transformation = edges[i][j][min_k].transformation_;
e.source_id = j;
e.target_id = i;
boost::add_edge ((int)j, (int)i, e, G);
/*std::stringstream title_str;
title_str << "best pw from " << j << " " << i;
pcl::visualization::PCLVisualizer vis(title_str.str().c_str());
vis.addCoordinateSystem(0.1);
for(size_t k=0; k < edges[i][j].size(); k++)
{
if(edges[i][j][k].cost_ * 0.25 <= min_cost)
{
std::cout << "cost:" << edges[i][j][k].cost_ << std::endl;
{
pcl::visualization::PointCloudColorHandlerRGBField<PointT> handler(partial_model_clouds[i]);
vis.addPointCloud(partial_model_clouds[i], handler, "cloud_i");
}
typename pcl::PointCloud<PointT>::Ptr transformed(new pcl::PointCloud<PointT>);
pcl::transformPointCloud(*partial_model_clouds[j], *transformed, edges[i][j][k].transformation_);
{
pcl::visualization::PointCloudColorHandlerRGBField<PointT> handler(transformed);
vis.addPointCloud(transformed, handler, "cloud_j");
}
vis.spin();
vis.removeAllPointClouds();
}
}*/
}
}
}
boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, G);
Graph MST;
for(size_t i=0; i < session_ranges_.size(); i++)
boost::add_vertex((int)i, MST);
//there is a bug in boost 1.54 that causes negative weights error (even though weights are positive)
/*std::vector < boost::graph_traits<Graph>::vertex_descriptor > p (boost::num_vertices (G));
boost::prim_minimum_spanning_tree (G, &p[0]);
for (std::size_t i = 0; i != p.size (); ++i)
{
if (p[i] != i)
std::cout << "parent[" << i << "] = " << p[i] << std::endl;
else
std::cout << "parent[" << i << "] = no parent" << std::endl;
}
typedef typename Graph::edge_iterator EdgeIterator;
std::pair<EdgeIterator, EdgeIterator> edges = boost::edges(G);
EdgeIterator edge;
for (edge = edges.first; edge != edges.second; edge++)
{
typename boost::graph_traits<Graph>::vertex_descriptor s, t;
s = boost::source(*edge, G);
t = boost::target(*edge, G);
if(p[s] == t || p[t] == s)
{
//edge in prim
boost::add_edge ((int)s, (int)t, weightmap[*edge], MST);
}
}*/
std::vector < Edge > spanning_tree;
boost::kruskal_minimum_spanning_tree(G, std::back_inserter(spanning_tree));
std::cout << "Print the edges in the MST:" << std::endl;
for (std::vector < Edge >::iterator ei = spanning_tree.begin(); ei != spanning_tree.end(); ++ei)
{
std::cout << source(*ei, G) << " " << target(*ei, G) << std::endl;
boost::add_edge(source(*ei, G), target(*ei, G), weightmap[*ei], MST);
}
std::cout << boost::num_edges(MST) << " " << boost::num_vertices(MST) << std::endl;
output_session_poses_.resize(boost::num_vertices(MST));
computeAbsolutePoses(MST, output_session_poses_);
//finally, define output_cloud_poses_
output_cloud_poses_.resize(clouds_.size());
for(size_t i=0; i < output_session_poses_.size(); i++)
{
std::cout << output_session_poses_[i] << std::endl;
for(int j=session_ranges_[i].first; j <= session_ranges_[i].second; j++)
{
Eigen::Matrix4f trans = output_session_poses_[i] * poses_[j];
output_cloud_poses_[j] = trans;
}
}
break;
}
default:
{
break;
}
}
}
virtual void process(){
// do something with cloud
PointCloud::Ptr fc = getCloud();
//cloud = statisticalFilter( fc );
cloud = makeVoxelGrid(fc);
}
