bool PlanePrimitiveShape::Fit(const PointCloud &pc, float epsilon, float normalThresh, MiscLib::Vector< size_t >::const_iterator begin, MiscLib::Vector< size_t >::const_iterator end) { Plane fit = m_plane; //if(fit.LeastSquaresFit(pc, begin, end)) if(fit.LeastSquaresFit(pc, begin, end)) { m_plane = fit; m_hcs.FromNormal(m_plane.getNormal()); return true; } return false; }
PrimitiveShape *PlanePrimitiveShape::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 { Plane fit = m_plane; if(fit.LeastSquaresFit(pc, begin, end)) { score->first = -1; return new PlanePrimitiveShape(fit); } score->first = 0; return NULL; }
void ConePrimitiveShape::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, bmpv; if(m_cone.Angle() >= M_PI / 4) { bmpu = std::sin(v) * u; bmpv = std::cos(v) * u; } else { bmpu = u; float r = m_cone.RadiusAtLength(u); bmpv = (v - float(M_PI)) * r; } InSpace(bmpu, bmpv, &samples[i * 5 + j], &samples[i * 5 + j + 25]); } } size_t c = samples.size() / 2; // now check all the shape types 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(); } } /*// simpler shapes are suggested if the maximal curvature of the cone // is small compared to the extend in relation to the distThresh float meanRadius, length, meanLength, radialExtent; // the cone is parameterized as length and angle // in this case the cone is parametrized as length and arclength meanRadius = (m_cone.RadiusAtLength(m_extBbox.Min()[0]) + m_cone.RadiusAtLength(m_extBbox.Max()[0])) / 2; length = m_extBbox.Max()[0] - m_extBbox.Min()[0]; meanLength = (m_extBbox.Max()[0] + m_extBbox.Min()[0]) / 2; // the radial extent radialExtent = m_extBbox.Max()[1] - m_extBbox.Min()[1]; // We suggest a cylinder if the opening angle of the cone is so small // that over the whole height the difference is less than distThresh if(std::sin(m_cone.Angle()) * length / 2 < distThresh) { // construct the cylinder // it has the same axis as the cone // and we use the average radius of the cone Cylinder cylinder(m_cone.AxisDirection(), m_cone.Center(), meanRadius); suggestions->push_back(new CylinderPrimitiveShape(cylinder)); suggestions->back()->Release(); } // We suggest a sphere if a curvature of mean radius along the height // does not introduce too large an error float sphereRadius = std::tan(m_cone.Angle()) * meanLength; float radiusDiff = (std::sqrt(sphereRadius * sphereRadius + length * length / 4) - sphereRadius) / 2; if(radiusDiff < distThresh) { // the center of the sphere is given as the point on the axis // with the height of the mean length Vec3f center = (meanLength / std::cos(m_cone.Angle())) * m_cone.AxisDirection() + m_cone.Center(); Sphere sphere(center, sphereRadius + radiusDiff); suggestions->push_back(new SpherePrimitiveShape(sphere)); suggestions->back()->Release(); } // We suggest a plane if the mean radius causes only a small error // for this we need the angular extent in the curved direction of the cone radiusDiff = meanRadius - std::sin(radialExtent) * meanRadius; if(radiusDiff < distThresh) { GfxTL::Vector2Df bboxCenter; m_extBbox.Center(&bboxCenter); Vec3f pos, normal; InSpace(bboxCenter[0], bboxCenter[1] * m_cone.RadiusAtLength(bboxCenter[0]), &pos, &normal); Plane plane(pos, normal); suggestions->push_back(new PlanePrimitiveShape(plane)); suggestions->back()->Release(); }*/ }
void CylinderPrimitiveShape::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) InSpace(u, v * m_cylinder.Radius(), &samples[i * 5 + j], &samples[i * 5 + j + 25]); } size_t c = samples.size() / 2; // now check all the shape types 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(); } } /*// We suggest a sphere if a curvature of radius along the height // does not introduce too large an error float length = m_extBbox.Max()[0] - m_extBbox.Min()[0]; float meanLength = (m_extBbox.Max()[0] + m_extBbox.Min()[0]) / 2; float radiusDiff = (std::sqrt(m_cylinder.Radius() * m_cylinder.Radius() + length * length / 4) - m_cylinder.Radius()) / 2; float radialExtent = m_extBbox.Max()[1] - m_extBbox.Min()[1]; if(radiusDiff < distThresh) { // the center of the sphere is given as the point on the axis // with the height of the mean length Vec3f center = meanLength * m_cylinder.AxisDirection() + m_cylinder.AxisPosition(); Sphere sphere(center, m_cylinder.Radius() + radiusDiff); suggestions->push_back(new SpherePrimitiveShape(sphere)); suggestions->back()->Release(); } // We suggest a plane if the mean radius causes only a small error // for this we need the angular extent in the curved direction of the cone radiusDiff = (m_cylinder.Radius() - std::cos(radialExtent / 2) * m_cylinder.Radius()) / 2; if(radiusDiff < distThresh) { GfxTL::Vector2Df bboxCenter; m_extBbox.Center(&bboxCenter); Vec3f pos, normal; InSpace(bboxCenter[0], bboxCenter[1] * m_cylinder.Radius(), &pos, &normal); // offset position pos -= radiusDiff * normal; Plane plane(pos, normal); suggestions->push_back(new PlanePrimitiveShape(plane)); suggestions->back()->Release(); }*/ }