| 109 | |
| 110 | #ifdef BASALT_INSTANTIATIONS_DOUBLE |
| 111 | TEST(LinearizationTestSuite, VoNoMargLinearizationTest) { |
| 112 | using Scalar = double; |
| 113 | static constexpr int POSE_SIZE = 6; |
| 114 | static constexpr int NUM_FRAMES = 6; |
| 115 | |
| 116 | basalt::BundleAdjustmentBase<Scalar> estimator; |
| 117 | basalt::AbsOrderMap aom; |
| 118 | |
| 119 | get_vo_estimator<Scalar>(NUM_FRAMES, estimator, aom); |
| 120 | |
| 121 | typename basalt::LinearizationBase<Scalar, POSE_SIZE>::Options options; |
| 122 | options.lb_options.huber_parameter = estimator.huber_thresh; |
| 123 | options.lb_options.obs_std_dev = estimator.obs_std_dev; |
| 124 | |
| 125 | Eigen::MatrixXd H_abs_qr, H_abs_sc, H_rel_sc; |
| 126 | Eigen::VectorXd b_abs_qr, b_abs_sc, b_rel_sc; |
| 127 | |
| 128 | options.linearization_type = basalt::LinearizationType::ABS_QR; |
| 129 | std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_abs_qr; |
| 130 | |
| 131 | l_abs_qr = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(&estimator, |
| 132 | aom, options); |
| 133 | Scalar error_abs_qr = l_abs_qr->linearizeProblem(); |
| 134 | l_abs_qr->performQR(); |
| 135 | |
| 136 | l_abs_qr->get_dense_H_b(H_abs_qr, b_abs_qr); |
| 137 | |
| 138 | options.linearization_type = basalt::LinearizationType::ABS_SC; |
| 139 | std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_abs_sc; |
| 140 | l_abs_sc = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(&estimator, |
| 141 | aom, options); |
| 142 | |
| 143 | Scalar error_abs_sc = l_abs_sc->linearizeProblem(); |
| 144 | l_abs_sc->performQR(); |
| 145 | |
| 146 | l_abs_sc->get_dense_H_b(H_abs_sc, b_abs_sc); |
| 147 | |
| 148 | options.linearization_type = basalt::LinearizationType::REL_SC; |
| 149 | std::unique_ptr<basalt::LinearizationBase<Scalar, POSE_SIZE>> l_rel_sc; |
| 150 | l_rel_sc = basalt::LinearizationBase<Scalar, POSE_SIZE>::create(&estimator, |
| 151 | aom, options); |
| 152 | |
| 153 | Scalar error_rel_sc = l_rel_sc->linearizeProblem(); |
| 154 | l_rel_sc->performQR(); |
| 155 | |
| 156 | l_rel_sc->get_dense_H_b(H_rel_sc, b_rel_sc); |
| 157 | |
| 158 | Scalar error_diff = std::abs(error_abs_qr - error_abs_sc); |
| 159 | Scalar H_diff = (H_abs_qr - H_abs_sc).norm(); |
| 160 | Scalar b_diff = (b_abs_qr - b_abs_sc).norm(); |
| 161 | |
| 162 | Scalar error_diff2 = std::abs(error_abs_qr - error_rel_sc); |
| 163 | Scalar H_diff2 = (H_abs_qr - H_rel_sc).norm(); |
| 164 | Scalar b_diff2 = (b_abs_qr - b_rel_sc).norm(); |
| 165 | |
| 166 | EXPECT_LE(error_diff, 1e-8); |
| 167 | EXPECT_LE(H_diff, 1e-8); |
| 168 | EXPECT_LE(b_diff, 1e-8); |
nothing calls this directly
no test coverage detected