277 lines
9.2 KiB
C++
277 lines
9.2 KiB
C++
/*
|
|
* 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_
|