PoseInterpolator.cpp

Go to the documentation of this file.

#include "PoseInterpolator.hpp"
#include "../utility/TableHelper.hpp"
#include "../utility/GeometryHelper.hpp"

#include <fstream>
#include <set>

namespace ISM
{
void PoseInterpolator::interpolate(const std::string& sourceFile, const std::string& targetFile, int stepNumber)
{
    if (stepNumber < 1)
    {
        std::cerr << "stepNumber is < 1!" << std::endl;
        exit(0);
    }

    // copy source to target, so that only the copy will be modified
    std::ifstream source(sourceFile, std::fstream::binary);
    std::ofstream target(targetFile, std::fstream::trunc|std::fstream::binary);
    target << source.rdbuf();

    TableHelper sourceHelper(sourceFile);
    TableHelper targetHelper(targetFile);
    // drop tables to write the sets in the right order
    targetHelper.dropTable("recorded_objects");
    targetHelper.dropTable("recorded_sets");
    targetHelper.createTablesIfNecessary();

    // assume we have only one pattern (scene)
    std::string pattern = sourceHelper.getRecordedPatternNames().at(0);
    // counter begins with 1 because databases IDs begin also with 1
    int counter = 1;
    ObjectSetPtr from = sourceHelper.getRecordedObjectSet(counter);
    ObjectSetPtr to = sourceHelper.getRecordedObjectSet(counter + 1);
    while (from->objects.size() != 0 && to->objects.size() != 0)
    {
        std::cout << "Interpolating between object sets " << counter << " and " << counter + 1 << "..." << std::endl;
        targetHelper.insertRecordedObjectSet(from, pattern);
        for (ObjectSetPtr objectSet : interpolateSets(from, to, stepNumber))
        {
            targetHelper.insertRecordedObjectSet(objectSet, pattern);
        }
        ++counter;
        from = sourceHelper.getRecordedObjectSet(counter);
        to = sourceHelper.getRecordedObjectSet(counter + 1);
    }
    // insert the last ObjectSet
    targetHelper.insertRecordedObjectSet(from, pattern);
    std::cout << "Finished!" << std::endl;
}

std::vector<PosePtr> PoseInterpolator::interpolatePoses(PosePtr from, PosePtr to, int stepNumber)
{
    std::vector<PosePtr> result;
    int divisor = stepNumber + 1;
    Eigen::Vector3d fromPosition = GeometryHelper::pointToVector(from->point);
    Eigen::Vector3d toPosition = GeometryHelper::pointToVector(to->point);
    Eigen::Quaterniond fromOrient = GeometryHelper::quatToEigenQuat(from->quat);
    Eigen::Quaterniond toOrient = GeometryHelper::quatToEigenQuat(to->quat);
    Eigen::Vector3d interpolatedPosition;
    Eigen::Quaterniond interpolatedQuat;
    for (int i = 1; i <= stepNumber; i++)
    {
        interpolatedPosition = fromPosition * (divisor - i)/divisor + toPosition * i/divisor;
        interpolatedQuat = fromOrient.slerp((double)i/divisor, toOrient);
        result.push_back(PosePtr(new Pose(GeometryHelper::vectorToPoint(interpolatedPosition), GeometryHelper::eigenQuatToQuat(interpolatedQuat))));
    }

    return result;
}

std::vector<ObjectSetPtr> PoseInterpolator::interpolateSets(const ObjectSetPtr from, const ObjectSetPtr to, const int stepNumber)
{
    std::vector<ObjectSetPtr> result;
    // prepare sets in the vector
    for (int i = 0; i < stepNumber; i++)
    {
        result.push_back(ObjectSetPtr(new ObjectSet()));
    }
    std::vector<PosePtr> interpolatedPoses;
    PosePtr pose;
    ObjectPtr interpolatedObject;
    for (unsigned i = 0; i < from->objects.size(); i++)
    {
        interpolatedPoses = interpolatePoses(from->objects.at(i)->pose, to->objects.at(i)->pose, stepNumber);
        for (unsigned j = 0; j < interpolatedPoses.size(); j++)
        {
            pose = interpolatedPoses.at(j);
            interpolatedObject = ObjectPtr(new Object(*(from->objects.at(i).get()))); // copy objectPtr
            interpolatedObject->pose = pose;
            result.at(j)->insert(interpolatedObject);
        }
    }

    return result;
}

}