SpherePrimitiveShape.cpp

Go to the documentation of this file.

#include "SpherePrimitiveShape.h"
#include "PrimitiveShapeVisitor.h"
#include "Bitmap.h"
#include <GfxTL/AABox.h>
#include <GfxTL/NullClass.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include "IndexIterator.h"
#include <GfxTL/KdTree.h>
#include <GfxTL/BBoxDistanceKdTreeStrategy.h>
#include <GfxTL/CellRangeDataTreeStrategy.h>
#include <GfxTL/ImmediateTreeDataKernels.h>
#include <GfxTL/VectorKernel.h>
#include <GfxTL/NullTreeStrategy.h>
#include <GfxTL/IncrementalDistanceKdTreeStrategy.h>
#include <GfxTL/MaxIntervalSplittingKdTreeStrategy.h>
#include <GfxTL/CellBBoxBuildInformationKdTreeStrategy.h>
#include <GfxTL/BBoxBuildInformationTreeStrategy.h>
#include <GfxTL/BucketSizeSubdivisionTreeStrategy.h>
#include <GfxTL/L2Norm.h>
#include <GfxTL/NearestNeighbors.h>
#include <GfxTL/Mean.h>
#include <GfxTL/Covariance.h>
#include <GfxTL/IndexedIterator.h>
#include <GfxTL/Jacobi.h>
#include <MiscLib/Performance.h>
#include "PlanePrimitiveShape.h"
extern MiscLib::performance_t totalTime_sphereConnected;
 
SpherePrimitiveShape::SpherePrimitiveShape(const Sphere& s)
    : m_sphere(s)
    , m_parametrization(m_sphere)
{}
 
SpherePrimitiveShape::SpherePrimitiveShape(const SpherePrimitiveShape& sps)
    : BitmapPrimitiveShape(sps)
    , m_sphere(sps.m_sphere)
    , m_parametrization(sps.m_parametrization)
{
    m_parametrization.Shape(m_sphere);
}
 
size_t SpherePrimitiveShape::Identifier() const
{
    return 1;
}
 
bool SpherePrimitiveShape::Init(bool binary, std::istream* i)
{
    // read the polygons but ignore them
    GfxTL::AABox< GfxTL::Vector2Df > bboxUpper, bboxLower;
    size_t upperuextent, uppervextent, loweruextent, lowervextent;
    if (binary)
    {
        // read number of components
        size_t size;
        i->read((char*)&size, sizeof(size));
        if (size)
        {
            // read upper bbox
            i->read((char*)&bboxUpper, sizeof(bboxUpper));
            // read upperuextent and uppervextent
            i->read((char*)&upperuextent, sizeof(upperuextent));
            i->read((char*)&uppervextent, sizeof(uppervextent));
            for (size_t j = 0; j < size; ++j)
            {
                // read number of polys of component
                size_t numPolys;
                i->read((char*)&numPolys, sizeof(numPolys));
                for (size_t k = 0; k < numPolys; ++k)
                {
                    // read number of points in poly
                    size_t numPoints;
                    i->read((char*)&numPoints, sizeof(numPoints));
                    GfxTL::VectorXD< 2, size_t > pp;
                    for (size_t l = 0; l < numPoints; ++l)
                    {
                        i->read((char*)&pp, sizeof(pp));
                    }
                }
            }
        }
        // do the same for lower bitmap
        // read number of components
        i->read((char*)&size, sizeof(size));
        if (size)
        {
            // read lower bbox
            i->read((char*)&bboxLower, sizeof(bboxLower));
            // read loweruextent and lowervextent
            i->read((char*)&loweruextent, sizeof(loweruextent));
            i->read((char*)&lowervextent, sizeof(lowervextent));
            for (size_t j = 0; j < size; ++j)
            {
                // read number of polys of component
                size_t numPolys;
                i->read((char*)&numPolys, sizeof(numPolys));
                for (size_t k = 0; k < numPolys; ++k)
                {
                    // read number of points in poly
                    size_t numPoints;
                    i->read((char*)&numPoints, sizeof(numPoints));
                    GfxTL::VectorXD< 2, size_t > pp;
                    for (size_t l = 0; l < numPoints; ++l)
                    {
                        i->read((char*)&pp, sizeof(pp));
                    }
                }
            }
        }
    }
    else
    {
        // read number of components
        size_t size;
        (*i) >> size;
        if (size)
        {
            // read upper bbox
            (*i) >> bboxUpper.Min()[0] >> bboxUpper.Max()[0]
                 >> bboxUpper.Min()[1] >> bboxUpper.Max()[1];
            // read upperuextent and uppervextent
            (*i) >> upperuextent >> uppervextent;
            for (size_t j = 0; j < size; ++j)
            {
                // read number of polys of component
                size_t numPolys;
                (*i) >> numPolys;
                for (size_t k = 0; k < numPolys; ++k)
                {
                    // read number of points in poly
                    size_t numPoints;
                    (*i) >> numPoints;
                    GfxTL::VectorXD< 2, size_t > pp;
                    for (size_t l = 0; l < numPoints; ++l)
                    {
                        (*i) >> pp[0] >> pp[1];
                    }
                }
            }
        }
        // read number of components
        (*i) >> size;
        if (size)
        {
            // read lower bbox
            (*i) >> bboxLower.Min()[0] >> bboxLower.Max()[0]
                 >> bboxLower.Min()[1] >> bboxLower.Max()[1];
            // read loweruextent and lowervextent
            (*i) >> loweruextent >> lowervextent;
            for (size_t j = 0; j < size; ++j)
            {
                // read number of polys of component
                size_t numPolys;
                (*i) >> numPolys;
                for (size_t k = 0; k < numPolys; ++k)
                {
                    // read number of points in poly
                    size_t numPoints;
                    (*i) >> numPoints;
                    GfxTL::VectorXD< 2, size_t > pp;
                    for (size_t l = 0; l < numPoints; ++l)
                    {
                        (*i) >> pp[0] >> pp[1];
                    }
                }
            }
        }
    }
    return true;
}
 
PrimitiveShape* SpherePrimitiveShape::Clone() const
{
    return new SpherePrimitiveShape(*this);
}
 
float SpherePrimitiveShape::Distance(const Vec3f& p) const
{
    return m_sphere.Distance(p);
}
 
float SpherePrimitiveShape::SignedDistance(const Vec3f& p) const
{
    return m_sphere.SignedDistance(p);
}
 
float SpherePrimitiveShape::NormalDeviation(const Vec3f& p,
        const Vec3f& n) const
{
    Vec3f normal;
    m_sphere.Normal(p, &normal);
    return n.dot(normal);
}
 
void SpherePrimitiveShape::DistanceAndNormalDeviation(
    const Vec3f& p, const Vec3f& n, std::pair< float, float >* dn) const
{
    Vec3f normal;
    dn->first = m_sphere.DistanceAndNormal(p, &normal);
    dn->second = n.dot(normal);
}
 
void SpherePrimitiveShape::Project(const Vec3f& p, Vec3f* pp) const
{
    m_sphere.Project(p, pp);
}
 
void SpherePrimitiveShape::Normal(const Vec3f& p, Vec3f* n) const
{
    m_sphere.Normal(p, n);
}
 
unsigned int SpherePrimitiveShape::ConfidenceTests(unsigned int numTests,
        float epsilon, float normalThresh, float rms, const PointCloud& pc,
        const MiscLib::Vector< size_t >& indices) const
{
    return BasePrimitiveShape::ConfidenceTests< Sphere >(numTests, epsilon,
            normalThresh, rms, pc, indices);
}
 
void SpherePrimitiveShape::Description(std::string* s) const
{
    *s = "Sphere";
}
 
bool SpherePrimitiveShape::Fit(const PointCloud& pc, float epsilon,
                               float normalThresh, MiscLib::Vector< size_t >::const_iterator begin,
                               MiscLib::Vector< size_t >::const_iterator end)
{
    // do LS-fitting
    Sphere fit = m_sphere;
    if (fit.LeastSquaresFit(pc, begin, end))
    {
        m_sphere = fit;
        m_parametrization.Shape(m_sphere);
        return true;
    }
    return false;
}
 
PrimitiveShape* SpherePrimitiveShape::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
{
    // do LS-fitting
    Sphere fit = m_sphere;
    if (fit.LeastSquaresFit(pc, begin, end))
    {
        score->first = -1;
        return new SpherePrimitiveShape(fit);
    }
    score->first = 0;
    return NULL;
}
 
void SpherePrimitiveShape::Serialize(std::ostream* o, bool binary) const
{
    if (binary)
    {
        const char id = 1;
        (*o) << id;
    }
    else
    {
        (*o) << "1" << " ";
    }
    m_sphere.Serialize(binary, o);
    m_parametrization.Serialize(o, binary);
    if (!binary)
    {
        *o << std::endl;
    }
}
 
void SpherePrimitiveShape::Deserialize(std::istream* i, bool binary)
{
    m_sphere.Init(binary, i);
    m_parametrization.Shape(m_sphere);
    m_parametrization.Deserialize(i, binary);
}
 
size_t SpherePrimitiveShape::SerializedSize() const
{
    return m_sphere.SerializedSize()
           + m_parametrization.SerializedSize() + 1;
}
 
LevMarFunc< float >* SpherePrimitiveShape::SignedDistanceFunc() const
{
    return new SphereLevMarFunc(m_sphere);
}
 
void SpherePrimitiveShape::Transform(float scale, const Vec3f& translate)
{
    m_sphere.Transform(scale, translate);
}
 
void SpherePrimitiveShape::Visit(PrimitiveShapeVisitor* visitor) const
{
    visitor->Visit(*this);
}
 
void SpherePrimitiveShape::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;
    samples.resize(2 * 25);
    size_t c = samples.size() / 2;
    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)
            m_parametrization.InSpace(u, v,
                                      &samples[i * 5 + j], &samples[i * 5 + j + c]);
    }
 
    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();
        }
    }
 
    /*// we suggest a plane if the radius is large enough so that the error
    // along the two directions is less than distThresh
    float ulength = 0, vlength = 0;
    if(m_hasBitmap.first)
    {
        // has points on the upper side
        ulength = m_extBboxUpper.Max()[0] - m_extBboxUpper.Min()[0];
        vlength = m_extBboxUpper.Max()[1] - m_extBboxUpper.Min()[1];
    }
    if(m_hasBitmap.second)
    {
        ulength += m_extBboxLower.Max()[0] - m_extBboxLower.Min()[0];
        vlength += m_extBboxLower.Max()[1] - m_extBboxLower.Min()[1];
    }
    float arcLength = M_PI * std::max(ulength, vlength);
    float radiusDiff = (m_sphere.Radius() - std::sin(arcLength / 2)
        * m_sphere.Radius()) / 2;
    if(radiusDiff < distThresh)
    {
        Vec3f pos, normal;
        SphereAsSquaresParametrization ssp(m_sphere, m_parametrizationNormal);
        if(m_hasBitmap.first && m_hasBitmap.second)
        {
            GfxTL::Vector3Df center, eigenValues;
            GfxTL::Mean(GfxTL::IndexIterate(begin, pc.begin()),
                GfxTL::IndexIterate(end, pc.begin()), &center);
            GfxTL::MatrixXX< 3, 3, float > cov, eigenVectors;
            GfxTL::CovarianceMatrix(center, GfxTL::IndexIterate(begin, pc.begin()),
                GfxTL::IndexIterate(end, pc.begin()), &cov);
            GfxTL::Jacobi(cov, &eigenValues, &eigenVectors);
            GfxTL::EigSortDesc(&eigenValues, &eigenVectors);
            GfxTL::Vector3Df n = GfxTL::Vector3Df(eigenVectors[2]);
            Plane plane(Vec3f(center.Data()), Vec3f(n.Data()));
            suggestions->push_back(new PlanePrimitiveShape(plane));
            suggestions->back()->Release();
        }
        else if(m_hasBitmap.first)
        {
            GfxTL::Vector2Df center;
            m_extBboxUpper.Center(&center);
            ssp.InSpace(std::make_pair(center[0], center[1]), false, &pos, &normal);
            // offset position
            pos -= radiusDiff * normal;
            Plane plane(pos, normal);
            suggestions->push_back(new PlanePrimitiveShape(plane));
            suggestions->back()->Release();
        }
        else if(m_hasBitmap.second)
        {
            GfxTL::Vector2Df center;
            m_extBboxUpper.Center(&center);
            ssp.InSpace(std::make_pair(center[0], center[1]), true, &pos, &normal);
            // offset position
            pos -= radiusDiff * normal;
            Plane plane(pos, normal);
            suggestions->push_back(new PlanePrimitiveShape(plane));
            suggestions->back()->Release();
        }
    }*/
}
 
void SpherePrimitiveShape::OptimizeParametrization(const PointCloud& pc,
        MiscLib::Vector< size_t >::const_iterator begin,
        MiscLib::Vector< size_t >::const_iterator end, float epsilon)
{
    m_parametrization.Optimize(GfxTL::IndexIterate(begin, pc.begin()),
                               GfxTL::IndexIterate(end, pc.begin()), epsilon);
}
 
void SpherePrimitiveShape::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 SpherePrimitiveShape::Similar(float tolerance,
                                   const SpherePrimitiveShape& shape) const
{
    return m_sphere.Radius() <= (1.f + tolerance) * shape.m_sphere.Radius()
           && (1.f + tolerance) * m_sphere.Radius() >= shape.m_sphere.Radius();
}
 
void SpherePrimitiveShape::Parameters(const Vec3f& p,
                                      std::pair< float, float >* param) const
{
    m_parametrization.Parameters(p, param);
}
 
void SpherePrimitiveShape::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 SpherePrimitiveShape::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 SpherePrimitiveShape::InSpace(float u, float v, Vec3f* p, Vec3f* n) const
{
    return m_parametrization.InSpace(u, v, p, n);
}
 
void SpherePrimitiveShape::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 SpherePrimitiveShape::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 SpherePrimitiveShape::WrapBitmap(
    const GfxTL::AABox< GfxTL::Vector2Df >& bbox,
    float epsilon, bool* uwrap, bool* vwrap) const
{
    m_parametrization.WrapBitmap(bbox, epsilon, uwrap, vwrap);
}
 
void SpherePrimitiveShape::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);
}
 
bool SpherePrimitiveShape::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);
}