Detecção de Obstáculos Lidar

Github: https://github.com/williamhyin/CarND-Path-Planning

Sensores Lidar

sinal Lidar sensor de emitida por um milhares de laser, fornecemos dados de alta resolução. A luz do laser é refletida pelo objeto de volta para o sensor, então a distância de um objeto pode ser determinada calculando o tempo necessário para retornar um sinal. Nós também pode medir a intensidade do sinal de retorno para compreender a situação em que é atingido pelo objecto do laser J. cada feixe de laser são no espectro de infravermelho, e emitida em ângulos diferentes, geralmente na gama de 360 graus. Embora um sensor de radar de laser fornece-nos muito alta precisão 3 modelo d do mundo, mas eles são actualmente muito caro, e até mesmo alguns até US $ 60.000.

transmissor radar a laser milhares de feixes de laser em ângulos diferentes
A luz laser é emitida, reflectida a partir do obstáculo, e, em seguida, detectadas pelo receptor
A transmissão do laser e recepção diferença de tempo, calcula a distância
As propriedades dos materiais do valor de intensidade de laser de luz a laser reflectido recebido pode ser usada para avaliar o objecto

Este é Velodyne LIDAR Sensor, da esquerda para a direita usando HDL 64, HDL 32, maior será o sensor 16. VLP, maior será a resolução. O seguinte é um radar de laser 64 especificações de HDL. LIDAR total de 64 camadas, cada camada com o eixo z é emitida em ângulos diferentes, de modo que a inclinação ou diferentes cada camada que cobre o ponto de vista de 360 graus, resolução angular de 0,08 graus. os varrimentos médios de radar de laser por dez. LIDAR pode detectar longe 120 metros automóveis e plantas, também podem ser detectadas, tanto quanto 50 metros de calçada.

VLP 64 uma vista esquemática, mostrando um transmissor de radar de laser, um receptor e um alojamento

Esta camada de sensor 64, uma resolução angular de 0,09 graus, a frequência de actualização média de 10 Hz, recolhido por segundo ((360 / 0,08) x10 64x) = 288 milhões de dados

dados PCD

Vamos mergulhar em como os dados LIDAR é armazenado. de dados Lidar em um conjunto de dados em nuvem chamado de ponto (referido como PCD) arquivo de formato de armazenamento. PCD é uma lista de (x, y, z) coordenadas cartesianas e valores de intensidade, é uma fotografia de uma análise após o ambiente. VLP de 64 utilizando um radar de laser, arquivo PCD terá aproximadamente 256.000 (x, y, z, i) o valor da nuvem de pontos.

Os mesmos dados de ponto de nuvem, com o sistema de coordenadas do sistema local de um automóvel de coordenadas. Neste sistema de coordenadas, X-eixo que aponta para a parte da frente do automóvel, y-eixo que aponta para a esquerda do veículo. Além disso, z-eixo que aponta veículo para cima.

biblioteca PCL é amplamente utilizado no campo da robótica, para o processamento de dados de nuvens de pontos, há muitos tutoriais on-line disponíveis. PCL tem muitos recursos internos de para ajudar a detectar obstáculos. Mais tarde, este projecto vai usar o built-in PCL dividir, extrair e poli função de classe. aqui você pode encontrar o PCL biblioteca de documentos.

Passos para a detecção de obstáculos

fluxo PCD

Primeiro precisamos de dados ponto a laser nuvem de fluxo de carga.

template<typename PointT>
std::vector<boost::filesystem::path> ProcessPointClouds<PointT>::streamPcd(std::string dataPath) {

    std::vector<boost::filesystem::path>　paths(boost::filesystem::directory_iterator{dataPath},boost::filesystem::directory_iterator{});

    // sort files in accending order so playback is chronological
    sort(paths.begin(), paths.end());

    return paths;

}

// ####################################################

ProcessPointClouds<pcl::PointXYZI>* pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
std::vector<boost::filesystem::path> stream = pointProcessorI >streamPcd("../src/sensors/data/pcd/data_1");
auto streamIterator = stream.begin();
pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloudI;

dados reais PCD

ponto Processing

O primeiro passo é o de criar um ponto de dados de processamento de nuvem processPointCloudsdo objecto, que contém todo o processamento de dados de ponto de nuvem módulos de laser, tais como a filtragem, a segmentação, o agrupamento, carga, excepto dados de PCD. É preciso uma nuvem de pontos diferente dados para criar um modelo genérico: template<typename PointT>nos dados de nuvem de pontos real, tipos de nuvens de pontos são pcl::PointXYZI. criado pointProcessorpode ser construído na pilha também pode ser construída sobre o Heap, recomenda-se que no Heap, afinal de contas, o uso de ponteiro mais leve.

// Build PointProcessor on the heap
ProcessPointClouds<pcl::PointXYZI> *pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
// Build PointProcessor on the stack
ProcessPointClouds<pcl::PointXYZI> pointProcessorI;

filtragem

Note-se que os dados de nuvem de pontos geralmente tem resolução relativamente alta e longa distância visual. Queremos que o pipeline de processamento de código é capaz de processar a nuvem de pontos o mais rápido possível, é necessário para filtrar a nuvem de pontos. Há duas maneiras usado para fazer isso.

voxel Grade

grade voxel cria um cubo de filtro de malha, ponto de nuvem filtração deixando apenas um ponto de cada voxel dentro de um cubo, de modo que quanto maior for o comprimento de cada lado do cubo, a diminuir o ponto de turvação de resolução. No entanto, se o corpo grade su é muito grande, ele vai perder as características originais do objeto. implementação específica pode visualizar a grade de PCL-voxel filtrando documentos.

Região de interesse

região de interesse definida, e remover quaisquer pontos fora da região de interesse de ambos os lados para selecionar a região de interesse precisam tentar cobrir a largura da pista, enquanto a área antes e depois de se certificar que você pode mover e para trás no tempo para detectar veículos. aplicação específica pode ver o PCL -região de juros documentos. no resultado final, usamos pcl CropBoxa encontrar seus próprios dados de ponto índice teto do veículo nuvem, que são fornecidos com o índice de pcl ExtractIndicesexclusão de objeto, porque estes para a nossa análise dos dados de nuvens de pontos inúteis.

A região de interesse e o resultado de filtro de rede do voxel

A seguir está o código que a filtragem de implementos:

filterResÉ o tamanho da grade voxel, minPoint/maxPointo ponto mais próximo eo ponto mais distante da região de interesse.

Em primeiro lugar, realizar VoxelGrid reduzir o número de ponto de turvação, e, em seguida, definir a região de interesse entre o ponto mais próxima e mais distante, ponto de turvação e, finalmente, removida do telhado.

// To note, "using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;"
template<typename PointT>
PtCdtr<PointT> ProcessPointClouds<PointT>::FilterCloud(PtCdtr<PointT> cloud, float filterRes, Eigen::Vector4f minPoint,Eigen::Vector4f maxPoint) {

    // Time segmentation process
    auto startTime = std::chrono::steady_clock::now();

    // TODO:: Fill in the function to do voxel grid point reduction and region based filtering
    // Create the filtering object: downsample the dataset using a leaf size of .2m
    pcl::VoxelGrid<PointT> vg;
    PtCdtr<PointT> cloudFiltered(new pcl::PointCloud<PointT>);
    vg.setInputCloud(cloud);
    vg.setLeafSize(filterRes, filterRes, filterRes);
    vg.filter(*cloudFiltered);

    PtCdtr<PointT> cloudRegion(new pcl::PointCloud<PointT>);
    pcl::CropBox<PointT> region(true);
    region.setMin(minPoint);
    region.setMax(maxPoint);
    region.setInputCloud(cloudFiltered);
    region.filter(*cloudRegion);

    std::vector<int> indices;
    pcl::CropBox<PointT> roof(true);
    roof.setMin(Eigen::Vector4f(-1.5, -1.7, -1, 1));
    roof.setMax(Eigen::Vector4f(2.6, 1.7, -0.4, 1));
    roof.setInputCloud(cloudRegion);
    roof.filter(indices);

    pcl::PointIndices::Ptr inliers{new pcl::PointIndices};
    for (int point : indices) {
        inliers->indices.push_back(point);
    }
    pcl::ExtractIndices<PointT> extract;
    extract.setInputCloud(cloudRegion);
    extract.setIndices(inliers);
    extract.setNegative(true);
    extract.filter(*cloudRegion);

    auto endTime = std::chrono::steady_clock::now();
    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "filtering took " << elapsedTime.count() << " milliseconds" << std::endl;

//    return cloud;
    return cloudRegion;
}

segmentação

ponto e ponto autónomo de que é a tarefa cena Segmentação é pertencer a uma segmentação nuvem de pontos caminho dos detalhes específicos do site oficial do recomendado visualização de documentos PCL :. Segmentação e índices Extraindo .

Os resultados apontam segmentação nuvem

PCL RANSAC Segmentaion

Para este projeto, eu criei duas funções SegmentPlanee SeparateCloudsforam usadas para encontrar os valores atípicos de nuvem de pontos (objeto) no plano das inliers estrada (em pontos) e nuvem de pontos de extração.

A seguir, os principais códigos:

// To note, "using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;"
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::SegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceThreshold) {
    // Time segmentation process
    auto startTime = std::chrono::steady_clock::now();
//	pcl::PointIndices::Ptr inliers; // Build on the stack
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Build on the heap
    // TODO:: Fill in this function to find inliers for the cloud.
    pcl::ModelCoefficients::Ptr coefficient(new pcl::ModelCoefficients);
    pcl::SACSegmentation<PointT> seg;

    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(maxIterations);
    seg.setDistanceThreshold(distanceThreshold);

    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficient);

    if (inliers->indices.size() == 0) {
        std::cerr << "Could not estimate a planar model for the given dataset" << std::endl;
    }

    auto endTime = std::chrono::steady_clock::now();
    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "plane segmentation took " << elapsedTime.count() << " milliseconds" << std::endl;

    std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult = SeparateClouds(
            inliers, cloud);
    return segResult;
}

template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::SeparateClouds(pcl::PointIndices::Ptr inliers, PtCdtr<PointT> cloud) {
    // TODO: Create two new point clouds, one cloud with obstacles and other with segmented plane
    PtCdtr<PointT> obstCloud(new pcl::PointCloud<PointT>());
    PtCdtr<PointT> planeCloud(new pcl::PointCloud<PointT>());
    for (int index : inliers->indices) {
        planeCloud->points.push_back(cloud->points[index]);
    }
    // create extraction object
    pcl::ExtractIndices<PointT> extract;
    extract.setInputCloud(cloud);
    extract.setIndices(inliers);
    extract.setNegative(true);
    extract.filter(*obstCloud);
    std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult(obstCloud,
                                                        planeCloud);
//    std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult(cloud, cloud);
    return segResult;
}

Na SegmentPlanefunção de aceitarmos a nuvem de pontos, o número máximo de iterações ea distância tolerância como parâmetros, utilize std::pairobjetos para objeto nuvem loja de ponto e do pavimento da estrada. SeparateCloudsFunção de extracção de pontos não-inliers na nuvem de pontos, que os obstáculos ponto de partículas é uma divisão iterativo processo, mais iterações ter a oportunidade de retornar resultados melhores, mas leva mais tempo. demasiado grande tolerância a distância não irá resultar em um objeto de ponto nuvem é tratado como o mesmo objecto e sobre a vontade tolerância pequena distância resultando em objectos longos, que não pode ser tratado como um único objecto, tal como um camião.

Manual RANSAC Segmentação

Actualmente partículas divididas principalmente usados RANSACalgoritmo RANSACnome completo Random Sample Consensus, isto é, a consistência da amostra aleatória, é um método para detectar os dados de valores anormais. RANSACAtravés de várias iterações, retorna o melhor modelo. Cada iteração um subconjunto seleccionado aleatoriamente de dados, e gera ajustamento de um modelo da sub-amostra, por exemplo, uma linha ou um plano, em seguida, tem um máximo inliers(ponto de entrada), ou o menor ruído é modelo ajustado como o melhor modelo. como uma das RANSACmais pequenas dados algoritmo proposto como subconjuntos possíveis objetos juntos por dois pontos é uma linha recta, é um plano de três pontos de cada iteração, então a contagem do número de pontos restantes e a sua distância ao modelo para calcular inliers. ponto dentro de uma certa distância do modelo é calculado como inliers. que tem o maior número de inliers modelo iterativo é o melhor modelo. esta é a nossa versão alcançado neste projeto. RANSACalgoritmo através de iteração constante, encontrar as mais inliers modelo de montagem, enquanto os outliers são excluídos. RANSACoutra um método para uma percentagem dos pontos de modelo são amostrados, por exemplo, 20% do total de pontos, em seguida, ajustando uma linha recta e, em seguida, o cálculo de um erro da linha, método iterativo minimização do erro é o melhor modelo. A vantagem deste método é que cada necessidade ponto de não ser considerado em cada iteração. O seguinte é um RANSACalgoritmo para a montagem de uma linha de esquema, os dados laser é um avião de ajuste real para separar e estrada de objetos. separadamente abaixo RANSACalgoritmo de planos implementados.

O que se segue descreve a fórmula plano RANSAC:

Então precisamos calcular os pontos (x,y,z)para a distância do avião

Os seguintes códigos planares RANSAC corpo

template<typename PointT>
std::unordered_set<int> Ransac<PointT>::Ransac3d(PtCdtr<PointT> cloud) {
    std::unordered_set<int> inliersResult; // unordered_set element has been unique
    // For max iterations
    while (maxIterations--) {
        std::unordered_set<int> inliers;
        while (inliers.size() < 3) {
            inliers.insert(rand()%num_points);
        }
        // TO define plane, need 3 points
        float x1, y1, z1, x2, y2, z2, x3, y3, z3;
        auto itr = inliers.begin();
        x1 = cloud->points[*itr].x; 　
        y1 = cloud->points[*itr].y;
        z1 = cloud->points[*itr].z; 　
        itr++;
        x2 = cloud->points[*itr].x;
        y2 = cloud->points[*itr].y;
        z2 = cloud->points[*itr].z;
        itr++;
        x3 = cloud->points[*itr].x;
        y3 = cloud->points[*itr].y;
        z3 = cloud->points[*itr].z;
        // Calulate plane coefficient
        float a, b, c, d, sqrt_abc;
        a = (y2 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);
        b = (z2 - z1) * (x3 - x1) - (x2 - x1) * (z3 - z1);
        c = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);
        d = -(a * x1 + b * y1 + c * z1);
        sqrt_abc = sqrt(a * a + b * b + c * c);
        // Check distance from point to plane
        for (int ind = 0; ind < num_points; ind++) {
            if (inliers.count(ind) > 0) { // that means: if the inlier in already exist, we dont need do anymore
                continue;
            }
            PointT point = cloud->points[ind];
            float x = point.x;
            float y = point.y;
            float z = point.z;
            float dist = fabs(a * x + b * y + c * z + d) / sqrt_abc; // calculate the distance between other points and plane

            if (dist < distanceTol) {
                inliers.insert(ind);
            }
            if (inliers.size() > inliersResult.size()) {
                inliersResult = inliers;

            }
        }
    }
    return inliersResult;
}

Mas, na prática, PCL foi construído com RANSACfunções e velocidade de cálculo mais rápido do que o que eu escrevi, então o uso direto do built-in na linha.

Clustering

Clustering refere-se às nuvens de pontos são uma combinação de diferentes objetos se reuniram para permitem que você acompanhe carros múltiplos alvos, pedestres, etc. Uma maneira de agrupar os dados de nuvem de pontos e é referido como o agrupamento agrupamento euclidiana .

Clustering Europeia refere-se à distância a partir de perto nuvem alta ponto reunir. Em ordem a executar eficientemente busca do vizinho mais próximo, pode ser usado estrutura de dados de árvore-kd, o que pode acelerar a partir de um ó média (n) para o (log (n) ) tempo de pesquisa. isso é porque Kd-Treeque lhe permite dividir o seu melhor espaço de busca. agrupados KD-Tree na região através do ponto, você pode evitar o cálculo pode ter milhares de pontos de distância, porque você sabe que eles não será considerado em uma área imediatamente adjacente.

Os resultados do agrupamento euclidiana

PCL euclidiana agrupamento

Primeiro, usamos a função built-in PCL Europeia agrupamento. Ponto nuvem agrupamento é recomendado para ver os detalhes do site oficial do documento PCL euclidiana Cluster .

alvos de agrupamento euclidianas ectêm uma tolerância à distância. Qualquer ponto dentro desta distância serão agrupados. Ele também é usado para indicar o número de pontos do min parâmetros de cluster e Max. Se um cluster muito pequeno, só pode ruído, mínimo de parâmetros limitar o uso do cluster. o parâmetro máximo nos permite melhor quebrar aglomerados muito grandes, se um cluster é muito grande, pode ser apenas sobrepostas em muitos outros clusters, a tolerância máxima pode nos ajudar a melhor detecção de objetos endereço. o último parâmetro é o cluster Europeia Kd-Tree. Kd-Treesão criados e construídos usando a nuvem de pontos de entrada, o ponto de nuvem é o ponto de entrada ponto de divisão de nuvem em nuvem obstáculo inicial por filtração.

O seguinte é um built-in de código função de agrupamento PCL Europeia:

// To note, "using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;"
template<typename PointT>
std::vector<PtCtr<PointT>>
ProcessPointClouds<PointT>::Clustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize, int maxSize) {

    // Time clustering process
    auto startTime = std::chrono::steady_clock::now();

    std::vector<PtCdtr<PointT>> clusters;

    // TODO:: Fill in the function to perform euclidean clustering to group detected obstacles
    // Build Kd-Tree Object
    typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
    // Input obstacle point cloud to create KD-tree
    tree->setInputCloud(cloud);

    std::vector<pcl::PointIndices> clusterIndices; // this is point cloud indice type
    pcl::EuclideanClusterExtraction<PointT> ec; // clustering object
    ec.setClusterTolerance(clusterTolerance);
    ec.setMinClusterSize(minSize);
    ec.setMaxClusterSize(maxSize);
    ec.setSearchMethod(tree);
    ec.setInputCloud(cloud); // feed point cloud
    ec.extract(clusterIndices);// get all clusters Indice 
	
    // For each cluster indice
    for (pcl::PointIndices getIndices: clusterIndices) {
        PtCdtr<PointT> cloudCluster(new pcl::PointCloud<PointT>);
        // For each point indice in each cluster
        for (int index:getIndices.indices) {
            cloudCluster->points.push_back(cloud->points[index]);
        }
        cloudCluster->width = cloudCluster->points.size();
        cloudCluster->height = 1;
        cloudCluster->is_dense = true;
        clusters.push_back(cloudCluster);

    }

    auto endTime = std::chrono::steady_clock::now();
    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size()<< " clusters" << std::endl;

    return clusters;
}

agrupamento euclidiana manual

Além disso, também pode ser usado diretamente KD-Treeser agrupamento euclidiana.

Aqui nós primeiro KD-Treeprincípio será descrito. KD-TreeÉ uma estrutura de dados, representada por uma árvore binária, no valor de diferentes dimensões entre o ponto de inserção da comparação alternadamente para a divisão de espaço, dividindo a região como nos dados 3D, é alternadamente necessário no X, Y, Z comparar eixos diferentes. Assim, seria muito mais rápido busca vizinho mais próximo.

Primeiro, tentamos construir no espaço bidimensional KD-Tree, e sobre todo o processo de implementação distâncias euclidianas no espaço bidimensional, vamos eventualmente, ser prorrogada para o espaço tridimensional.

KD-TreeNo ponto de inserção (a nuvem de pontos que é a entrada para a árvore de criar e construir KD-Treepassos)

Suponhamos que temos de KD-Treeinserir os quatro pontos (-6,3, 8,4), (-6,2, 7), (-5,2, 7,1) (-5,7, 6,3)

Em primeiro lugar, temos que determinar um ponto de base (-6,2, 7), a primeira camada é um 0 eixo x, o ponto de inserção (-6,3, 8,4), (-5,2, 7,1), uma vez que a -6,3 <-6,2 (- 6,3 , 8,4) é dividido em um nó filho esquerdo, enquanto -5.2> -6.2 (-5.2, 7.1) é dividido em nó filho direito. (-5,7, 6,3) no eixo x -5.7> -6.2, assim por diante (-5.2, 7.1) nó, então a primeira camada usando o eixo-y, 6,3 <7,1, e, portanto, sobre a (-5.2, 7.1) do nó filho esquerdo. da mesma forma, se queremos inserir o quinto ponto (7.2, 6.1), você comparação alternadamente x, o valor de eixo y, (7.2>-6.2)->(6.1<7.1)->(7.2>-5.7)o nó filho direita, para ser inserido no quinto ponto (-5,7, 6,3) de C.

Abaixo está KD-Treea estrutura.

KD-TreeO espaço do objeto em diferentes regiões, mais próximas para reduzir o tempo de busca.

É uma nova maneira de usar o nó de atualização ponto de inserção recursiva. A idéia básica é a de percorrer a árvore até atingir um nó é NULL, neste caso, criará um novo nó e substituir os nós nulos. Podemos usar um ponteiro duplo alocar um nó, o que significa que você pode passar um ponto para começar a partir do ponteiro nó raiz, em seguida, quando você deseja substituir um nó, você pode dobrar-ponteiro dereference e atribuí-lo ao nó recém-criado.

Podemos entender o código na KD-Treeideia do ponto de inserção:

struct Node {
    std::vector<float> point;
    int id;
    Node *left;
    Node *right;

    Node(std::vector<float> arr, int setId)
            : point(arr), id(setId), left(NULL), right(NULL) {}
};

struct KdTree {
    Node *root;

    KdTree()
            : root(NULL) {}
// Kd-Tree insert
    void insertHelper(Node **node, uint depth, std::vector<float> point, int id) {
        // Tree is empty
        if (*node == NULL) {
            *node = new Node(point, id);
        } else {
            // calculate current dim (1 means x axes, 2means y axes)
            uint cd = depth % 2;
            if (point[cd] < ((*node)->point[cd])) {
                insertHelper(&((*node)->left), depth + 1, point, id);
            } else {
                insertHelper(&((*node)->right), depth + 1, point, id);
            }
        }
    }

    void insert(std::vector<float> point, int id) {
        // TODO: Fill in this function to insert a new point into the tree
        // the function should create a new node and place correctly with in the root
        insertHelper(&root, 0, point, id);
    }
// #############################################################################################################

// Kd-Tree search
    void searchHelper(std::vector<float> target, Node *node, int depth, float distanceTol, std::vector<int> &ids)
    {
        if (node != NULL)
        {
            // Check whether the node inside box  or not, point[0] means x axes,　point[1]means y axes
            if ((node->point[0] >= (target[0] - distanceTol) && node->point[0] <= (target[0] + distanceTol)) &&(node->point[1] >= (target[1] - distanceTol) && node->point[1] <= (target[1] + distanceTol)))
            {
                float distance = sqrt((node->point[0] - target[0]) * (node->point[0] - target[0]) +(node->point[1] - target[1]) * (node->point[1] - target[1]));
                if (distance <= distanceTol)
                {
                    ids.push_back(node->id);
                }
            }
            // check across boundary
            if ((target[depth % 2] - distanceTol) < node->point[depth % 2])
            {
                searchHelper(target, node->left, depth + 1, distanceTol, ids);
            }
            if ((target[depth % 2] + distanceTol) > node->point[depth % 2])
            {
                searchHelper(target, node->right, depth + 1, distanceTol, ids);
            }

        }
    }

    // return a list of point ids in the tree that are within distance of target
    std::vector<int> search(std::vector<float> target, float distanceTol)
    {
        std::vector<int> ids;
        searchHelper(target, root, 0, distanceTol, ids);
        return ids;
    }

};

KD-Tree dividido utilizando um espaço boa pesquisa

Kd-Treeregião dividida e permite que certas zonas sejam completamente excluída, acelerando assim o processo de procura de pontos perto do Conselho

Na imagem acima, temos oito pontos, o método convencional é calcular a distância a percorrer cada ponto a ponto raiz, o ponto dentro de uma tolerância distância de pontos vizinhos. Agora temos Kd-Treeinserido todos os pontos, criamos uma raiz em torno de um ponto 2 x distanceTolcomprimento do bloco, se o nó atual está localizado na caixa, a distância pode ser calculado diretamente, e ver se ele deve ser adicionado imediatamente ao ponto da lista de id ponto id, em seguida, determina se o nó é dividido exibindo as extensões região bloco, comparação de se o próximo nó. recursivamente executar esta operação, é vantajoso se a área de quadro não está dentro de uma região dividida, a região é simplesmente passado por cima. como indicado acima, superior esquerdo, inferior esquerdo e inferior do lado direito da região dividida não estão incluídos no bloco região, ignore estas áreas precisam apenas contar o ponto verde no bloco com a distância a partir do nó raiz.

Acima do segundo bloco de código é dividido em partes com base no Kd-Treecódigo de pesquisa.

Uma vez que a busca de pontos vizinhos para conseguir Kd-Treeum método, é difícil de alcançar agrupados com base no único método de agrupamento euclidiana proximidade índice de agrupamento.

iterações de agrupamento euclidianas necessário para executar cada nuvem de pontos travessia, e manter o controle dos pontos foram processados. Para cada ponto, adicioná-lo à lista de pontos de um cluster (cluster), e, em seguida, usar a função de busca para obter a frente uma lista de todos os pontos perto do ponto. para cada ponto de muito perto, mas ainda não processado, ele é adicionado ao cluster, e repetiu as chamadas para proximityo processo. pela primeira cluster, a recursão pára, criar um novo cluster e mover o ponto lista para o novo cluster repita o processo. uma vez que todos os pontos tenham sido processados, você vai encontrar um certo número de clusters, retorna uma lista de clusters.

O seguinte é a Euclidiana distâncias pseudocódigo:

Proximity(point,cluster):
    mark point as processed
    add point to cluster
    nearby points = tree(point)
    Iterate through each nearby point
        If point has not been processed
            Proximity(cluster)

EuclideanCluster():
    list of clusters 
    Iterate through each point
        If point has not been processed
            Create cluster
            Proximity(point, cluster)
            cluster add clusters
    return clusters

Real Código:

void clusterHelper(int indice, const std::vector<std::vector<float>> &points, std::vector<int> &cluster,std::vector<bool> &processed, KdTree *tree, float distanceTol) {

    processed[indice] = true;
    cluster.push_back(indice);
    std::vector<int> nearest = tree->search(points[indice], distanceTol);
    for (int id:nearest) {
        if (!processed[id]) {
            clusterHelper(id, points, cluster, processed, tree, distanceTol);
        }
    }
}

std::vector<std::vector<int>>euclideanCluster(const std::vector<std::vector<float>> &points, KdTree *tree, float distanceTol) {

    // TODO: Fill out this function to return list of indices for each cluster

    std::vector<std::vector<int>> clusters;
    std::vector<bool> processed(points.size(), false);

    int i = 0;
    while (i < points.size()) {
        if (processed[i]) {
            i++;
            continue;
        }
        std::vector<int> cluster;
        clusterHelper(i, points, cluster, processed, tree, distanceTol);
        clusters.push_back(cluster);
        i++;
    }

    return clusters;
}

Estes são os clusters europeus para alcançar no espaço bidimensional, em um laser de reais dados de nuvens de pontos, precisamos estender clusters europeus para o espaço tridimensional. Posso referir-se a implementação de código específico de GitHub em cluster3d / kdtree3d arquivo. Próprio punho da Europa agrupamento pode melhorar a compreensão do conceito, mas na verdade um projeto real, com funções de agrupamento PCL euclidianas construída diretamente sobre a linha.

caixas delimitadoras

Após o ponto de conclusão agrupamento nuvem, o último passo, precisamos adicionar um ponto cravejado caixa delimitadora. Outros objetos, como veículos, o volume de espaço dentro da caixa delimitadora de pedestres estão proibidos de entrar, e, a fim de evitar uma colisão.

O seguinte é o código para a geração de caixa delimitadora:

template<typename PointT>
Box ProcessPointClouds<PointT>::BoundingBox(PtCdtr<PointT> cluster) {

    // Find bounding box for one of the clusters
    PointT minPoint, maxPoint;
    pcl::getMinMax3D(*cluster, minPoint, maxPoint);

    Box box;
    box.x_min = minPoint.x;
    box.y_min = minPoint.y;
    box.z_min = minPoint.z;
    box.x_max = maxPoint.x;
    box.y_max = maxPoint.y;
    box.z_max = maxPoint.z;

    return box;
}

// Calling Bouding box function and render box
Box box = pointProcessor->BoundingBox(cluster);
renderBox(viewer,box,clusterId);

Nós finalmente renderboxmostrar um por um funções caixas delimitadoras.

Para delimitadoras caixas podem ser usadas para gerar APC análise de componente principal gera um bloco menor, para atingir mais elevadas previsões precisão implementação específica PCL pode ver esta ligação :. PCL-PCA .