url-kaist / TRAVEL

Traversable ground and above-ground object segmentation using graph representation of 3D LiDAR scans

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

Evaluation Code

HMX2013 opened this issue · comments

commented

Thanks for your great work. I am currently working on a new clustering method, and want to use the Evaluation Metrics, OSE and USE, proposed in your paper. But I run into some problem when implementing it. Could you share this part of code. I need to use the labels generated by clustering algorithm to calculate the corresponding evaluation metrics.

Sorry for the late code cleanup...
I'm still busy with other things, so I can only share the source code below, I'll organize it later when I have time.

bool isSameCluster(const pcl::PointXYZINormal &p1, const pcl::PointXYZINormal &p2, float sqr_dist) {
    if (!debug) {
        if (sqr_dist < gt_clustering_tol*gt_clustering_tol && 
            p1.curvature == p2.curvature &&
            p1.intensity == p2.intensity) {  // intensity: label, curvature: id
            // printf("Label: %.1f, id: %.1f, dist: %.2f, thres: %.2f\n", p1.intensity, p1.curvature, sqr_dist, gt_clustering_tol*gt_clustering_tol);
            // printf("p1: %f;%f;%f,  p2: %f;%f;%f\n", p1.x, p1.y, p1.z, p2.x, p2.y, p2.z);
            return true;
        }
    } else {
        if (sqr_dist < gt_clustering_tol*gt_clustering_tol && 
            p1.intensity == p2.intensity) {  // intensity: label, curvature: id
            // printf("Label: %.1f, id: %.1f, dist: %.2f, thres: %.2f\n", p1.intensity, p1.curvature, sqr_dist, gt_clustering_tol*gt_clustering_tol);
            // printf("p1: %f;%f;%f,  p2: %f;%f;%f\n", p1.x, p1.y, p1.z, p2.x, p2.y, p2.z);
            return true;
        }
    }
    return false;
} 
// function for copy
void copyPointCloudCEC(pcl::PointCloud<PointType> &src, 
        pcl::PointCloud<pcl::PointXYZINormal> &dst) {
    dst.points.resize(src.points.size());
    for (size_t i = 0; i < src.points.size(); i++) {
        dst[i].x = src[i].x;
        dst[i].y = src[i].y;
        dst[i].z = src[i].z;
        dst[i].intensity = src[i].label;
    }
}
//function for USE
float evalUSE(pcl::PointCloud<PointType>::Ptr cloud_in) {
    float USE = 0.0;
    std::map<uint32_t, std::map<uint32_t, int>> pred_to_tgt;
    for(auto &pt : cloud_in->points) {
        uint32_t pred_label = pt.id;
        uint32_t tgt_label = pt.label;
        if (pred_label == 0) // Unlabelled due to minimum cluster size
            continue;
        if (pred_to_tgt.find(pred_label) == pred_to_tgt.end()) {
            pred_to_tgt.emplace(pred_label, std::map<uint32_t, int>());
        }
        if (pred_to_tgt[pred_label].find(tgt_label) == pred_to_tgt[pred_label].end()) {
            pred_to_tgt[pred_label][tgt_label] = 1;
        } else {
            pred_to_tgt[pred_label][tgt_label]++;
        }
    }

    for (const auto &element : pred_to_tgt) {
        std::map<uint32_t, int> targets = element.second;
        int total = 0;
        float entropy = 0.0;
        for (auto label : targets) {
            total += label.second;
        }
        if (total > 0) {
            for (auto label : targets) {
                if (label.second > 0) {
                    entropy -= label.second / (1.0*total) * log (label.second / (1.0*total)); 
                }
            }
        }
        // printf("Per-cluster USE entropy: %f\n", entropy);
        USE += entropy;
    }
    return USE;
}
//
// function for OSE

float evalOSE(pcl::PointCloud<PointType>::Ptr cloud_in,
             std::vector<pcl::PointIndices> &cluster_indices) {
    float OSE = 0.0;
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr cec_tgt(new pcl::PointCloud<pcl::PointXYZINormal>());
    copyPointCloudCEC(*cloud_in, *cec_tgt);
    cluster_extractor_.setInputCloud(cec_tgt);
    cluster_extractor_.setConditionFunction(&isSameCluster);
    cluster_extractor_.setMinClusterSize(min_cluster_size);
    cluster_extractor_.setMaxClusterSize(max_cluster_size);
    cluster_extractor_.setClusterTolerance(gt_clustering_tol);
    cluster_extractor_.segment(cluster_indices);

    // compute OSE
    std::map<uint32_t, int> pred_count;
    for (auto cluster : cluster_indices) {
        pred_count.clear();
        size_t cluster_size = cluster.indices.size();
        float entropy = 0.0;
        for (auto idx : cluster.indices) {
            uint32_t pred_label = cloud_in->points[idx].id;
            if (pred_count.find(pred_label) == pred_count.end()) {
                pred_count[pred_label] = 1;
            } else {
                pred_count[pred_label]++;
            }               
        }
        for (auto element : pred_count) {
            // printf("element second: %d, cluster_size: %d\n", element.second, (int)cluster_size);
            entropy -= element.second / (1.0*cluster_size) * log(element.second / (1.0*cluster_size));
        }
        // printf("Per-cluster OSE entropy: %f\n", entropy);
        OSE += entropy;
    }

    // Checking clustering result
    printf("GT: # of clusters: %d\n", (int)cluster_indices.size());
    pcl::PointCloud<pcl::PointXYZL>::Ptr test_pcl(new pcl::PointCloud<pcl::PointXYZL>());
    test_pcl->points.resize(cec_tgt->points.size());
    for (auto cluster : cluster_indices) {
        uint label = rand() % 1000;
        for (auto idx : cluster.indices) {
            // visualization
            test_pcl->points[idx].x = cec_tgt->points[idx].x;
            test_pcl->points[idx].y = cec_tgt->points[idx].y;
            test_pcl->points[idx].z = cec_tgt->points[idx].z;
            test_pcl->points[idx].label = label;
        }
    }
    if (cloud_test_pub.getNumSubscribers() > 0) {
        sensor_msgs::PointCloud2 test_cloud;
        pcl::toROSMsg(*test_pcl, test_cloud);
        test_cloud.header = cloud_header;
        cloud_test_pub.publish(test_cloud);    
    }
    return OSE;
}

commented

Thanks. It will help me a lot.

Thank you for your nice work and for providing above evaluation code. However, I still have a few questions:

  1. What value is the distance threshold gt_clustering_tol?
  2. Is instance ID from ground truth used (is variable debug true or false)?
  3. What values are min_cluster_size and max_cluster_size?
  4. What happens with ground points? They are not explicitly removed in the code above. Are they handled as regular ground truth cluster(s) while evaluating over-segmentation entropy?

@Minho5Oh Friendly ping

Maybe at least Number 1. would be great to know and probably easy to answer for you. Thanks.

@AndreasR30 Sorry for late reply~!

Answer for 1, 3

  • gt_clustering_tol = 1.0m
  • min_cluster_size= 10 (same as AOS module parameter)
  • max_cluster_size= 300000 (same as AOS module parameter)

Answer for 2

  • Ground truth labels are not used in the AOS module. However, for evaluation purposes, gt label numbers were used to calculate OSE and USE. The concepts of OSE and USE can be found in the paper!
  • Actually the "debug" variable doesn't work... (sorry for the uncleaned version...)

Answer for 4
no. You can check it in the "Results and Discussion" section of the paper. We evaluate the performance using a ground segmentation algorithms (GPF and TGS).

I've been very busy these days, and it took a long time to find the evaluation code, so I was late in responding.
Sorry :(....