Robust isometry initialization

micmacIGN · Dec 2, 2024 · 4edb8fb · 4edb8fb
1 parent 6db4bce
commit 4edb8fb
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 11 deletions.
diff --git a/MMVII/include/MMVII_Geom3D.h b/MMVII/include/MMVII_Geom3D.h
@@ -405,6 +405,8 @@ cPt3dr  RobustBundleInters(const std::vector<tSeg3dr> & aVSeg);
 /// Compute bundle intersection using a L1 criteria with barodale, "NbSegCompl" handle to be closer to euclidian distance
 // cPt3dr  L1_BundleInters(const std::vector<tSeg3dr> & aVSeg,int NbSegCompl=0,const std::vector<tREAL8> * aVWeight = nullptr);
 
+/// Compute isometry between two 3D points vectors. at least 3 points
+tPoseR RobustIsometry(const std::vector<cPt3dr> & aPtsA, const std::vector<cPt3dr> & aPtsB);
 
 /**  Class for sampling the space of quaternion/quaternion.  Method :
  *

diff --git a/MMVII/src/Geom3D/GeomsBase3D.cpp b/MMVII/src/Geom3D/GeomsBase3D.cpp
@@ -136,7 +136,7 @@ cPt3dr  L1_BundleInters(const std::vector<tSeg3dr> & aVSeg,int NbSegCompl,const
 
 
 cPt3dr  RobustBundleInters(const std::vector<tSeg3dr> & aVSeg)
-{		     
+{
      if (aVSeg.size() == 2)
         return BundleInters(aVSeg[0],aVSeg[1],0.5);
 
@@ -158,11 +158,11 @@ cPt3dr  RobustBundleInters(const std::vector<tSeg3dr> & aVSeg)
              for (size_t  aKSeg3=0 ;  aKSeg3<aVSeg.size() ; aKSeg3++)
              {
                  if ((aKSeg3!=aKSeg1) && (aKSeg3!=aKSeg2))
-		 {
+                 {
                     aSum += aVSC[aKSeg3].Dist(aInt);
-		 }
+                 }
              }
-	     aWMin.Add(aInt,aSum);
+             aWMin.Add(aInt,aSum);
          }
      }
 
@@ -177,6 +177,31 @@ cPt3dr  BundleFixZ(const tSeg3dr & aSeg,const tREAL8 & aZ)
 }
 
 
+tPoseR RobustIsometry(const std::vector<cPt3dr> & aPtsA, const std::vector<cPt3dr> & aPtsB)
+{
+    MMVII_INTERNAL_ASSERT_strong(aPtsA.size()>2, "Isometry initialization needs at least 3 points")
+    MMVII_INTERNAL_ASSERT_strong(aPtsA.size()==aPtsB.size(), "Isometry initialization needs two sets with same number of points")
+    if (aPtsA.size() == 3)
+        return tPoseR::FromTriInAndOut(0, {aPtsA[0], aPtsA[1], aPtsA[2]}, 0, {aPtsB[0], aPtsB[1], aPtsB[2]});
+
+    // find the best 3 points
+    cWhichMin<tPoseR,tREAL8>  aIsoWMin(tPoseR(), INFINITY);
+    for (unsigned int i = 0; i < aPtsA.size()-2; ++i)
+        for (unsigned int j = i+1; j < aPtsA.size()-1; ++j)
+            for (unsigned int k = j+1; k < aPtsA.size(); ++k)
+            {
+                tTri3dr aTriA = cTriangle(aPtsA[i], aPtsA[j], aPtsA[k]);
+                tTri3dr aTriB = cTriangle(aPtsB[i], aPtsB[j], aPtsB[k]);
+                auto anIso = tPoseR::FromTriInAndOut(0, aTriA, 0, aTriB);
+                tREAL8 score = 0.;
+                for (unsigned int l = 0; l < aPtsA.size(); ++l)
+                {
+                    score += SqN2(anIso.Value(aPtsA[l])-aPtsB[l]);
+                }
+                aIsoWMin.Add(anIso, score);
+            }
+    return aIsoWMin.IndexExtre();
+}
 
 /*  *********************************************************** */
 /*                                                              */

diff --git a/MMVII/src/Topo/ctopoobsset.cpp b/MMVII/src/Topo/ctopoobsset.cpp
@@ -360,18 +360,18 @@ bool cTopoObsSetStation::initialize()
                 // 1. get all 3d vectors from this station
         tPointToVectorMap aToInstrVectorMap = toInstrVectorMap();
                 // 2. find 3 points init on ground and in previous map
-        std::vector<std::pair<cPt3dr,cPt3dr> > a3DcoordPairs; // (ground, instr) only for init points
+        std::vector<cPt3dr> aVectPtsGnd, aVectPtsInstr;
         for (auto const& [aPt, aInstrVect] : aToInstrVectorMap)
         {
             if (aPt->isInit())
-                a3DcoordPairs.push_back({*aPt->getPt(), aToInstrVectorMap[aPt]});
+            {
+                aVectPtsGnd.push_back(*aPt->getPt());
+                aVectPtsInstr.push_back(aToInstrVectorMap[aPt]);
+            };
         }
-        // TODO JM: improve with a better selection of points
-        if (a3DcoordPairs.size()>=3)
+        if (aVectPtsGnd.size()>=3)
         {
-            tTri3dr aTriInstr = cTriangle(a3DcoordPairs[0].second, a3DcoordPairs[1].second, a3DcoordPairs[2].second);
-            tTri3dr aTriGround = cTriangle(a3DcoordPairs[0].first, a3DcoordPairs[1].first, a3DcoordPairs[2].first);
-            auto anIso = tPoseR::FromTriInAndOut(0, aTriGround, 0, aTriInstr);
+            auto anIso = RobustIsometry(aVectPtsGnd, aVectPtsInstr);
             mRotVert2Instr = anIso.Rot();
             anIso.Tr() = -(anIso.Rot().Mat().Transpose()*anIso.Tr()); // Tr = origin coords
         #ifdef VERBOSE_TOPO