Loading...
Searching...
No Matches
elch.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#ifndef PCL_REGISTRATION_IMPL_ELCH_H_
#define PCL_REGISTRATION_IMPL_ELCH_H_
#include <pcl/common/transforms.h>
#include <pcl/registration/registration.h>
#include <boost/graph/dijkstra_shortest_paths.hpp> // for dijkstra_shortest_paths
#include <algorithm>
#include <list>
#include <tuple>
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
pcl::registration::ELCH<PointT>::loopOptimizerAlgorithm(LOAGraph& g, double* weights)
{
std::list<int> crossings, branches;
crossings.push_back(static_cast<int>(loop_start_));
crossings.push_back(static_cast<int>(loop_end_));
weights[loop_start_] = 0;
weights[loop_end_] = 1;
int* p = new int[num_vertices(g)];
int* p_min = new int[num_vertices(g)];
double* d = new double[num_vertices(g)];
double* d_min = new double[num_vertices(g)];
bool do_swap = false;
std::list<int>::iterator start_min, end_min;
// process all junctions
while (!crossings.empty()) {
double dist = -1;
// find shortest crossing for all vertices on the loop
for (auto crossings_it = crossings.begin(); crossings_it != crossings.end();) {
dijkstra_shortest_paths(g,
*crossings_it,
predecessor_map(boost::make_iterator_property_map(
p, get(boost::vertex_index, g)))
.distance_map(boost::make_iterator_property_map(
d, get(boost::vertex_index, g))));
auto end_it = crossings_it;
end_it++;
// find shortest crossing for one vertex
for (; end_it != crossings.end(); end_it++) {
if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist)) {
dist = d[*end_it];
start_min = crossings_it;
end_min = end_it;
do_swap = true;
}
}
if (do_swap) {
std::swap(p, p_min);
std::swap(d, d_min);
do_swap = false;
}
// vertex starts a branch
if (dist < 0) {
branches.push_back(static_cast<int>(*crossings_it));
crossings_it = crossings.erase(crossings_it);
}
else
crossings_it++;
}
if (dist > -1) {
remove_edge(*end_min, p_min[*end_min], g);
for (int i = p_min[*end_min]; i != *start_min; i = p_min[i]) {
// even right with weights[*start_min] > weights[*end_min]! (math works)
weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) *
d_min[i] / d_min[*end_min];
remove_edge(i, p_min[i], g);
if (degree(i, g) > 0) {
crossings.push_back(i);
}
}
if (degree(*start_min, g) == 0)
crossings.erase(start_min);
if (degree(*end_min, g) == 0)
crossings.erase(end_min);
}
}
delete[] p;
delete[] p_min;
delete[] d;
delete[] d_min;
boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
// error propagation
while (!branches.empty()) {
int s = branches.front();
branches.pop_front();
for (std::tie(adjacent_it, adjacent_it_end) = adjacent_vertices(s, g);
adjacent_it != adjacent_it_end;
++adjacent_it) {
weights[*adjacent_it] = weights[s];
if (degree(*adjacent_it, g) > 1)
branches.push_back(static_cast<int>(*adjacent_it));
}
clear_vertex(s, g);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
bool
pcl::registration::ELCH<PointT>::initCompute()
{
/*if (!PCLBase<PointT>::initCompute ())
{
PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n");
return (false);
}*/ //TODO
if (loop_end_ == 0) {
PCL_ERROR("[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
deinitCompute();
return (false);
}
// compute transformation if it's not given
if (compute_loop_) {
PointCloudPtr meta_start(new PointCloud);
PointCloudPtr meta_end(new PointCloud);
*meta_start = *(*loop_graph_)[loop_start_].cloud;
*meta_end = *(*loop_graph_)[loop_end_].cloud;
typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
for (std::tie(si, si_end) = adjacent_vertices(loop_start_, *loop_graph_);
si != si_end;
si++)
*meta_start += *(*loop_graph_)[*si].cloud;
for (std::tie(si, si_end) = adjacent_vertices(loop_end_, *loop_graph_);
si != si_end;
si++)
*meta_end += *(*loop_graph_)[*si].cloud;
// TODO use real pose instead of centroid
// Eigen::Vector4f pose_start;
// pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);
// Eigen::Vector4f pose_end;
// pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);
PointCloudPtr tmp(new PointCloud);
// Eigen::Vector4f diff = pose_start - pose_end;
// Eigen::Translation3f translation (diff.head<3> ());
// Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
// pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);
reg_->setInputTarget(meta_start);
reg_->setInputSource(meta_end);
reg_->align(*tmp);
loop_transform_ = reg_->getFinalTransformation();
// TODO hack
// loop_transform_ *= trans.matrix ();
}
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void
pcl::registration::ELCH<PointT>::compute()
{
if (!initCompute()) {
return;
}
LOAGraph grb[4];
typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
for (std::tie(edge_it, edge_it_end) = edges(*loop_graph_); edge_it != edge_it_end;
edge_it++) {
for (auto& j : grb)
add_edge(source(*edge_it, *loop_graph_),
target(*edge_it, *loop_graph_),
1,
j); // TODO add variance
}
double* weights[4];
for (int i = 0; i < 4; i++) {
weights[i] = new double[num_vertices(*loop_graph_)];
loopOptimizerAlgorithm(grb[i], weights[i]);
}
// TODO use pose
// Eigen::Vector4f cend;
// pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend);
// Eigen::Translation3f tend (cend.head<3> ());
// Eigen::Affine3f aend (tend);
// Eigen::Affine3f aendI = aend.inverse ();
// TODO iterate ovr loop_graph_
// typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end;
// for (std::tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it !=
// vertex_it_end; vertex_it++)
for (std::size_t i = 0; i < num_vertices(*loop_graph_); i++) {
Eigen::Vector3f t2;
t2[0] = loop_transform_(0, 3) * static_cast<float>(weights[0][i]);
t2[1] = loop_transform_(1, 3) * static_cast<float>(weights[1][i]);
t2[2] = loop_transform_(2, 3) * static_cast<float>(weights[2][i]);
Eigen::Affine3f bl(loop_transform_);
Eigen::Quaternionf q(bl.rotation());
Eigen::Quaternionf q2;
q2 = Eigen::Quaternionf::Identity().slerp(static_cast<float>(weights[3][i]), q);
// TODO use rotation from branch start
Eigen::Translation3f t3(t2);
Eigen::Affine3f a(t3 * q2);
// a = aend * a * aendI;
pcl::transformPointCloud(*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a);
(*loop_graph_)[i].transform = a;
}
add_edge(loop_start_, loop_end_, *loop_graph_);
deinitCompute();
}
#endif // PCL_REGISTRATION_IMPL_ELCH_H_
pcl::PCLBase::deinitCompute
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition pcl_base.hpp:175
pcl::registration::ELCH::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition elch.h:65
pcl::registration::ELCH::PointCloud
pcl::PointCloud< PointT > PointCloud
Definition elch.h:64
pcl::registration::ELCH::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition elch.hpp:154
pcl::registration::ELCH::compute
void compute()
Computes new poses for all point clouds by closing the loop between start and end point cloud.
Definition elch.hpp:216
pcl::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
Definition transforms.hpp:221