Cloud Indices, after a PassThrough filter with KeepOrganized =true, are rearranged.

[ZeroxCorbin]

Hello, thank you to all the developers who have worked on this project. It is amazing.

I am attempting to use the visualizer to select a range of indices. It is working well until I introduce a PassThrough filter. Then something happens I do not understand and would appreciate any feedback on.

I use setKeepOrganized(true); and the filter keeps the size of the points the same. The clouds are organized; 320x240. I am not processing normals. (ProcessNormals_=false;)

Since a picture is worth a thousand words I have attached two images showing the result of a select with and without a filter applied. The cloud processing code is below each image.

I am using PCL-1.12.0-AllInOne-msvc2019-win64.exe library.

This is the code that is not working as expected after applying a filter.

void ProcessPointCloud()
{
    PassThroughFilter(cloudIn_, cloudIntermediate_);

    if (ProcessNormals_)
    {
        NormalEstimationOMP(cloudIntermediate_, cloudNormals_);
    }
    else
    {
        cloudNormals_->points.clear();
    }

    pcl::copyPointCloud(*cloudIntermediate_, *cloudOut_);

    for(int i =0; i < cloudOut_->points.size(); i++){
        cloudOut_->points[i].r = 50;
        cloudOut_->points[i].g = 255;
        cloudOut_->points[i].b = 255;
    }

    std::unique_lock<std::mutex> updateLock(selectedIndicesMutex_);
    
    for(int i =0; i < selectedIndices->size(); i++){
        cloudOut_->points[(*selectedIndices)[i]].r = 255;
        cloudOut_->points[(*selectedIndices)[i]].g = 25;
        cloudOut_->points[(*selectedIndices)[i]].b = 25;
    }

    updateLock.unlock();
}

This is the code that is working as expected, no filters.

void ProcessPointCloud()
{
   // PassThroughFilter(cloudIn_, cloudIntermediate_);

    if (ProcessNormals_)
    {
        NormalEstimationOMP(cloudIn_, cloudNormals_);
    }
    else
    {
        cloudNormals_->points.clear();
    }

    pcl::copyPointCloud(*cloudIn_, *cloudOut_);

    for(int i =0; i < cloudOut_->points.size(); i++){
        cloudOut_->points[i].r = 50;
        cloudOut_->points[i].g = 255;
        cloudOut_->points[i].b = 255;
    }

    std::unique_lock<std::mutex> updateLock(selectedIndicesMutex_);

    for(int i =0; i < selectedIndices->size(); i++){
        cloudOut_->points[(*selectedIndices)[i]].r = 255;
        cloudOut_->points[(*selectedIndices)[i]].g = 25;
        cloudOut_->points[(*selectedIndices)[i]].b = 25;
    }

    updateLock.unlock();
}

Here is most of the supporting code;

void PassThroughFilter(pcl::PointCloud<Point>::Ptr cloud_in, pcl::PointCloud<Point>::Ptr filtered_cloud)
{
    Timer t;
    t.start();
    pcl::PassThrough<Point> *pass (new pcl::PassThrough<Point>);
    pass->setKeepOrganized(true);
    pass->setInputCloud(cloud_in);
    pass->setFilterFieldName("z");
    pass->setFilterLimits(1.0, 6.0);
    pass->filter(*filtered_cloud);
    std::cout << "pass: " << t.elapsedMilliseconds() << std::endl;

    delete pass;
}

void areaPickingEventOccurred (const pcl::visualization::AreaPickingEvent& event, void* viewer_void)
{
    std::unique_lock<std::mutex> updateLock(selectedIndicesMutex_);

    event.getPointsIndices(*selectedIndices);

    updateLock.unlock();
}

void DisplayCloud()
{
    std::cout << "Visualizer starting...." << std::endl;
    pcl::visualization::PCLVisualizer::Ptr viewer(
        new pcl::visualization::PCLVisualizer());
    // pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>
    //     *handler (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>(cloudOut_,"intensity"));

    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloudOut_);

    viewer->initCameraParameters();
    viewer->setShowFPS(false);

    viewer->registerKeyboardCallback (keyboardEventOccurred, (void *)viewer.get());
    viewer->registerMouseCallback (mouseEventOccurred, (void *)viewer.get());
    viewer->registerPointPickingCallback (pointPickingEventOccurred, (void *)viewer.get());
    viewer->registerAreaPickingCallback (areaPickingEventOccurred, (void *)viewer.get());

    Timer t;
    t.start();

    while (!viewer->wasStopped())
    {

        if (update_)
        {
            std::unique_lock<std::mutex> updateLock(updateModelMutex_);

            if (!viewer->updatePointCloud<pcl::PointXYZRGB>(cloudOut_, rgb, "id"))
            {
                viewer->addPointCloud<pcl::PointXYZRGB>(cloudOut_, rgb, "id");
                viewer->setPointCloudRenderingProperties(
                    pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "id");
            }

            viewer->removePointCloud("normals");
            if (cloudNormals_->points.size() > 0)
            {
                viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(
                    cloudOut_, cloudNormals_, 100, (0.1F), "normals");
            }

            std::string s = std::to_string(t.elapsedMilliseconds());
            viewer->removeShape("time");
            viewer->addText(s, 1, 1, "time");
            t.start();

            update_ = false;
            updateLock.unlock();
        }

        viewer->spinOnce(50);

        if (exit_app_)
            viewer->close();
    }

    exit_app_ = true;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr
    cloudIn_(new pcl::PointCloud<pcl::PointXYZ>);

pcl::PointCloud<pcl::PointXYZRGB>::Ptr
    cloudOut_(new pcl::PointCloud<pcl::PointXYZRGB>);

pcl::PointCloud<pcl::Normal>::Ptr
    cloudNormals_(new pcl::PointCloud<pcl::Normal>);

pcl::Indices
    *selectedIndices(new pcl::Indices);
	
bool ProcessNormals_ = false;

pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIntermediate_ (
    new pcl::PointCloud<pcl::PointXYZ>);