Trouble with Camera Calibration

[Jackson Oursland]

Hi GTSAM Users!

I am working on a multicam motion capture system to be open sourced later this summer when it is functional.  I am running into an issue with getting my camera intrinsics calibration tool functioning and request some assistance.

Using the file examples/SelfCalibrationExample.cpp as a reference for camera calibration, I have written a ROS2 node that receives images, detects AprilTags, and adds their corners to a factor graph to be optimized.  The following code is used to insert the observations:

    // detect tags
    auto img_apriltags = detector_->Detect(img);
    const unsigned int num_tags = target_tag_rows_ * target_tag_cols_;
    if(img_apriltags.size() >= 4) {
      pose_count_++;

      for (auto const &tag : img_apriltags) {
        const size_t id = tag.id;

        for (int k = 0; k < 4; ++k) {
          const auto &ip = tag.corners[k];

          gtsam::Point2 measurement{ ip.x, ip.y };
          auto measurementNoise = gtsam::noiseModel::Isotropic::Sigma(2, 1.0);
          graph_.emplace_shared<gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> >(
            measurement,
            measurementNoise,
            gtsam::Symbol('x', pose_count_),
            gtsam::Symbol('l', id*4 + k),
            gtsam::Symbol('K', 0));
        }
        tag_count_++;
      }
    }

In testing, the camera is stationary and the AprilTag calibration target is moved, resulting in a detection of 5769 AprilTags.  The following code is used to perform the optimization:

      K_ = gtsam::Cal3_S2(
        530.766, 532.796, 0.0, 295.858, 266.729);

      // Add a prior on the position of the first landmark.
      auto pointNoise = gtsam::noiseModel::Isotropic::Sigma(3, 0.1);
      graph_.addPrior(gtsam::Symbol('l', 0), target_tag_corner_positions_[0][0], pointNoise);  // add directly to graph

      // Add a prior on the calibration.
      auto calNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(9) << 500, 500, 0.1, 100, 100, 1.0, 1.0, 1.0, 1.0).finished());
      graph_.addPrior(gtsam::Symbol('K', 0), K_, calNoise);

      // Create the initial estimate to the solution
      // now including an estimate on the camera calibration parameters
      gtsam::Values initialEstimate;
      initialEstimate.insert(gtsam::Symbol('K', 0), K_);
      for (unsigned int i = 0; i < pose_count_; ++i) {
        initialEstimate.insert(gtsam::Symbol('x', i), pose_);
      }
      for (size_t j = 0; j < target_tag_corner_positions_.size(); ++j) {
        for (auto k = 0; k < 4; ++k) {
          auto pos = target_tag_corner_positions_[j][k];
          initialEstimate.insert<gtsam::Point3>(gtsam::Symbol('l', j*4 + k), pos);
        }
      }

      RCLCPP_INFO(get_logger(), "Number of factors: %d", graph_.nrFactors());

      /* Optimize the graph and print results */
      gtsam::Values result = gtsam::DoglegOptimizer(graph_, initialEstimate).optimize();
      result.print("Final results:\n");

When calibration is initiated 23079 factors are present in the graph when optimization begins.  Unfortunately, this fails with the following logs:

    ...
    [component_container-1] [INFO] [1718829282.800061657] [feynman.mocap_intrinsic_calibration_node]: frames: 1390, poses: 701, total # tags found: 5769
    [component_container-1] [INFO] [1718829347.113950259] [feynman.mocap_intrinsic_calibration_node]: Calibrating!
    [component_container-1] [INFO] [1718829347.114066760] [feynman.mocap_intrinsic_calibration_node]: Number of factors: 23079
    [component_container-1] CheiralityException: Landmark l16 behind Camera x1
    [component_container-1] CheiralityException: Landmark l17 behind Camera x1
    [component_container-1] CheiralityException: Landmark l18 behind Camera x1
    ...
    [component_container-1] CheiralityException: Landmark l17 behind Camera x700
    [component_container-1] CheiralityException: Landmark l18 behind Camera x700
    [component_container-1] CheiralityException: Landmark l19 behind Camera x700
    [component_container-1] libc++abi: terminating due to uncaught exception of type gtsam::ValuesKeyDoesNotExist: Attempting to at the key "x701", which does not exist in the Values.

My attempts to correct these issues have so far failed.

More information including the complete source file is available on the GTSAM GitHub Discussion: https://github.com/borglab/gtsam/discussions/1765

Thanks,

Jackson

[Jackson Oursland]

Hi,

The source to the project, a Docker container to build and test, and the calibration video have been uploaded to the repository: https://github.com/oursland/camera-calibration-gtsam

An off by one error was corrected and the new error is:

    ...
    [component_container-1] CheiralityException: Landmark l41 behind Camera x630
    [component_container-1] CheiralityException: Landmark l7 behind Camera x374
    [component_container-1] CheiralityException: Landmark l42 behind Camera x630
    [component_container-1] CheiralityException: Landmark l43 behind Camera x630
    [component_container-1] CheiralityException: Landmark l9 behind Camera x374
    [component_container-1] CheiralityException: Landmark l10 behind Camera x374
    [component_container-1] CheiralityException: Landmark l29 behind Camera x630
    [component_container-1] CheiralityException: Landmark l28 behind Camera x630
    [component_container-1] CheiralityException: Landmark l8 behind Camera x374
    [component_container-1] libc++abi: terminating due to uncaught exception of type gtsam::IndeterminantLinearSystemException:
    [component_container-1] Indeterminant linear system detected while working near variable
    [component_container-1] 8646911284551352622 (Symbol: x302).
    [component_container-1]
    [component_container-1] Thrown when a linear system is ill-posed.  The most common cause for this
    [component_container-1] error is having underconstrained variables.  Mathematically, the system is
    [component_container-1] underdetermined.  See the GTSAM Doxygen documentation at
    [component_container-1] http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
    [component_container-1] more information.

Thanks,

Jackson

[Dellaert, Frank]

A cheirality exception means that the point is behind the camera. There is a compile flag that you can set so that it is not fatal. But if you can avoid it, it would be better. Sometimes having small lambda in LM might throw points behind the camera during optimization so that’s one possible reason you could see it. Good luck debugging!

Best!

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Jackson Oursland <jacob.o...@gmail.com>
Sent: Thursday, June 20, 2024 2:05:06 AM
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Re: Trouble with Camera Calibration

 

You don't often get email from jacob.o...@gmail.com. Learn why this is important

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/fabbe119-f565-4e88-927a-d0d7f0cdb1bfn%40googlegroups.com.