ICPPlugin.cc

/*===========================================================================*\
*                                                                            *
*                              OpenFlipper                                   *
 *           Copyright (c) 2020, RWTH-Aachen University                      *
 *           Department of Computer Graphics and Multimedia                  *
 *                          All rights reserved.                             *
 *                            www.openflipper.org                            *
 *                                                                           *
 *---------------------------------------------------------------------------*
 * This file is part of OpenFlipper.                                         *
 *---------------------------------------------------------------------------*
 *                                                                           *
 * Redistribution and use in source and binary forms, with or without        *
 * modification, are permitted provided that the following conditions        *
 * are met:                                                                  *
 *                                                                           *
 * 1. Redistributions of source code must retain the above copyright notice, *
 *    this list of conditions and the following disclaimer.                  *
 *                                                                           *
 * 2. 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.   *
 *                                                                           *
 * 3. Neither the name of the copyright holder 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 HOLDER *
 * 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.              *
*                                                                            *
\*===========================================================================*/


#include "ICPPlugin.hh"
#include <QGridLayout>
#include <QPushButton>
#include <QLabel>
#include <QBitmap>

ICPPlugin::ICPPlugin() :
    tool_(nullptr),
    toolIcon_(nullptr)
{

}

ICPPlugin::~ICPPlugin()
{

}

void ICPPlugin::initializePlugin()
{
    if ( OpenFlipper::Options::gui() ) {
        tool_ = new ICPToolbarWidget();
        // connect signals->slots
        connect( tool_->btnICP,               SIGNAL(clicked()), this, SLOT(toolbarICP()) );

        QLabel* label = tool_->img_step_1;
        QPixmap pixmap(OpenFlipper::Options::iconDirStr()+OpenFlipper::Options::dirSeparator()+"icp_step1.png");
        label->setPixmap(pixmap);
        label->setMask(pixmap.mask());

        label->show();

        toolIcon_ = new QIcon(OpenFlipper::Options::iconDirStr()+OpenFlipper::Options::dirSeparator()+"ICPPluginIcon.png");
        emit addToolbox( tr("ICP") , tool_, toolIcon_ );
    }

}

void ICPPlugin::pluginsInitialized() {
    
    
}

static bool compare_ev(const Eigen::VectorXd& _lhs, const Eigen::VectorXd& _rhs) {
    return std::abs(_lhs(_lhs.rows()-1)) > std::abs(_rhs(_rhs.rows()-1));
}

static Eigen::MatrixXd sorted_cols_by_ev(Eigen::MatrixXd _A, Eigen::MatrixXd _EV)
{
    // add eigenvalues to column eigenvector
    _A.conservativeResize(_A.rows()+1, _A.cols());
    for (int64_t i=0;i<_A.cols();i++) {
        _A(_A.rows()-1, i) = _EV(i, 0);
    }
    std::vector<Eigen::VectorXd> vec;
    for (int64_t i = 0; i < _A.cols(); ++i)
        vec.push_back(_A.col(i));

    // sort eigenvectors by last component (eigenvalue)
    std::sort(vec.begin(), vec.end(), &compare_ev);

    for (unsigned int i = 0; i < _A.cols(); ++i)
        _A.col(i) = vec[i];

    return _A;
}

// from MovePlugin
template< typename MeshT >
static void transformMesh(ACG::Matrix4x4d _mat , MeshT* _mesh ) {
    // Get the inverse matrix of the transformation for the normals
    ACG::Matrix4x4d invTranspMat = _mat;

    // Build inverse transposed matrix of _mat
    invTranspMat.invert();
    invTranspMat.transpose();

    for ( auto v_it : _mesh->vertices() ) {

        // transform the mesh vertex
        _mesh->set_point(v_it,_mat.transform_point(_mesh->point(v_it)));

        // transform the vertex normal
        typename MeshT::Normal n = invTranspMat.transform_vector(_mesh->normal(v_it));

        n.normalize();

        _mesh->set_normal(v_it,n);
    }

    for ( auto f_it : _mesh->faces() ) {

        // transform the face normal
        typename MeshT::Normal n = invTranspMat.transform_vector(_mesh->normal(f_it));

        n.normalize();

        _mesh->set_normal(f_it,n);
    }
}

static void getMeshPoints(TriMesh* _mesh, ICPContext* _context, VertexList* _result) {
    unsigned long counter = 0;

    for ( auto v_it : _mesh->vertices() ) {
        counter++;
        if (counter % 10000 == 0) {
            if (_context->params_->debugOutput_)
                std::cout << counter << "/" << _mesh->n_vertices() << " mesh point copy" << std::endl << std::flush;
        }
        PolyMesh::VertexHandle point = v_it;

        _result->push_back(point);
    }
}

static void findAllNearestPoints(VertexList &_sourcePoints, ICPContext* _context, PointList* _result) {
    // for every source point
    for (VertexList::iterator it = _sourcePoints.begin() ; it != _sourcePoints.end() ; ++it) {
        PolyMesh::VertexHandle closest;
        double closestDistance = DBL_MAX;
        PolyMesh::Point referencePoint = _context->sourceMesh_->point(*it);


        // search clostest target point
        for (auto it2 : _context->targetMesh_->vertices()) {
            PolyMesh::Point delta = referencePoint - _context->targetMesh_->point(it2);
            double distance = delta[0]*delta[0] + delta[1]*delta[1] + delta[2]*delta[2];
            if (distance < 0) distance *= -1;
            if (distance < closestDistance) {
                // current point is smallest so far
                closestDistance = distance;
                closest = it2;
            }
        }

        // save point info
        PointNormalCollection info;
        info.point_ = _context->targetMesh_->point(closest);
        info.normal_ = _context->targetMesh_->normal(closest);
        _result->push_back(info);
    }
}

static void findAllNearestPointsBSP(VertexList &_sourcePoints, ICPContext* _context, PointList* _result) {
    if (_context->params_->debugOutput_)
        std::cout << "generate BSP ... " << std::flush;

    // generate bsp
    TriMeshObject::OMTriangleBSP* targetBSP = _context->targetObject_->requestTriangleBsp();

    if (_context->params_->debugOutput_)
        std::cout << "done. nearest point calculation" << std::endl << std::flush;

    unsigned long long counter = 0;

    // for every source point
    for (VertexList::iterator it = _sourcePoints.begin() ; it != _sourcePoints.end() ; ++it) {
        counter++;
        if (counter % 1000 == 0) {
            if (_context->params_->debugOutput_)
                std::cout << counter << "/" << _sourcePoints.size() << " nearest point calculation" << std::endl << std::flush;
        }

        // find nearest surface on target
        PolyMesh::Point point = _context->sourceMesh_->point(*it);
        TriMeshObject::OMTriangleBSP::NearestNeighbor nearest = targetBSP->nearest(point);
        PolyMesh::FaceHandle face = nearest.handle;
        PolyMesh::HalfedgeHandle he = _context->targetMesh_->halfedge_handle(face);
        PolyMesh::VertexHandle vh = _context->targetMesh_->to_vertex_handle(he);
        PolyMesh::Point pivot = _context->targetMesh_->point(vh);
        PolyMesh::Normal faceNormal = _context->targetMesh_->normal(face);

        // find nearest point on that surface
        PolyMesh::Point projected = ACG::Geometry::projectToPlane(pivot, faceNormal, point);

        PointNormalCollection info;
        info.point_ = projected;
        info.normal_ = faceNormal;

        // now calculate interpolated normal

        // find face's vertices
        PolyMesh::VertexHandle adjacentVH[3];
        auto faceIterator = _context->targetMesh_->fv_iter(face);
        bool error = false;
        if (faceIterator.is_valid()) { adjacentVH[0] = *faceIterator; faceIterator++; } else error = true;
        if (faceIterator.is_valid()) { adjacentVH[1] = *faceIterator; faceIterator++; } else error = true;
        if (faceIterator.is_valid()) { adjacentVH[2] = *faceIterator; faceIterator++; } else error = true;

        ACG::Vec3d projectedACG;

        projectedACG[0] = projected[0];
        projectedACG[1] = projected[1];
        projectedACG[2] = projected[2];

        if (!error && _context->params_->interpolateNormals_) {
            ACG::Vec3d adjacentV[3];
            for (int i=0;i<3;i++) {
                const PolyMesh::Point p1 = _context->targetMesh_->point(adjacentVH[i]);
                adjacentV[i][0] = p1[0];
                adjacentV[i][1] = p1[1];
                adjacentV[i][2] = p1[2];
            }

            ACG::Vec3d baryCoord;

            // calculate bary coords
            if (ACG::Geometry::baryCoord(projectedACG, adjacentV[0], adjacentV[1], adjacentV[2], baryCoord)) {

                // interpolate normals
                info.normal_ = (
                        baryCoord[0] * _context->targetMesh_->normal(adjacentVH[0]) +
                        baryCoord[1] * _context->targetMesh_->normal(adjacentVH[1]) +
                        baryCoord[2] * _context->targetMesh_->normal(adjacentVH[2])
                        ).normalized();

            } else {
                if (_context->params_->debugOutput_)
                    std::cout << "barycoord failed" << std::endl << std::flush;
            }
        } else {
            if (_context->params_->debugOutput_)
                std::cout << "circulator failed" << std::endl << std::flush;
        }

        _result->push_back(info);
    }
}

static double mse(VertexList &_pointsSource, PointList &_pointsTarget, ICPContext* _context) {
    double sum = 0;
    VertexList::iterator itSource = _pointsSource.begin();
    PointList::iterator itTarget = _pointsTarget.begin();

    // for every source point
    for (unsigned int i=0;i<_pointsSource.size();i++) {
        PolyMesh::VertexHandle source = *itSource;
        PolyMesh::Point target = itTarget->point_;
        PolyMesh::Point delta = target-_context->sourceMesh_->point(source);
        // calculate distance to target
        sum += delta[0]*delta[0] + delta[1]*delta[1] + delta[2]*delta[2];
        ++itSource; ++itTarget;
    }

    return sum / static_cast<double>(_pointsSource.size());
}

static PolyMesh::Point calculateCOG(VertexList& _points, TriMesh* _mesh) {
    PolyMesh::Point sum(0,0,0);
    for (VertexList::iterator it=_points.begin() ; it != _points.end() ; ++it) {
        sum += _mesh->point(*it);
    }
    return sum / static_cast<double>(_points.size());
}

static PolyMesh::Point calculateCOG(PointList& _points) {
    PolyMesh::Point sum(0,0,0);
    for (PointList::iterator it=_points.begin() ; it != _points.end() ; ++it) {
        sum += it->point_;
    }
    return sum / static_cast<double>(_points.size());
}

template < typename VectorT >
static VectorT calculateCOG(const std::vector<VectorT>& _points) {
    VectorT sum;
    for (unsigned int i=0;i<sum.dim();i++) sum[i] = 0;

    for (unsigned int i=0;i<_points.size();i++) {
        sum += _points[i];
    }

    return sum / static_cast<double>(_points.size());
}

static double calculateSize(VertexList* _points, TriMesh* _mesh, PolyMesh::Point &_origin) {
    double sum = 0;
    for (VertexList::iterator it=_points->begin() ; it != _points->end() ; ++it) {
        PolyMesh::Point p = _mesh->point(*it);
        PolyMesh::Point delta = p - _origin;

        sum += (delta[0]*delta[0] + delta[1]*delta[1] + delta[2]*delta[2]);
    }

    sum = std::sqrt(sum);

    sum /= static_cast<double>(_points->size());

    return sum;
}

static double calculateSize(PointList* _points, PolyMesh::Point &_origin) {
    double sum = 0;
    for (PointList::iterator it=_points->begin() ; it != _points->end() ; ++it) {
        PolyMesh::Point p = it->point_;
        PolyMesh::Point delta = p - _origin;

        sum += (delta[0]*delta[0] + delta[1]*delta[1] + delta[2]*delta[2]);
    }

    sum = std::sqrt(sum);

    sum /= static_cast<double>(_points->size());

    return sum;
}

static void deleteNonPairs(VertexList &_sourceP, PointList &_targetP, ICPContext* _context) {
    if (_context->params_->debugOutput_)
        std::cout << "delete non pairs" << std::endl << std::flush;

    VertexList::iterator itSource = _sourceP.begin();
    PointList::iterator itTarget = _targetP.begin();

    const double distanceThreshold = _context->params_->distanceThreshold_;
    const double distanceThreasholdSquared = distanceThreshold * distanceThreshold;

    const double normalDotThreshold = _context->params_->normalDotThreshold_ / 100.0;

    unsigned long rmDistance = 0;
    unsigned long rmNormal = 0;

    // for every source target pair
    while (itSource != _sourceP.end()) {
        PolyMesh::Point source = _context->sourceMesh_->point(*itSource);
        PolyMesh::Point target = itTarget->point_;

        PolyMesh::Normal sourceN = _context->sourceMesh_->normal(*itSource);
        PolyMesh::Normal targetN = itTarget->normal_;

        PolyMesh::Point delta = target - source;

        double distanceSquared = delta[0]*delta[0] + delta[1]*delta[1] + delta[2]*delta[2];
        double normalDot = sourceN[0]*targetN[0] + sourceN[1]*targetN[1] + sourceN[2]*targetN[2];

        // TODO rm test
        // if (normalDot < 0) normalDot *= -1.0;

        // check if distance differs
        if (distanceSquared > distanceThreasholdSquared) {
            // remove pair

            rmDistance++;

            _sourceP.erase(itSource++);
            _targetP.erase(itTarget++);

            // check if normal differs
        } else if ( normalDot < normalDotThreshold || normalDot < 0.0) {
            // remove pair

            rmNormal++;

            _sourceP.erase(itSource++);
            _targetP.erase(itTarget++);
        } else {
            // continue

            ++itSource;
            ++itTarget;
        }
    }

    if (_context->params_->debugOutput_)
        std::cout << "after deletion " << _sourceP.size() << " points left | rmDistance:" << rmDistance << ",rmNormal:" << rmNormal << std::endl << std::flush;
}

template < typename VectorT >
static Eigen::MatrixXd estimatedRigidTransform(const std::vector<VectorT>& _sourceCloud, const std::vector<VectorT>& _targetCloud) {
    assert(_sourceCloud.size() == _targetCloud.size());
    unsigned int dim = 3;
    {
        VectorT assertVector;
        dim = assertVector.dim();
        assert(dim == 3); // todo: check math for other dimensions
    }

    VectorT cogSource = calculateCOG(_sourceCloud);
    VectorT cogTarget = calculateCOG(_targetCloud);

    Eigen::MatrixXd centroid_A(dim,1);
    Eigen::MatrixXd centroid_B(dim,1);

    for (unsigned int i=0;i<dim;i++) {
        centroid_A(i,0) = cogSource[i];
        centroid_B(i,0) = cogTarget[i];
    }

    Eigen::MatrixXd Am(dim, _sourceCloud.size());
    Eigen::MatrixXd Bm(dim, _targetCloud.size());

    {
        for (unsigned int i=0;i<_sourceCloud.size();i++) {
            VectorT a = _sourceCloud[i];
            VectorT b = _targetCloud[i];

            for (unsigned int d=0 ; d<dim ; d++) {
                Am(d, i) = a[d] - cogSource[d];
                Bm(d, i) = b[d] - cogTarget[d];
            }
        }
    }

    Eigen::MatrixXd H = Am * Bm.transpose();
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);

    Eigen::MatrixXd R = svd.matrixV() * svd.matrixU().transpose();

    if (R.determinant() < 0) {
        Eigen::MatrixXd V = svd.matrixV();

        for (unsigned int i=0 ; i<dim ; i++) {
            V(i,dim-1) = V(i,dim-1) * -1.0;
        }

        R = V * svd.matrixU().transpose();
    }

    Eigen::MatrixXd t = -R * centroid_A + centroid_B;

    R.conservativeResize(R.rows()+1, R.cols()+1);

    for (unsigned int i=0;i<dim+1;i++) {
        R(dim, i) = i==dim ? 1 : 0;
        R(i, dim) = i==dim ? 1 : t(i, 0);
    }

    return R;
}

static ACG::GLMatrixd calculateRotationMatrix(VertexList &_sourceP, PointList &_targetP, ICPContext* _context) {
    assert(_sourceP.size() == _targetP.size());

    std::vector<ACG::Vec3d> sourceCloud(_sourceP.size()), targetCloud(_sourceP.size());
    VertexList::iterator iterA = _sourceP.begin();
    PointList::iterator iterB = _targetP.begin();

    for (unsigned int i=0;i<_sourceP.size();i++) {
        sourceCloud[i] = _context->sourceMesh_->point(*iterA);
        targetCloud[i] = iterB->point_;

        ++iterA;
        ++iterB;
    }

    Eigen::MatrixXd rotEigen = estimatedRigidTransform(sourceCloud, targetCloud);
    ACG::GLMatrixd rot;
    rot.identity();

    if (_context->params_->debugOutput_)
        std::cout << "Final rotation matrix:" << std::endl << rotEigen << std::endl << std::flush;

    for (unsigned int x=0;x<3;x++) {
        for (unsigned int y=0;y<3;y++) {
            rot(x,y) = rotEigen(x,y);
        }
    }

    return rot;
}

static void debugOutPairs(ICPContext* _context, VertexList& _source, PointList& _target) {
    if (!_context->params_->debugOutput_) return;

    VertexList::iterator sourceIt = _source.begin();
    PointList::iterator targetIt = _target.begin();

    for (;sourceIt != _source.end();) {

        PolyMesh::Point a = _context->sourceMesh_->point(*sourceIt);
        PolyMesh::Point b = targetIt->point_;
        double dist = (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]);
        dist = std::sqrt(dist);
        std::cout << a << " -> " << b << " distance: " << dist << std::endl << std::flush;

        ++sourceIt;
        ++targetIt;
    }
}

void ICPPlugin::doICP(ICPContext* _context) {
    const int maxIterations = _context->params_->maxIterations_;
    const double errorThreshold = _context->params_->errorThreshold_;

    double currentError = DBL_MAX;
    // loop CP while error is below threshold for max maxIterations
    int i = 0;
    for (i=0; (i<maxIterations && currentError > errorThreshold) || maxIterations == 0 ; i++) {
        VertexList meshPointsSource;
        getMeshPoints(_context->sourceMesh_, _context, &meshPointsSource);

        if (_context->params_->debugOutput_)
            std::cout << "ICP Iteration " << i+1 << "/" << maxIterations << " mse: " << ( (currentError<DBL_MAX-1e3) ? currentError : -1.0 ) << std::endl << std::flush;

        // determine for each point in S the closest point in T
        PointList closestsPointOnTarget;
        findAllNearestPointsBSP(meshPointsSource, _context, &closestsPointOnTarget);
        
        // delete posible non pairs
        deleteNonPairs(meshPointsSource, closestsPointOnTarget, _context);
        // debugOutPairs(_context, meshPointsSource, closestsPointOnTarget);

        if (meshPointsSource.size() <=6) {
            if (_context->params_->debugOutput_)
                std::cout << "ICP not enough points" << std::endl << std::flush;
            break;
        }

        {
            PolyMesh::Point cogSource = calculateCOG(meshPointsSource, _context->sourceMesh_);
            PolyMesh::Point cogTarget = calculateCOG(closestsPointOnTarget);

            double sizeSource = calculateSize(&meshPointsSource, _context->sourceMesh_, cogSource);
            double sizeTarget = calculateSize(&closestsPointOnTarget, cogTarget);

            double sizeAvg = (1.0/sizeSource+1.0/sizeTarget)/2.0;
            if (_context->params_->doResize_)
                _context->params_->distanceThreshold_ *= sizeAvg;

            if (_context->params_->debugOutput_)
                std::cout << "translate source by " << -cogSource << " and scale by " << 1.0/sizeSource << std::endl
                          << "translate target by " << -cogTarget << " and scale by " << 1.0/sizeTarget << std::endl << std::flush;

            ACG::GLMatrixd rotMat =  calculateRotationMatrix(meshPointsSource, closestsPointOnTarget, _context);

            if (maxIterations == 0) {
                break;
            }

            // rotate
            transformMesh(rotMat,
                          _context->sourceMesh_);

            cogSource = calculateCOG(meshPointsSource, _context->sourceMesh_);
            cogTarget = calculateCOG(closestsPointOnTarget);

            ACG::GLMatrixd transMat;
            transMat.identity();
            transMat.translate(cogTarget-cogSource);

            // translate
            transformMesh(transMat,
                          _context->sourceMesh_);

            emit updatedObject(_context->sourceID_, UPDATE_GEOMETRY);
        }

        // calculate MSE
        currentError = mse(meshPointsSource, closestsPointOnTarget, _context);
    }

    if (_context->params_->debugOutput_)
        std::cout << "Exited ICP with error: " << currentError << " after " << i << "/" << maxIterations << " iterations" << std::endl << std::flush;

    emit updatedObject(_context->sourceID_, UPDATE_GEOMETRY);
    emit updatedObject(_context->targetID_, UPDATE_GEOMETRY);
}

void ICPPlugin::icp(int _maxIterations, double _errorThreshold, double _distanceThreshold, double _normalDotThreshold, bool _debugOutput, bool _doResize, bool _interpolateNormals, int _sourceObjectID, int _targetObjectID) {
    // pack structs
    ICPParameters icpParams;
    icpParams.distanceThreshold_ = _distanceThreshold;
    icpParams.errorThreshold_ = _errorThreshold;
    icpParams.maxIterations_ = _maxIterations;
    icpParams.normalDotThreshold_ = _normalDotThreshold;
    icpParams.debugOutput_ = _debugOutput;
    icpParams.doResize_ = _doResize;
    icpParams.interpolateNormals_ = _interpolateNormals;

    ICPContext icpContext;
    icpContext.sourceMesh_ = PluginFunctions::triMesh(_sourceObjectID);
    icpContext.targetMesh_ = PluginFunctions::triMesh(_targetObjectID);
    icpContext.sourceObject_ = PluginFunctions::triMeshObject(_sourceObjectID);
    icpContext.targetObject_ = PluginFunctions::triMeshObject(_targetObjectID);
    icpContext.sourceID_ = _sourceObjectID;
    icpContext.targetID_ = _targetObjectID;
    icpContext.params_ = &icpParams;

    // execute icp
    doICP(&icpContext);
}

void ICPPlugin::toolbarICP() {
    if(OpenFlipper::Options::nogui()) {
        emit log(LOGERR, "no gui detected - use icp(...) instead");
        return;
    }

    // pack structs
    ICPParameters icpParams;
    icpParams.distanceThreshold_ = this->tool_->icp_distanceThreshold->value();
    icpParams.errorThreshold_ = this->tool_->icp_errorThreshold->value();
    icpParams.maxIterations_ = this->tool_->icp_maxIterations->value();
    icpParams.normalDotThreshold_ = this->tool_->icp_normalDotThreshold->value();
    icpParams.debugOutput_ = this->tool_->ck_debugOutput->isChecked();
    icpParams.doResize_ = this->tool_->ck_doResize->isChecked();
    icpParams.interpolateNormals_ = this->tool_->ck_interpolateNormals->isChecked();

    ICPContext icpContext;
    icpContext.sourceMesh_ = nullptr;
    icpContext.targetMesh_ = nullptr;
    icpContext.sourceObject_ = nullptr;
    icpContext.targetObject_ = nullptr;
    icpContext.sourceID_ = icpContext.targetID_ = -1;
    icpContext.params_ = &icpParams;

    // find target
    for (auto* o_it : PluginFunctions::objects(PluginFunctions::TARGET_OBJECTS) ) {
        if ( o_it->dataType( DATA_TRIANGLE_MESH ) ) {
            if (icpContext.targetMesh_ == nullptr) {
                icpContext.targetMesh_ = PluginFunctions::triMesh(o_it);
                icpContext.targetID_ = o_it->id();
                icpContext.targetObject_ = PluginFunctions::triMeshObject(o_it->id());
            } else {
                emit log(LOGERR, "Only one target is supported");
            }
        } else {
            emit log(LOGERR, "DataType not supported.");
        }
    }
    if (icpContext.targetMesh_ == nullptr) {
        emit log(LOGERR, "no target selected");
        return;
    }

    // for every source object execute icp source -> target
    for (auto* o_it : PluginFunctions::objects(PluginFunctions::SOURCE_OBJECTS) ) {
        if ( o_it->dataType( DATA_TRIANGLE_MESH ) ) {
            icpContext.sourceMesh_ = PluginFunctions::triMesh(o_it);
            icpContext.sourceID_ = o_it->id();
            icpContext.sourceObject_ = PluginFunctions::triMeshObject(o_it->id());

            doICP(&icpContext);
        } else {
            emit log(LOGERR, "DataType not supported.");
        }
    }
}