TorusPrimitiveShape.cpp

Go to the documentation of this file.

#include "TorusPrimitiveShape.h"
 
#include <sstream>
 
#include "ConePrimitiveShape.h"
#include "CylinderPrimitiveShape.h"
#include "PlanePrimitiveShape.h"
#include "PrimitiveShapeVisitor.h"
#include "SpherePrimitiveShape.h"
#include <MiscLib/Performance.h>
extern MiscLib::performance_t totalTime_torusConnected;
 
TorusPrimitiveShape::TorusPrimitiveShape(const Torus& torus) :
    m_torus(torus), m_parametrization(m_torus)
{
}
 
TorusPrimitiveShape::TorusPrimitiveShape(const TorusPrimitiveShape& tps) :
    BitmapPrimitiveShape(tps), m_torus(tps.m_torus), m_parametrization(tps.m_parametrization)
{
    m_parametrization.Shape(m_torus);
}
 
size_t
TorusPrimitiveShape::Identifier() const
{
    return 4;
}
 
PrimitiveShape*
TorusPrimitiveShape::Clone() const
{
    return new TorusPrimitiveShape(*this);
}
 
float
TorusPrimitiveShape::Distance(const Vec3f& p) const
{
    return m_torus.Distance(p);
}
 
float
TorusPrimitiveShape::SignedDistance(const Vec3f& p) const
{
    return m_torus.SignedDistance(p);
}
 
float
TorusPrimitiveShape::NormalDeviation(const Vec3f& p, const Vec3f& n) const
{
    Vec3f normal;
    m_torus.Normal(p, &normal);
    return n.dot(normal);
}
 
void
TorusPrimitiveShape::DistanceAndNormalDeviation(const Vec3f& p,
                                                const Vec3f& n,
                                                std::pair<float, float>* dn) const
{
    Vec3f normal;
    dn->first = m_torus.DistanceAndNormal(p, &normal);
    dn->second = n.dot(normal);
}
 
void
TorusPrimitiveShape::Project(const Vec3f& p, Vec3f* pp) const
{
    m_torus.Project(p, pp);
}
 
void
TorusPrimitiveShape::Normal(const Vec3f& p, Vec3f* n) const
{
    m_torus.Normal(p, n);
}
 
unsigned int
TorusPrimitiveShape::ConfidenceTests(unsigned int numTests,
                                     float epsilon,
                                     float normalThresh,
                                     float rms,
                                     const PointCloud& pc,
                                     const MiscLib::Vector<size_t>& indices) const
{
    return BasePrimitiveShape::ConfidenceTests<Torus>(
        numTests, epsilon, normalThresh, rms, pc, indices);
}
 
void
TorusPrimitiveShape::Description(std::string* s) const
{
    std::ostringstream ostr;
    ostr << "Torus (minor=" << m_torus.MinorRadius() << " major=" << m_torus.MajorRadius() << ")";
    *s = ostr.str();
    //  *s = "Torus";
}
 
bool
TorusPrimitiveShape::Fit(const PointCloud& pc,
                         float epsilon,
                         float normalThresh,
                         MiscLib::Vector<size_t>::const_iterator begin,
                         MiscLib::Vector<size_t>::const_iterator end)
{
    Torus fit = m_torus;
    if (fit.LeastSquaresFit(pc, begin, end))
    {
        m_torus = fit;
        m_parametrization.Shape(m_torus);
        return true;
    }
    return false;
}
 
PrimitiveShape*
TorusPrimitiveShape::LSFit(const PointCloud& pc,
                           float epsilon,
                           float normalThresh,
                           MiscLib::Vector<size_t>::const_iterator begin,
                           MiscLib::Vector<size_t>::const_iterator end,
                           std::pair<size_t, float>* score) const
{
    Torus fit = m_torus;
    if (fit.LeastSquaresFit(pc, begin, end))
    {
        score->first = -1;
        return new TorusPrimitiveShape(fit);
    }
    score->first = 0;
    return NULL;
}
 
LevMarFunc<float>*
TorusPrimitiveShape::SignedDistanceFunc() const
{
    return new TorusLevMarFunc(m_torus);
}
 
void
TorusPrimitiveShape::Serialize(std::ostream* o, bool binary) const
{
    if (binary)
    {
        const char id = 4;
        (*o) << id;
    }
    else
    {
        (*o) << "4"
             << " ";
    }
    m_torus.Serialize(binary, o);
    m_parametrization.Serialize(o, binary);
    if (!binary)
    {
        *o << std::endl;
    }
}
 
void
TorusPrimitiveShape::Deserialize(std::istream* i, bool binary)
{
    m_torus.Init(binary, i);
    m_parametrization.Shape(m_torus);
    m_parametrization.Deserialize(i, binary);
}
 
size_t
TorusPrimitiveShape::SerializedSize() const
{
    return m_torus.SerializedSize() + m_parametrization.SerializedSize() + 1;
}
 
void
TorusPrimitiveShape::Transform(float scale, const Vec3f& translate)
{
    m_torus.Transform(scale, translate);
}
 
void
TorusPrimitiveShape::Visit(PrimitiveShapeVisitor* visitor) const
{
    visitor->Visit(*this);
}
 
void
TorusPrimitiveShape::SuggestSimplifications(
    const PointCloud& pc,
    MiscLib::Vector<size_t>::const_iterator begin,
    MiscLib::Vector<size_t>::const_iterator end,
    float distThresh,
    MiscLib::Vector<MiscLib::RefCountPtr<PrimitiveShape>>* suggestions) const
{
    // sample the bounding box in parameter space at 25 locations
    // these points are used to estimate the other shapes
    // if the shapes succeed the suggestion is returned
    MiscLib::Vector<Vec3f> samples(2 * 25);
    float uStep = (m_extBbox.Max()[0] - m_extBbox.Min()[0]) / 4;
    float vStep = (m_extBbox.Max()[1] - m_extBbox.Min()[1]) / 4;
    float u = m_extBbox.Min()[0];
    for (unsigned int i = 0; i < 5; ++i, u += uStep)
    {
        float v = m_extBbox.Min()[1];
        for (unsigned int j = 0; j < 5; ++j, v += vStep)
        {
            float bmpu;
            if (m_torus.MajorRadius() < m_torus.MinorRadius() * 2)
            {
                bmpu = u * (m_torus.MajorRadius() + m_torus.MinorRadius());
            }
            else
            {
                bmpu = u * m_torus.MajorRadius();
            }
            InSpace(bmpu, v * m_torus.MinorRadius(), &samples[i * 5 + j], &samples[i * 5 + j + 25]);
        }
    }
    size_t c = samples.size() / 2;
    // now check all the shape types
    Cone cone;
    if (cone.InitAverage(samples))
    {
        cone.LeastSquaresFit(samples.begin(), samples.begin() + c);
        bool failed = false;
        for (size_t i = 0; i < c; ++i)
            if (cone.Distance(samples[i]) > distThresh)
            {
                failed = true;
                break;
            }
        if (!failed)
        {
            suggestions->push_back(new ConePrimitiveShape(cone));
            suggestions->back()->Release();
        }
    }
    Cylinder cylinder;
    if (cylinder.InitAverage(samples))
    {
        cylinder.LeastSquaresFit(samples.begin(), samples.begin() + c);
        bool failed = false;
        for (size_t i = 0; i < c; ++i)
            if (cylinder.Distance(samples[i]) > distThresh)
            {
                failed = true;
                break;
            }
        if (!failed)
        {
            suggestions->push_back(new CylinderPrimitiveShape(cylinder));
            suggestions->back()->Release();
        }
    }
    Sphere sphere;
    if (sphere.Init(samples))
    {
        sphere.LeastSquaresFit(samples.begin(), samples.begin() + c);
        bool failed = false;
        for (size_t i = 0; i < c; ++i)
            if (sphere.Distance(samples[i]) > distThresh)
            {
                failed = true;
                break;
            }
        if (!failed)
        {
            suggestions->push_back(new SpherePrimitiveShape(sphere));
            suggestions->back()->Release();
        }
    }
    Plane plane;
    if (plane.LeastSquaresFit(samples.begin(), samples.begin() + c))
    {
        bool failed = false;
        for (size_t i = 0; i < c; ++i)
            if (plane.Distance(samples[i]) > distThresh)
            {
                failed = true;
                break;
            }
        if (!failed)
        {
            suggestions->push_back(new PlanePrimitiveShape(plane));
            suggestions->back()->Release();
        }
    }
    /*// although theoretically possible, we never suggest a cone since a misclassification
    // of a cone as a torus is extremley seldom
    // The parametrization is given as major arclength and minor arclength
    float radialMajor = m_extBbox.Max()[0] - m_extBbox.Min()[0];
    float radialMinor = m_extBbox.Max()[1] - m_extBbox.Min()[1];
    float lengthMajor = radialMajor * m_torus.MajorRadius();
    float lengthMinor = radialMinor * m_torus.MinorRadius();
    float meanRadius = (m_torus.MajorRadius() + m_torus.MinorRadius()) / 2;
    // suggest a cylinder if either of the two radii can be replaced by
    // a non curved direction
    // we suggest a cylinder if the major radius causes an error less than distThresh
    // this tests if the major radius can be replaced
    float radiusDiffMajor = (m_torus.MajorRadius() - (std::cos(radialMajor / 2)
        * m_torus.MajorRadius())) / 2;
    if(radiusDiffMajor < distThresh)
    {
        // construct the cylinder
        // the axis of the cylinder is given
        float majorCenter = (m_extBbox.Max()[0] - m_extBbox.Min()[0]) / 2
            * m_torus.MajorRadius();
        Vec3f pos, normal, cyAxisDir;
        InSpace(majorCenter, M_PI * m_torus.MinorRadius(), &pos, &normal);
        cyAxisDir = normal.cross(m_torus.AxisDirection());
        cyAxisDir.normalize();
        pos -= (m_torus.MinorRadius() - radiusDiffMajor) * normal;
        Cylinder cylinder(cyAxisDir, pos, m_torus.MinorRadius());
        suggestions->push_back(new CylinderPrimitiveShape(cylinder));
        suggestions->back()->Release();
    }
    // now test if the minor radius can be replaced
    float radiusDiffMinor = (m_torus.MinorRadius() - (std::cos(radialMinor / 2)
        * m_torus.MinorRadius())) / 2;
    if(radiusDiffMinor < distThresh)
    {
        // if the minor radius is replaced
    }
    // we suggest a sphere if the torus is apple shaped
    if(m_torus.IsAppleShaped())
    {
        Sphere sphere(m_torus.Center(),
            (m_torus.MajorRadius() + m_torus.MinorRadius()) / 2);
        suggestions->push_back(new SpherePrimitiveShape(sphere));
        suggestions->back()->Release();
    }
    // we can also suggest a sphere if the error introduced by a common radius
    // for minor and major does not introduce an error
    //else if()
    //{
    //}
    // we suggest a plane if both major and minor radius cause ony small error
    float radiusDiffMinor = (m_torus.MinorRadius() - (std::cos(radialMinor / 2)
        * m_torus.MinorRadius())) / 2;
    if(radiusDiffMajor < distThresh && radiusDiffMinor < distThresh)
    {
        GfxTL::Vector2Df paramCenter;
        m_extBbox.Center(&paramCenter);
        Vec3f pos, normal;
        InSpace(paramCenter[0] * m_torus.MajorRadius(),
            paramCenter[1] * m_torus.MinorRadius(), &pos, &normal);
        Plane plane(pos, normal);
        suggestions->push_back(new PlanePrimitiveShape(plane));
        suggestions->back()->Release();
    }*/
}
 
void
TorusPrimitiveShape::OptimizeParametrization(const PointCloud& pc,
                                             size_t begin,
                                             size_t end,
                                             float epsilon)
{
    m_parametrization.Optimize(GfxTL::IndexIterate(IndexIterator(begin), pc.begin()),
                               GfxTL::IndexIterate(IndexIterator(end), pc.begin()),
                               epsilon);
}
 
bool
TorusPrimitiveShape::Similar(float tolerance, const TorusPrimitiveShape& shape) const
{
    return m_torus.MajorRadius() <= (1.f + tolerance) * shape.m_torus.MajorRadius() &&
           (1.f + tolerance) * m_torus.MajorRadius() >= shape.m_torus.MajorRadius() &&
           m_torus.MinorRadius() <= (1.f + tolerance) * shape.m_torus.MinorRadius() &&
           (1.f + tolerance) * m_torus.MinorRadius() >= shape.m_torus.MinorRadius();
}
 
void
TorusPrimitiveShape::Parameters(const Vec3f& p, std::pair<float, float>* param) const
{
    m_parametrization.Parameters(p, param);
}
 
void
TorusPrimitiveShape::Parameters(
    GfxTL::IndexedIterator<MiscLib::Vector<size_t>::iterator, PointCloud::const_iterator> begin,
    GfxTL::IndexedIterator<MiscLib::Vector<size_t>::iterator, PointCloud::const_iterator> end,
    MiscLib::Vector<std::pair<float, float>>* bmpParams) const
{
    ParametersImpl(begin, end, bmpParams);
}
 
void
TorusPrimitiveShape::Parameters(
    GfxTL::IndexedIterator<IndexIterator, PointCloud::const_iterator> begin,
    GfxTL::IndexedIterator<IndexIterator, PointCloud::const_iterator> end,
    MiscLib::Vector<std::pair<float, float>>* bmpParams) const
{
    ParametersImpl(begin, end, bmpParams);
}
 
bool
TorusPrimitiveShape::InSpace(float u, float v, Vec3f* p, Vec3f* n) const
{
    return m_parametrization.InSpace(u, v, p, n);
}
 
void
TorusPrimitiveShape::BitmapExtent(float epsilon,
                                  GfxTL::AABox<GfxTL::Vector2Df>* bbox,
                                  MiscLib::Vector<std::pair<float, float>>* params,
                                  size_t* uextent,
                                  size_t* vextent)
{
    *uextent = std::ceil((bbox->Max()[0] - bbox->Min()[0]) / epsilon);
    *vextent = std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon);
}
 
void
TorusPrimitiveShape::InBitmap(const std::pair<float, float>& param,
                              float epsilon,
                              const GfxTL::AABox<GfxTL::Vector2Df>& bbox,
                              size_t uextent,
                              size_t vextent,
                              std::pair<int, int>* inBmp) const
{
    inBmp->first = std::floor((param.first - bbox.Min()[0]) / epsilon);
    inBmp->second = std::floor((param.second - bbox.Min()[1]) / epsilon);
}
 
void
TorusPrimitiveShape::WrapBitmap(const GfxTL::AABox<GfxTL::Vector2Df>& bbox,
                                float epsilon,
                                bool* uwrap,
                                bool* vwrap) const
{
    m_parametrization.WrapBitmap(bbox, epsilon, uwrap, vwrap);
}
 
void
TorusPrimitiveShape::WrapComponents(const GfxTL::AABox<GfxTL::Vector2Df>& bbox,
                                    float epsilon,
                                    size_t uextent,
                                    size_t vextent,
                                    MiscLib::Vector<int>* componentImg,
                                    MiscLib::Vector<std::pair<int, size_t>>* labels) const
{
    m_parametrization.WrapComponents(bbox, epsilon, uextent, vextent, componentImg, labels);
}
 
void
TorusPrimitiveShape::SetExtent(const GfxTL::AABox<GfxTL::Vector2Df>& bbox,
                               const MiscLib::Vector<int>& componentsImg,
                               size_t uextent,
                               size_t vextent,
                               float epsilon,
                               int label)
{
}
 
bool
TorusPrimitiveShape::InSpace(size_t u,
                             size_t v,
                             float epsilon,
                             const GfxTL::AABox<GfxTL::Vector2Df>& bbox,
                             size_t uextent,
                             size_t vextent,
                             Vec3f* p,
                             Vec3f* n) const
{
    return m_parametrization.InSpace(
        (u + .5f) * epsilon + bbox.Min()[0], (v + .5f) * epsilon + bbox.Min()[1], p, n);
}