SpherePrimitiveShape.cpp

Go to the documentation of this file.

#include "SpherePrimitiveShape.h"
 
#include <fstream>
#include <iostream>
#include <sstream>
 
#include "Bitmap.h"
#include "IndexIterator.h"
#include "PlanePrimitiveShape.h"
#include "PrimitiveShapeVisitor.h"
#include <GfxTL/AABox.h>
#include <GfxTL/BBoxBuildInformationTreeStrategy.h>
#include <GfxTL/BBoxDistanceKdTreeStrategy.h>
#include <GfxTL/BucketSizeSubdivisionTreeStrategy.h>
#include <GfxTL/CellBBoxBuildInformationKdTreeStrategy.h>
#include <GfxTL/CellRangeDataTreeStrategy.h>
#include <GfxTL/Covariance.h>
#include <GfxTL/ImmediateTreeDataKernels.h>
#include <GfxTL/IncrementalDistanceKdTreeStrategy.h>
#include <GfxTL/IndexedIterator.h>
#include <GfxTL/Jacobi.h>
#include <GfxTL/KdTree.h>
#include <GfxTL/L2Norm.h>
#include <GfxTL/MaxIntervalSplittingKdTreeStrategy.h>
#include <GfxTL/Mean.h>
#include <GfxTL/NearestNeighbors.h>
#include <GfxTL/NullClass.h>
#include <GfxTL/NullTreeStrategy.h>
#include <GfxTL/VectorKernel.h>
#include <MiscLib/Performance.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);
}