Use abc terminology
							parent
							
								
									19c75cb95c
								
							
						
					
					
						commit
						be68e07ddb
					
				|  | @ -26,59 +26,56 @@ namespace gtsam { | |||
|  * Binary factor in the context of Structure from Motion (SfM). | ||||
|  * It is used to transfer points between different views based on the | ||||
|  * fundamental matrices between these views. The factor computes the error | ||||
|  * between the transferred points `pi` and `pj`, and the actual point `pk` in | ||||
|  * between the transferred points `pa` and `pb`, and the actual point `pc` in | ||||
|  * the target view. Jacobians are done using numerical differentiation. | ||||
|  */ | ||||
| template <typename F> | ||||
| class TransferFactor : public NoiseModelFactorN<F, F> { | ||||
|   EdgeKey key1_, key2_;  ///< the two EdgeKeys
 | ||||
|   Point2 pi, pj, pk;     ///< The points in the three views
 | ||||
|   Point2 pa, pb, pc;     ///< The points in the three views
 | ||||
| 
 | ||||
|  public: | ||||
|   /**
 | ||||
|    * @brief Constructor for the TransferFactor class. | ||||
|    * | ||||
|    * Uses EdgeKeys to determine how to use the two fundamental matrix unknowns | ||||
|    * F1 and F2, to transfer points pi and pj to the third view, and minimize the | ||||
|    * difference with pk. | ||||
|    * F1 and F2, to transfer points pa and pb to the third view, and minimize the | ||||
|    * difference with pc. | ||||
|    * | ||||
|    * The edge keys must represent valid edges for the transfer operation, | ||||
|    * specifically one of the following configurations: | ||||
|    * - (i, k) and (j, k) | ||||
|    * - (i, k) and (k, j) | ||||
|    * - (k, i) and (j, k) | ||||
|    * - (k, i) and (k, j) | ||||
|    * - (a, c) and (b, c) | ||||
|    * - (a, c) and (c, b) | ||||
|    * - (c, a) and (b, c) | ||||
|    * - (c, a) and (c, b) | ||||
|    * | ||||
|    * @param key1 First EdgeKey specifying F1: (i, k) or (k, i). | ||||
|    * @param key2 Second EdgeKey specifying F2: (j, k) or (k, j). | ||||
|    * @param pi The point in the first view (i). | ||||
|    * @param pj The point in the second view (j). | ||||
|    * @param pk The point in the third (and transfer target) view (k). | ||||
|    * @param key1 First EdgeKey specifying F1: (a, c) or (c, a). | ||||
|    * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b). | ||||
|    * @param pa The point in the first view (a). | ||||
|    * @param pb The point in the second view (b). | ||||
|    * @param pc The point in the third (and transfer target) view (c). | ||||
|    * @param model An optional SharedNoiseModel that defines the noise model | ||||
|    *              for this factor. Defaults to nullptr. | ||||
|    */ | ||||
|   TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pi, const Point2& pj, | ||||
|                  const Point2& pk, const SharedNoiseModel& model = nullptr) | ||||
|   TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pa, const Point2& pb, | ||||
|                  const Point2& pc, const SharedNoiseModel& model = nullptr) | ||||
|       : NoiseModelFactorN<F, F>(model, key1, key2), | ||||
|         key1_(key1), | ||||
|         key2_(key2), | ||||
|         pi(pi), | ||||
|         pj(pj), | ||||
|         pk(pk) {} | ||||
|         pa(pa), | ||||
|         pb(pb), | ||||
|         pc(pc) {} | ||||
| 
 | ||||
|   // Create Matrix3 objects based on EdgeKey configurations
 | ||||
|   static std::pair<Matrix3, Matrix3> getMatrices(const EdgeKey& key1, | ||||
|                                                  const F& F1, | ||||
|                                                  const EdgeKey& key2, | ||||
|                                                  const F& F2) { | ||||
|     // Fill Fki and Fkj based on EdgeKey configurations
 | ||||
|     if (key1.i() == key2.i()) { | ||||
|   std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const { | ||||
|     // Fill Fca and Fcb based on EdgeKey configurations
 | ||||
|     if (key1_.i() == key2_.i()) { | ||||
|       return {F1.matrix(), F2.matrix()}; | ||||
|     } else if (key1.i() == key2.j()) { | ||||
|     } else if (key1_.i() == key2_.j()) { | ||||
|       return {F1.matrix(), F2.matrix().transpose()}; | ||||
|     } else if (key1.j() == key2.i()) { | ||||
|     } else if (key1_.j() == key2_.i()) { | ||||
|       return {F1.matrix().transpose(), F2.matrix()}; | ||||
|     } else if (key1.j() == key2.j()) { | ||||
|     } else if (key1_.j() == key2_.j()) { | ||||
|       return {F1.matrix().transpose(), F2.matrix().transpose()}; | ||||
|     } else { | ||||
|       throw std::runtime_error( | ||||
|  | @ -91,13 +88,13 @@ class TransferFactor : public NoiseModelFactorN<F, F> { | |||
|                        OptionalMatrixType H1 = nullptr, | ||||
|                        OptionalMatrixType H2 = nullptr) const override { | ||||
|     std::function<Point2(F, F)> transfer = [&](const F& F1, const F& F2) { | ||||
|       auto [Fki, Fkj] = getMatrices(key1_, F1, key2_, F2); | ||||
|       return Transfer(Fki, pi, Fkj, pj); | ||||
|       auto [Fca, Fcb] = getMatrices(F1, F2); | ||||
|       return Transfer(Fca, pa, Fcb, pb); | ||||
|     }; | ||||
|     if (H1) *H1 = numericalDerivative21<Point2, F, F>(transfer, F1, F2); | ||||
|     if (H2) *H2 = numericalDerivative22<Point2, F, F>(transfer, F1, F2); | ||||
|     return transfer(F1, F2) - pk; | ||||
|     return transfer(F1, F2) - pc; | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
| }  // namespace gtsam
 | ||||
|  | @ -71,7 +71,7 @@ TEST(TransferFactor, Jacobians) { | |||
|       factor2{key20, key12, p[0], p[1], p[2]}; | ||||
| 
 | ||||
|   // Check that getMatrices is correct
 | ||||
|   auto [Fki, Fkj] = factor2.getMatrices(key20, triplet.Fca, key12, triplet.Fbc); | ||||
|   auto [Fki, Fkj] = factor2.getMatrices(triplet.Fca, triplet.Fbc); | ||||
|   EXPECT(assert_equal<Matrix3>(triplet.Fca.matrix(), Fki)); | ||||
|   EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj)); | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue