example showing a degeneracy case in gtsam::Pose3AttitudeFactor

CustomAttitudeFactor.h

#include <math.h>

#include <gtsam/geometry/Pose3.h>

class CustomPose3AttitudeFactor
    : public gtsam::NoiseModelFactor1<gtsam::Pose3> {
protected:
  gtsam::Point3 nZ_, bRef_;

public:
  /** angle value error between two directions */
  static gtsam::Vector
  angleError(const gtsam::Point3& nZ,
             const gtsam::Point3& nRef,
             gtsam::OptionalJacobian<1, 3> H = boost::none) {
    const double ah = nZ.dot(nRef);
    const double e = acos(ah);
    if (H) {
      const gtsam::Matrix13 H_ah_nRef{nZ};
      const gtsam::Matrix11 H_e_ah{-1. / sqrt(1. - ah * ah)};
      *H = H_e_ah * H_ah_nRef;
    }
    return gtsam::Vector1{e};
  }

  /** vector of errors */
  gtsam::Vector attitudeError(const gtsam::Rot3& nRb,
                              gtsam::OptionalJacobian<1, 3> H) const {
    if (H) {
      gtsam::Matrix33 D_nRef_R;
      gtsam::Matrix13 D_e_nRef;
      const gtsam::Point3 nRef = nRb.rotate(bRef_, D_nRef_R);
      gtsam::Vector e = angleError(nZ_, nRef, D_e_nRef);

      (*H) = D_e_nRef * D_nRef_R;
      return e;
    } else {
      const gtsam::Point3 nRef = nRb * bRef_;
      return angleError(nZ_, nRef);
    }
  }

  /**
   * @brief Constructor
   * @param key of the Pose3 variable that will be constrained
   * @param nZ measured direction in navigation frame
   * @param model Gaussian noise model
   * @param bRef reference direction in body frame (default Z-axis)
   */
  CustomPose3AttitudeFactor(gtsam::Key key,
                            const gtsam::Point3& nZ,
                            const gtsam::SharedNoiseModel& model,
                            const gtsam::Point3& bRef = gtsam::Point3{0, 0, 1})
      : gtsam::NoiseModelFactor1<gtsam::Pose3>{model, key}
      , nZ_{nZ}
      , bRef_{bRef} {}

  ~CustomPose3AttitudeFactor() override {}

  /** vector of errors */
  gtsam::Vector evaluateError(
      const gtsam::Pose3& nTb,
      boost::optional<gtsam::Matrix&> H = boost::none) const override {
    gtsam::Vector e = attitudeError(nTb.rotation(), H);
    if (H) {
      const gtsam::Matrix H13 = *H;
      *H = gtsam::Matrix::Zero(1, 6);
      H->block<1, 3>(0, 0) = H13;
    }
    return e;
  }
};

CustomPose3AttitudeFactorExample.cpp

#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

#include "CustomAttitudeFactor.h"

int main(void) {
  gtsam::NonlinearFactorGraph graph;
  gtsam::Values initial_estimate;

  const gtsam::Point3 gravity{0., 0., 1.};
  const gtsam::Vector1 sigmas{.001};
  const gtsam::Symbol symbol{'x', 1};

  graph.push_back(CustomPose3AttitudeFactor{
      symbol, gravity, gtsam::noiseModel::Diagonal::Sigmas(sigmas)});

  initial_estimate.insert(
      symbol,
      gtsam::Pose3{gtsam::Rot3::Rx(3. * M_PI_4), gtsam::Point3{0., 0., 0.}});

  gtsam::LevenbergMarquardtParams params;
  gtsam::LevenbergMarquardtOptimizer optimizer{graph, initial_estimate, params};
  gtsam::Values estimate;
  estimate = optimizer.optimize();

  std::cout << "The pose is:" << std::endl
            << estimate.at<gtsam::Pose3>(symbol) << std::endl;
  
  return 0;
}

Pose3AttitudeFactorExample.cpp

#include <gtsam/geometry/Pose3.h>
#include <gtsam/navigation/AttitudeFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

int main(void) {
  gtsam::NonlinearFactorGraph graph;
  gtsam::Values initial_estimate;

  const gtsam::Point3 gravity{0., 0., 1.};
  const gtsam::Point2 sigmas{.001, .001};
  const gtsam::Symbol symbol{'x', 1};

  graph.push_back(
      gtsam::Pose3AttitudeFactor{symbol,
                                 gtsam::Unit3{gravity},
                                 gtsam::noiseModel::Diagonal::Sigmas(sigmas)});

  initial_estimate.insert(
      symbol, gtsam::Pose3{gtsam::Rot3::Rx(3. * M_PI_4), gtsam::Point3{0., 0., 0.}});

  gtsam::LevenbergMarquardtParams params;
  gtsam::LevenbergMarquardtOptimizer optimizer{graph, initial_estimate, params};
  gtsam::Values estimate;
  estimate = optimizer.optimize();

  std::cout << "The pose is:" << std::endl
            << estimate.at<gtsam::Pose3>(symbol) << std::endl;

  return 0;
}

Pose3AttitudeFactorTest.cpp

#include <gtsam/navigation/AttitudeFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

#include "CustomAttitudeFactor.h"

int main(void) {
  const gtsam::Point3 gravity{0., 0., 1.};
  const gtsam::Symbol symbol{'x', 1};

  const gtsam::Matrix31 z = gravity.matrix();

  std::cout << "The ground truth z-component of the pose is:"
            << std::endl
            << z << std::endl;

  std::mt19937 engine{0};

  for (int i = 0; i < 100; i++) {
    std::cout << "Running test " << i << "." << std::endl;
    const gtsam::Pose3 pose{gtsam::Rot3::Random(engine),
                            gtsam::Point3{0., 0., 0.}};

    for (int j = 0; j < 2; j++) {
      gtsam::NonlinearFactorGraph graph;
      gtsam::Values initial_estimate;

      switch (j) {
      case 0:
        graph.push_back(CustomPose3AttitudeFactor{
            symbol,
            gravity,
            gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1{.001})});
        break;
      case 1:
        graph.push_back(gtsam::Pose3AttitudeFactor{
            symbol,
            gtsam::Unit3{gravity},
            gtsam::noiseModel::Diagonal::Sigmas(gtsam::Point2{.001, .001})});
        break;
      default:
        throw std::runtime_error("Not supported.");
      }

      initial_estimate.insert(symbol, pose);

      gtsam::LevenbergMarquardtParams params;
      gtsam::LevenbergMarquardtOptimizer optimizer{
          graph, initial_estimate, params};
      gtsam::Values estimate;
      estimate = optimizer.optimize();

      const gtsam::Pose3 pose_estimate = estimate.at<gtsam::Pose3>(symbol);
      const gtsam::Matrix31 z_estimate =
          pose_estimate.rotation().matrix().block<3, 1>(0, 2);

      const std::string which =
          j ? "Pose3AttitudeFactor" : "CustomPose3AttitudeFactor";
      std::cout << "The z-component of the " << which << " pose is:"
                << std::endl
                << z_estimate << std::endl;
    }
  }

  return 0;
}

ResultsFromTests.txt

The ground truth z-component of the pose is:
0
0
1
Running test 0.
The z-component of the CustomPose3AttitudeFactor pose is:
3.35352e-10
5.02151e-11
          1
The z-component of the Pose3AttitudeFactor pose is:
 4.39839e-19
-1.01928e-19
          -1
Running test 1.
The z-component of the CustomPose3AttitudeFactor pose is:
 7.53779e-11
-1.22591e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
 2.63664e-24
-8.27181e-24
           1
Running test 2.
The z-component of the CustomPose3AttitudeFactor pose is:
-4.63799e-10
-2.49804e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-4.19043e-27
 8.17891e-27
           1
Running test 3.
The z-component of the CustomPose3AttitudeFactor pose is:
 7.89499e-10
-9.27415e-09
           1
The z-component of the Pose3AttitudeFactor pose is:
-7.96161e-24
-6.78547e-25
           1
Running test 4.
The z-component of the CustomPose3AttitudeFactor pose is:
-8.21274e-12
  5.4839e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
-4.03897e-27
-1.41364e-27
           1
Running test 5.
The z-component of the CustomPose3AttitudeFactor pose is:
1.04492e-10
2.30728e-10
          1
The z-component of the Pose3AttitudeFactor pose is:
  4.1359e-24
-2.48154e-24
           1
Running test 6.
The z-component of the CustomPose3AttitudeFactor pose is:
 3.41153e-11
-5.09197e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
8.62077e-24
6.05037e-25
          1
Running test 7.
The z-component of the CustomPose3AttitudeFactor pose is:
-4.34749e-11
 3.56353e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
-2.8399e-29
2.76101e-29
          1
Running test 8.
The z-component of the CustomPose3AttitudeFactor pose is:
-3.16862e-08
 3.22598e-08
           1
The z-component of the Pose3AttitudeFactor pose is:
6.46235e-27
4.84676e-27
         -1
Running test 9.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.00134e-11
-1.26431e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
-1.72563e-31
 6.16298e-32
           1
Running test 10.
The z-component of the CustomPose3AttitudeFactor pose is:
 7.84724e-11
-1.41195e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-1.98523e-23
 3.72231e-23
           1
Running test 11.
The z-component of the CustomPose3AttitudeFactor pose is:
-9.30836e-09
-4.51194e-09
           1
The z-component of the Pose3AttitudeFactor pose is:
-2.09521e-27
 4.39238e-27
           1
Running test 12.
The z-component of the CustomPose3AttitudeFactor pose is:
 2.75066e-10
-7.60529e-12
           1
The z-component of the Pose3AttitudeFactor pose is:
8.48183e-27
8.65955e-25
          1
Running test 13.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.37068e-10
 8.97181e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
-3.09233e-28
-3.66031e-28
           1
Running test 14.
The z-component of the CustomPose3AttitudeFactor pose is:
3.82007e-10
1.31987e-10
          1
The z-component of the Pose3AttitudeFactor pose is:
-1.48634e-25
-6.30079e-26
           1
Running test 15.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.37307e-11
 2.92505e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
 -6.7098e-21
-2.66121e-22
           1
Running test 16.
The z-component of the CustomPose3AttitudeFactor pose is:
 -6.9963e-10
-5.84151e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-2.0646e-31
2.49601e-31
          1
Running test 17.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.51639e-10
 1.61251e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
7.19957e-22
8.89089e-21
          1
Running test 18.
The z-component of the CustomPose3AttitudeFactor pose is:
 2.21232e-10
-1.16256e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-1.72563e-31
-2.46519e-31
           1
Running test 19.
The z-component of the CustomPose3AttitudeFactor pose is:
-4.39618e-11
-8.65309e-12
           1
The z-component of the Pose3AttitudeFactor pose is:
-6.46235e-27
-6.46235e-27
           1
Running test 20.
The z-component of the CustomPose3AttitudeFactor pose is:
-7.14874e-10
 5.56325e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-4.83177e-30
-6.21228e-30
          -1
Running test 21.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.38652e-10
-5.19393e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
 3.09779e-22
-8.22011e-23
           1
Running test 22.
The z-component of the CustomPose3AttitudeFactor pose is:
 6.34496e-09
-2.27984e-09
           1
The z-component of the Pose3AttitudeFactor pose is:
-4.61681e-20
-1.28455e-19
           1
Running test 23.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.05653e-10
 1.63472e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
1.03398e-25
7.75482e-26
         -1
Running test 24.
The z-component of the CustomPose3AttitudeFactor pose is:
 2.41339e-10
-1.23307e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
1.24077e-24
9.30578e-25
         -1
Running test 25.
The z-component of the CustomPose3AttitudeFactor pose is:
  4.673e-11
1.15037e-10
          1
The z-component of the Pose3AttitudeFactor pose is:
4.93038e-32
7.39557e-32
         -1
Running test 26.
The z-component of the CustomPose3AttitudeFactor pose is:
  5.3111e-08
-9.12177e-08
           1
The z-component of the Pose3AttitudeFactor pose is:
-1.90639e-19
-1.11056e-19
          -1
Running test 27.
The z-component of the CustomPose3AttitudeFactor pose is:
-7.24876e-12
-1.16199e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
 2.42338e-27
-3.63507e-27
          -1
Running test 28.
The z-component of the CustomPose3AttitudeFactor pose is:
 8.6101e-12
7.46245e-11
          1
The z-component of the Pose3AttitudeFactor pose is:
-5.80098e-21
 1.69785e-20
           1
Running test 29.
The z-component of the CustomPose3AttitudeFactor pose is:
2.26404e-10
4.58484e-11
          1
The z-component of the Pose3AttitudeFactor pose is:
 5.09081e-18
-1.63076e-17
          -1
Running test 30.
The z-component of the CustomPose3AttitudeFactor pose is:
 4.97121e-12
-3.25783e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
-2.01948e-28
 -5.6798e-29
           1
Running test 31.
The z-component of the CustomPose3AttitudeFactor pose is:
-2.23354e-11
 2.90982e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
1.11072e-26
4.74579e-27
          1
Running test 32.
The z-component of the CustomPose3AttitudeFactor pose is:
 3.14026e-10
-1.01791e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
 1.65436e-24
-6.20385e-25
          -1
Running test 33.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.26648e-07
-6.89528e-07
           1
The z-component of the Pose3AttitudeFactor pose is:
 9.09899e-24
-1.86116e-24
          -1
Running test 34.
The z-component of the CustomPose3AttitudeFactor pose is:
 1.10114e-10
-1.13547e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
9.79568e-19
9.18542e-19
          1
Running test 35.
The z-component of the CustomPose3AttitudeFactor pose is:
-5.94402e-08
 6.03073e-08
           1
The z-component of the Pose3AttitudeFactor pose is:
1.26218e-29
          0
          1
Running test 36.
The z-component of the CustomPose3AttitudeFactor pose is:
3.62794e-11
4.54401e-11
          1
The z-component of the Pose3AttitudeFactor pose is:
-9.49157e-27
 1.35305e-26
           1
Running test 37.
The z-component of the CustomPose3AttitudeFactor pose is:
 5.34712e-10
-1.99063e-09
           1
The z-component of the Pose3AttitudeFactor pose is:
-2.62533e-26
-7.06819e-27
           1
Running test 38.
The z-component of the CustomPose3AttitudeFactor pose is:
-4.13869e-10
 5.70096e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
 5.0891e-26
4.03897e-25
          1
Running test 39.
The z-component of the CustomPose3AttitudeFactor pose is:
-9.38978e-11
 3.91856e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
 -2.3006e-24
-2.79173e-24
           1
Running test 40.
The z-component of the CustomPose3AttitudeFactor pose is:
 1.36375e-06
-1.39236e-07
           1
The z-component of the Pose3AttitudeFactor pose is:
-9.08768e-28
-8.07794e-27
           1
Running test 41.
The z-component of the CustomPose3AttitudeFactor pose is:
-5.74136e-11
 -1.1036e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-5.80039e-21
  1.2802e-20
           1
Running test 42.
The z-component of the CustomPose3AttitudeFactor pose is:
2.67957e-10
3.04162e-11
          1
The z-component of the Pose3AttitudeFactor pose is:
 1.00677e-17
-3.33093e-18
          -1
Running test 43.
The z-component of the CustomPose3AttitudeFactor pose is:
 7.2521e-11
2.52397e-12
          1
The z-component of the Pose3AttitudeFactor pose is:
-8.27668e-22
 -1.2329e-20
           1
Running test 44.
The z-component of the CustomPose3AttitudeFactor pose is:
 -6.4822e-11
-2.78305e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
  5.0458e-23
-1.58405e-22
           1
Running test 45.
The z-component of the CustomPose3AttitudeFactor pose is:
1.80875e-10
1.01609e-11
          1
The z-component of the Pose3AttitudeFactor pose is:
-9.30578e-25
-7.23783e-25
          -1
Running test 46.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.83718e-11
 9.39866e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
 1.46289e-17
-1.65534e-17
           1
Running test 47.
The z-component of the CustomPose3AttitudeFactor pose is:
-7.20485e-10
 3.99797e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
1.20664e-26
2.17852e-26
          1
Running test 48.
The z-component of the CustomPose3AttitudeFactor pose is:
 -2.0368e-11
-1.19255e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-2.49669e-13
 -9.3819e-14
          -1
Running test 49.
The z-component of the CustomPose3AttitudeFactor pose is:
 1.02938e-09
-9.93597e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
-6.28624e-31
-5.81785e-30
          -1
Running test 50.
The z-component of the CustomPose3AttitudeFactor pose is:
-2.90581e-10
 2.03138e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
9.98382e-27
1.58782e-26
          1
Running test 51.
The z-component of the CustomPose3AttitudeFactor pose is:
-4.62071e-07
-8.17339e-07
           1
The z-component of the Pose3AttitudeFactor pose is:
 1.15805e-23
-4.96308e-24
          -1
Running test 52.
The z-component of the CustomPose3AttitudeFactor pose is:
 2.66247e-08
-2.90963e-08
           1
The z-component of the Pose3AttitudeFactor pose is:
-2.36658e-30
-2.14472e-30
          -1
Running test 53.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.52472e-08
 1.07196e-09
           1
The z-component of the Pose3AttitudeFactor pose is:
  -3.341e-20
-4.71053e-19
           1
Running test 54.
The z-component of the CustomPose3AttitudeFactor pose is:
-5.04272e-11
 1.51481e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-1.38051e-30
-6.90253e-31
           1
Running test 55.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.62317e-10
  3.6235e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
 1.61559e-27
-2.52435e-28
          -1
Running test 56.
The z-component of the CustomPose3AttitudeFactor pose is:
3.68432e-12
5.17947e-11
          1
The z-component of the Pose3AttitudeFactor pose is:
-3.66855e-22
-4.70128e-22
           1
Running test 57.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.44729e-11
  -2.311e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
 7.51883e-31
-6.77927e-31
           1
Running test 58.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.26125e-10
-1.25985e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-8.14256e-25
 -5.2345e-25
           1
Running test 59.
The z-component of the CustomPose3AttitudeFactor pose is:
-3.53621e-08
 1.48142e-08
           1
The z-component of the Pose3AttitudeFactor pose is:
-3.03646e-19
-7.25112e-19
          -1
Running test 60.
The z-component of the CustomPose3AttitudeFactor pose is:
2.29934e-11
-2.0333e-11
          1
The z-component of the Pose3AttitudeFactor pose is:
-6.64217e-19
-7.69407e-19
           1
Running test 61.
The z-component of the CustomPose3AttitudeFactor pose is:
-2.40702e-10
  8.5972e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
-3.69779e-32
-7.39557e-32
           1
Running test 62.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.84847e-11
-2.15623e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
-8.62336e-23
 3.75333e-23
           1
Running test 63.
The z-component of the CustomPose3AttitudeFactor pose is:
6.23811e-09
 1.0786e-08
          1
The z-component of the Pose3AttitudeFactor pose is:
-5.02221e-24
  1.4459e-23
          -1
Running test 64.
The z-component of the CustomPose3AttitudeFactor pose is:
1.32656e-10
1.58858e-12
          1
The z-component of the Pose3AttitudeFactor pose is:
-1.3902e-22
 5.7122e-22
         -1
Running test 65.
The z-component of the CustomPose3AttitudeFactor pose is:
 4.85601e-07
-6.28952e-07
           1
The z-component of the Pose3AttitudeFactor pose is:
-1.03559e-24
-7.96484e-25
           1
Running test 66.
The z-component of the CustomPose3AttitudeFactor pose is:
 1.76023e-10
-5.06602e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-1.49907e-21
  -5.333e-22
           1
Running test 67.
The z-component of the CustomPose3AttitudeFactor pose is:
-3.15153e-10
 1.88683e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
9.84874e-21
1.64939e-20
          1
Running test 68.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.76733e-10
 2.97575e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
3.97047e-23
6.51405e-24
          1
Running test 69.
The z-component of the CustomPose3AttitudeFactor pose is:
2.39504e-10
1.28761e-10
          1
The z-component of the Pose3AttitudeFactor pose is:
 7.39557e-32
-1.47911e-31
           1
Running test 70.
The z-component of the CustomPose3AttitudeFactor pose is:
 1.60991e-09
-6.80304e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-2.12538e-22
-4.43127e-22
           1
Running test 71.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.08807e-10
-1.65255e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-6.31089e-30
 9.86076e-31
           1
Running test 72.
The z-component of the CustomPose3AttitudeFactor pose is:
-6.82813e-11
   3.504e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
 5.45939e-23
-4.88037e-23
          -1
Running test 73.
The z-component of the CustomPose3AttitudeFactor pose is:
 2.65606e-09
-1.74572e-09
           1
The z-component of the Pose3AttitudeFactor pose is:
 -1.1918e-21
-1.80916e-21
           1
Running test 74.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.936e-12
 7.398e-11
         1
The z-component of the Pose3AttitudeFactor pose is:
1.50366e-19
3.91667e-21
         -1
Running test 75.
The z-component of the CustomPose3AttitudeFactor pose is:
  2.2601e-10
-5.08336e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
 1.65686e-18
-6.08907e-19
          -1
Running test 76.
The z-component of the CustomPose3AttitudeFactor pose is:
 8.5512e-11
3.47002e-10
          1
The z-component of the Pose3AttitudeFactor pose is:
 6.16298e-32
-1.38667e-32
           1
Running test 77.
The z-component of the CustomPose3AttitudeFactor pose is:
1.47271e-10
3.63223e-10
          1
The z-component of the Pose3AttitudeFactor pose is:
-4.70522e-21
 1.79598e-21
          -1
Running test 78.
The z-component of the CustomPose3AttitudeFactor pose is:
 8.88151e-11
-1.42649e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
  4.5658e-18
-5.91805e-18
           1
Running test 79.
The z-component of the CustomPose3AttitudeFactor pose is:
-2.29302e-11
 6.35609e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
-6.33305e-17
 4.70416e-18
          -1
Running test 80.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.14872e-09
 1.77676e-09
           1
The z-component of the Pose3AttitudeFactor pose is:
-9.97625e-26
-6.47245e-26
           1
Running test 81.
The z-component of the CustomPose3AttitudeFactor pose is:
7.25442e-11
1.85845e-10
          1
The z-component of the Pose3AttitudeFactor pose is:
-1.68021e-25
 6.70469e-26
           1
Running test 82.
The z-component of the CustomPose3AttitudeFactor pose is:
 3.46761e-11
-7.75296e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
1.29247e-26
1.61559e-26
         -1
Running test 83.
The z-component of the CustomPose3AttitudeFactor pose is:
 3.74399e-10
-5.93062e-09
           1
The z-component of the Pose3AttitudeFactor pose is:
-3.68556e-26
-2.33818e-27
           1
Running test 84.
The z-component of the CustomPose3AttitudeFactor pose is:
9.90101e-11
1.14316e-10
          1
The z-component of the Pose3AttitudeFactor pose is:
1.00363e-14
8.02986e-15
          1
Running test 85.
The z-component of the CustomPose3AttitudeFactor pose is:
3.67927e-08
2.57418e-08
          1
The z-component of the Pose3AttitudeFactor pose is:
 4.96308e-24
-7.23783e-24
          -1
Running test 86.
The z-component of the CustomPose3AttitudeFactor pose is:
-2.3726e-08
3.54726e-08
          1
The z-component of the Pose3AttitudeFactor pose is:
-1.86116e-24
-1.65436e-24
          -1
Running test 87.
The z-component of the CustomPose3AttitudeFactor pose is:
1.00893e-10
2.86703e-11
          1
The z-component of the Pose3AttitudeFactor pose is:
-3.60358e-18
  6.8057e-18
           1
Running test 88.
The z-component of the CustomPose3AttitudeFactor pose is:
2.17045e-11
1.59586e-10
          1
The z-component of the Pose3AttitudeFactor pose is:
-4.41452e-20
 6.01843e-21
          -1
Running test 89.
The z-component of the CustomPose3AttitudeFactor pose is:
2.49374e-10
2.08162e-12
          1
The z-component of the Pose3AttitudeFactor pose is:
 1.04044e-24
-1.53442e-22
           1
Running test 90.
The z-component of the CustomPose3AttitudeFactor pose is:
 1.08357e-10
-1.82718e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
1.91851e-27
1.14858e-27
          1
Running test 91.
The z-component of the CustomPose3AttitudeFactor pose is:
-1.01135e-10
 -1.6173e-10
           1
The z-component of the Pose3AttitudeFactor pose is:
-7.61006e-23
 3.63959e-23
           1
Running test 92.
The z-component of the CustomPose3AttitudeFactor pose is:
 3.97173e-11
-3.80017e-13
           1
The z-component of the Pose3AttitudeFactor pose is:
-1.57772e-30
  3.9443e-30
           1
Running test 93.
The z-component of the CustomPose3AttitudeFactor pose is:
3.31449e-08
2.69101e-08
          1
The z-component of the Pose3AttitudeFactor pose is:
0
0
1
Running test 94.
The z-component of the CustomPose3AttitudeFactor pose is:
-4.65429e-11
-1.17079e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
5.16988e-26
7.75482e-26
          1
Running test 95.
The z-component of the CustomPose3AttitudeFactor pose is:
2.00227e-10
9.58527e-12
          1
The z-component of the Pose3AttitudeFactor pose is:
-6.31089e-30
-1.57772e-30
           1
Running test 96.
The z-component of the CustomPose3AttitudeFactor pose is:
6.80808e-11
1.09311e-10
          1
The z-component of the Pose3AttitudeFactor pose is:
 7.94093e-23
-6.53473e-23
          -1
Running test 97.
The z-component of the CustomPose3AttitudeFactor pose is:
-3.25125e-10
 6.93802e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
2.85962e-30
1.31148e-29
          1
Running test 98.
The z-component of the CustomPose3AttitudeFactor pose is:
-2.06498e-10
 9.45156e-11
           1
The z-component of the Pose3AttitudeFactor pose is:
-1.66355e-26
-4.45296e-26
           1
Running test 99.
The z-component of the CustomPose3AttitudeFactor pose is:
-7.25965e-08
 9.95608e-09
           1
The z-component of the Pose3AttitudeFactor pose is:
-2.68978e-24
 -8.6618e-24
          -1

Notice the difference in output between Pose3AttitudeFactorExample.cpp and CustomPose3AttitudeFactorExample.cpp.
Pose3AttitudeFactorExample.cpp outputs:

The pose is:
R: [
        1, 0, 0;
        0, -1, 1.24077e-22;
        0, 4.30421e-17, -1
]
t: 0 0 0

CustomPose3AttitudeFactorExample.cpp outputs:

The pose is:
R: [
        1, 0, 0;
        0, 1, -2.35615e-11;
        0, 2.35615e-11, 1
]
t: 0 0 0

CustomPose3AttitudeFactorExample.cpp has the correct output.