[visualization] PCLVisualizer::addCorrespondences/updateCorrespondences does not show all correspondences

[themightyoarfish]

Describe the bug

I use Registration::registerVisualizationCallback() to show correspondences. The code employs PCLVisualizer::addCorrespondences(). This does not lead to all correspondences getting shown. If instead I iterate over all correspondences manually and addLine() for each, all are shown as expected. Somewhere inside PCLVisualizer, items get lost.

Expected behavior

All correspondences passed to addCorrespondences() function should be shown.

Current Behavior

Not all correspondences passed to addCorrespondences() function are shown.

To Reproduce

mwe.cpp

#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>

#include <pcl/visualization/pcl_visualizer.h>

using namespace std;
using namespace pcl;
using namespace pcl::visualization;

using PointT = PointXYZ;
using CloudT = PointCloud<PointT>;
using namespace Eigen;

struct RegistrationVisualizer {

  pcl::visualization::PCLVisualizer::Ptr optimization_viewer;

  RegistrationVisualizer();

  void visualize_registration(pcl::Registration<PointT, PointT> &reg);
};

int main(int argc, char *argv[]) {
  Matrix4f transformation;
  transformation << 1, 0, 0, 6.7, 0, 1, 0, 12.3, 0, 0, 1, 0.7, 0, 0, 0, 1;

  CloudT::Ptr source{new CloudT};
  CloudT::Ptr target{new CloudT};
  io::loadPCDFile("../scene_filtered.pcd", *target);
  io::loadPCDFile("../template.pcd", *source);

  IterativeClosestPoint<PointT, PointT> icp;
  icp.setInputSource(source);
  icp.setInputTarget(target);
  icp.setMaxCorrespondenceDistance(0.1);
  RegistrationVisualizer regvis;
  regvis.visualize_registration(icp);

  icp.align(*source, transformation);
  return 0;
}

RegistrationVisualizer::RegistrationVisualizer() {
  optimization_viewer = pcl::make_shared<pcl::visualization::PCLVisualizer>();
  optimization_viewer->setWindowName("Correspondences");
}

void RegistrationVisualizer::visualize_registration(
    pcl::Registration<PointT, PointT> &reg) {
  using namespace pcl;
  using namespace pcl::visualization;

  std::function<
      pcl::Registration<PointT, PointT>::UpdateVisualizerCallbackSignature>
      callback([this](const CloudT &c1, const pcl::Indices &idxs1,
                      const CloudT &c2, const pcl::Indices &idxs2) {
        if (!idxs1.size() || !idxs2.size()) {
          return;
        }

        // turn idxs into one correspondence vector
        Correspondences correspondences;
        for (size_t i = 0; i < idxs1.size(); ++i) {
          correspondences.push_back(Correspondence(idxs1[i], idxs2[i], 1));
        }

        // viewer needs ptrs, not values (expensiveish)
        const CloudT::Ptr c1_correspondences = c1.makeShared();
        const CloudT::Ptr c2_correspondences = c2.makeShared();

        if (!this->optimization_viewer->updatePointCloud<PointT>(
                c1_correspondences, "c1")) {
          this->optimization_viewer->addPointCloud<PointT>(c1_correspondences,
                                                           "c1");
          this->optimization_viewer->setPointCloudRenderingProperties(
              PCL_VISUALIZER_COLOR, 0, 1, 1, "c1");
          this->optimization_viewer->setPointCloudRenderingProperties(
              PCL_VISUALIZER_POINT_SIZE, 3, "c1");
        }
        if (!this->optimization_viewer->updateText("Source", 10, 10)) {
          this->optimization_viewer->addText("Source", 10, 10, 20, 0, 1, 1);
        }
        if (!this->optimization_viewer->updatePointCloud<PointT>(
                c2_correspondences, "c2")) {
          this->optimization_viewer->addPointCloud<PointT>(c2_correspondences,
                                                           "c2");
          this->optimization_viewer->setPointCloudRenderingProperties(
              PCL_VISUALIZER_COLOR, 1, 1, 0, "c2");
          this->optimization_viewer->setPointCloudRenderingProperties(
              PCL_VISUALIZER_POINT_SIZE, 3, "c2");
        }
        if (!this->optimization_viewer->updateText("Target", 10, 40)) {
          this->optimization_viewer->addText("Target", 10, 40, 20, 1, 1, 0);
        }

        // works as expected
        /* int c = 0; */
        /* for (const auto& corr : correspondences) { */
        /*   const auto p1     = c1_correspondences->points[corr.index_query]; */
        /*   const auto p2     = c2_correspondences->points[corr.index_match]; */
        /*   const string name = "line" + to_string(c); */
        /*   this->optimization_viewer->removeShape(name); */
        /*   this->optimization_viewer->addLine(p1, p2, 1, 0, 0, name); */
        /*   c++; */
        /* } */

        // many lines missing
        if (!this->optimization_viewer->updateCorrespondences<PointT>(
                c1_correspondences, c2_correspondences, correspondences)) {
          this->optimization_viewer->addCorrespondences<PointT>(
              c1_correspondences, c2_correspondences, correspondences,
              "correspondences");
        }
        this->optimization_viewer->setShapeRenderingProperties(
            PCL_VISUALIZER_LINE_WIDTH, 5, "correspondences");
        this->optimization_viewer->spin();
      });
  reg.registerVisualizationCallback(callback);
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.19.8)
set(CMAKE_CXX_STANDARD 20)
find_package(PCL REQUIRED)
add_executable(mwe mwe.cpp)
target_link_libraries(mwe ${PCL_LIBRARIES})
target_include_directories(mwe PRIVATE ${PCL_INCLUDE_DIRS})

pcds:

pcds.zip

First screenshot is using updateCorrespondences(). Notice how on the right there are no correspondences.

Second screenshot I add them by hand with addLine(). Many correspondences on the right.

Your Environment (please complete the following information):

• OS: macos 12.6.5
• Compiler: Apple clang version 14.0.0 (clang-1400.0.29.202) (arm)
• PCL Version 1.13 (4d8f607)

I'm at a loss here. I don't know VTK, but the code in addCorrespondences() does not look wrong. I could verify that in https://github.com/PointCloudLibrary/pcl/blob/master/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp#L1259, the full number of found correspondences is iterated, but somehow not everything shows up.
This led me on a merry chase as I had first assumed the correspondence estimation to somehow fail, but it seems to be just the visualization.

[larshg]

Seems to be working fine on windows, with vtk 9.2 - and recently PCL master.

[larshg]

Could be a glitch in VTK? since the code in PCL looks correct and seems to work correctly as well - however it fails to visualize on your system. What VTK version do you use?

[themightyoarfish]

VTK seems to be 9.1.0, perhaps I can try a different version here.

[themightyoarfish]

I built PCL and this sample code against VTK 9.2.6 (current release) and it exhibits the same behavior, so seems to be a macos problem.

[mvieth]

I (Ubuntu 23.04, VTK 9.1.0) see the same as Lars. So yes, possibly only a problem on macos.
@themightyoarfish Can you maybe try to shuffle or reverse correspondences, to check whether it then displays only lines on the right but not in the center?

[themightyoarfish]

If I shuffle them, i get some lines everywhere, just not all.

without shuffle:

with shuffle:

[themightyoarfish]

It seems that the points on the right just happen to come later in the vector, and VTK here manages only some percentage of the entries … if I limit the display to the last half or last third or so, I see an increasing number of lines on the right being drawn.

[mvieth]

@themightyoarfish I rewrote addCorrespondences as a VTK-only test program:
vtktest.txt (rename to vtktest.cpp)
CMakeLists.txt

It should display a whole circle (for me it does). You could test what you see, and also what happens when you change n_lines. You could also use it to estimate how many lines are displayed for you (how much of the circle is visible).

[themightyoarfish]

I tried different orders of magnitude, but I always see the full circle. So the same issue doesn't seem to appear here.