点云平面合并

 //优化平面，Ransac的反复提取，冗余
                template <typename Kernel>
                void Hypothesis<Kernel>::refine_planes() 
                {
                        std::vector< Planar_segment* >& segments = point_set_->planar_segments();
                        const typename Point_set_with_planes::Point_map& points = point_set_->point_map();

                        FT avg_max_dist = 0;
                        for (std::size_t i = 0; i < segments.size(); ++i) 
                        {
                                Planar_segment* s = segments[i];
                                const Plane* plane = s->fit_supporting_plane(); // user may provide invalid plane fitting (we always fit)

                                FT max_dist = -(std::numeric_limits<FT>::max)();
                                for (std::size_t j = 0; j < s->size(); ++j) 
                                {
                                        std::size_t idx = s->at(j);
                                        const Point& p = points[idx];
                                        FT sdist = CGAL::squared_distance(*plane, p);
                                        max_dist = (std::max)(max_dist, std::sqrt(sdist));
                                }

                                avg_max_dist += max_dist;
                        }
                        avg_max_dist /= segments.size();
                        avg_max_dist /= FT(2.0);

                        FT theta = static_cast<FT>(CGAL_PI * 10.0 / FT(180.0));        // in radian
                        bool merged = false;
                        do {
                                merged = false;
                                // Segments with less points have less confidences and thus should be merged first.
                                // So we sort the segments according to their sizes.
                                std::sort(segments.begin(), segments.end(), internal::SegmentSizeIncreasing<Planar_segment>());

                                for (std::size_t i = 0; i < segments.size(); ++i) 
                                {
                                        Planar_segment* s1 = segments[i];
                                        const Plane* plane1 = s1->supporting_plane();
                                        Vector n1 = plane1->orthogonal_vector();
                                        internal::normalize<FT, Vector>(n1);

                                        FT num_threshold = s1->size() / FT(5.0);
                                        for (std::size_t j = i + 1; j < segments.size(); ++j) 
                                        {
                                                Planar_segment* s2 = segments[j];
                                                const Plane* plane2 = s2->supporting_plane();
                                                Vector n2 = plane2->orthogonal_vector();
                                                internal::normalize<FT, Vector>(n2);

                                                if (std::abs(n1 * n2) > std::cos(theta)) 
                                                {
                                                        std::size_t set1on2 = number_of_points_on_plane(s1, plane2, avg_max_dist);
                                                        std::size_t set2on1 = number_of_points_on_plane(s2, plane1, avg_max_dist);
                                                        if (set1on2 > num_threshold || set2on1 > num_threshold) 
                                                        {
                                                                merge(s1, s2);
                                                                merged = true;
                                                                break;
                                                        }
                                                }
                                        }
                                        if (merged)
                                                break;
                                }
                        } while (merged);

                        std::sort(segments.begin(), segments.end(), internal::SegmentSizeDecreasing<Planar_segment>());

                        // Stores all the supporting planes
                        for (std::size_t i = 0; i < segments.size(); ++i) {
                                Planar_segment* s = segments[i];
                                const Plane* plane = s->supporting_plane();
                                supporting_planes_.push_back(plane);
                        }
                }