HungarianAlg.cpp
Go to the documentation of this file.
00001 // Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
00002 // Refer to README.md in this directory.
00003 
00004 #include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h>
00005 #include <limits>
00006 
00007 AssignmentProblemSolver::AssignmentProblemSolver() {}
00008 
00009 AssignmentProblemSolver::~AssignmentProblemSolver() {}
00010 
00011 track_t AssignmentProblemSolver::Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns,
00012                                        std::vector<int>& assignment, TMethod Method)
00013 {
00014   assignment.resize(nOfRows, -1);
00015 
00016   track_t cost = 0;
00017 
00018   switch (Method)
00019   {
00020   case optimal:
00021     assignmentoptimal(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
00022     break;
00023 
00024   case many_forbidden_assignments:
00025     assignmentsuboptimal1(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
00026     break;
00027 
00028   case without_forbidden_assignments:
00029     assignmentsuboptimal2(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
00030     break;
00031   }
00032 
00033   return cost;
00034 }
00035 // --------------------------------------------------------------------------
00036 // Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
00037 // --------------------------------------------------------------------------
00038 void AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track_t& cost,
00039                                                 const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
00040 {
00041   // Generate distance cv::Matrix
00042   // and check cv::Matrix elements positiveness :)
00043 
00044   // Total elements number
00045   size_t nOfElements = nOfRows * nOfColumns;
00046   // Memory allocation
00047   track_t* distMatrix = (track_t*)malloc(nOfElements * sizeof(track_t));
00048   // Pointer to last element
00049   track_t* distMatrixEnd = distMatrix + nOfElements;
00050 
00051   for (size_t row = 0; row < nOfElements; row++)
00052   {
00053     track_t value = distMatrixIn[row];
00054     assert(value >= 0);
00055     distMatrix[row] = value;
00056   }
00057 
00058   // Memory allocation
00059   bool* coveredColumns = (bool*)calloc(nOfColumns, sizeof(bool));
00060   bool* coveredRows = (bool*)calloc(nOfRows, sizeof(bool));
00061   bool* starMatrix = (bool*)calloc(nOfElements, sizeof(bool));
00062   bool* primeMatrix = (bool*)calloc(nOfElements, sizeof(bool));
00063   bool* newStarMatrix = (bool*)calloc(nOfElements, sizeof(bool)); /* used in step4 */
00064 
00065   /* preliminary steps */
00066   if (nOfRows <= nOfColumns)
00067   {
00068     for (size_t row = 0; row < nOfRows; row++)
00069     {
00070       /* find the smallest element in the row */
00071       track_t* distMatrixTemp = distMatrix + row;
00072       track_t minValue = *distMatrixTemp;
00073       distMatrixTemp += nOfRows;
00074       while (distMatrixTemp < distMatrixEnd)
00075       {
00076         track_t value = *distMatrixTemp;
00077         if (value < minValue)
00078         {
00079           minValue = value;
00080         }
00081         distMatrixTemp += nOfRows;
00082       }
00083       /* subtract the smallest element from each element of the row */
00084       distMatrixTemp = distMatrix + row;
00085       while (distMatrixTemp < distMatrixEnd)
00086       {
00087         *distMatrixTemp -= minValue;
00088         distMatrixTemp += nOfRows;
00089       }
00090     }
00091     /* Steps 1 and 2a */
00092     for (size_t row = 0; row < nOfRows; row++)
00093     {
00094       for (size_t col = 0; col < nOfColumns; col++)
00095       {
00096         if (distMatrix[row + nOfRows * col] == 0)
00097         {
00098           if (!coveredColumns[col])
00099           {
00100             starMatrix[row + nOfRows * col] = true;
00101             coveredColumns[col] = true;
00102             break;
00103           }
00104         }
00105       }
00106     }
00107   }
00108   else /* if(nOfRows > nOfColumns) */
00109   {
00110     for (size_t col = 0; col < nOfColumns; col++)
00111     {
00112       /* find the smallest element in the column */
00113       track_t* distMatrixTemp = distMatrix + nOfRows * col;
00114       track_t* columnEnd = distMatrixTemp + nOfRows;
00115       track_t minValue = *distMatrixTemp++;
00116       while (distMatrixTemp < columnEnd)
00117       {
00118         track_t value = *distMatrixTemp++;
00119         if (value < minValue)
00120         {
00121           minValue = value;
00122         }
00123       }
00124       /* subtract the smallest element from each element of the column */
00125       distMatrixTemp = distMatrix + nOfRows * col;
00126       while (distMatrixTemp < columnEnd)
00127       {
00128         *distMatrixTemp++ -= minValue;
00129       }
00130     }
00131     /* Steps 1 and 2a */
00132     for (size_t col = 0; col < nOfColumns; col++)
00133     {
00134       for (size_t row = 0; row < nOfRows; row++)
00135       {
00136         if (distMatrix[row + nOfRows * col] == 0)
00137         {
00138           if (!coveredRows[row])
00139           {
00140             starMatrix[row + nOfRows * col] = true;
00141             coveredColumns[col] = true;
00142             coveredRows[row] = true;
00143             break;
00144           }
00145         }
00146       }
00147     }
00148 
00149     for (size_t row = 0; row < nOfRows; row++)
00150     {
00151       coveredRows[row] = false;
00152     }
00153   }
00154   /* move to step 2b */
00155   step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
00156          nOfColumns, (nOfRows <= nOfColumns) ? nOfRows : nOfColumns);
00157   /* compute cost and remove invalid assignments */
00158   computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
00159   /* free allocated memory */
00160   free(distMatrix);
00161   free(coveredColumns);
00162   free(coveredRows);
00163   free(starMatrix);
00164   free(primeMatrix);
00165   free(newStarMatrix);
00166   return;
00167 }
00168 // --------------------------------------------------------------------------
00169 //
00170 // --------------------------------------------------------------------------
00171 void AssignmentProblemSolver::buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows,
00172                                                     size_t nOfColumns)
00173 {
00174   for (size_t row = 0; row < nOfRows; row++)
00175   {
00176     for (size_t col = 0; col < nOfColumns; col++)
00177     {
00178       if (starMatrix[row + nOfRows * col])
00179       {
00180         assignment[row] = static_cast<int>(col);
00181         break;
00182       }
00183     }
00184   }
00185 }
00186 // --------------------------------------------------------------------------
00187 //
00188 // --------------------------------------------------------------------------
00189 void AssignmentProblemSolver::computeassignmentcost(const assignments_t& assignment, track_t& cost,
00190                                                     const distMatrix_t& distMatrixIn, size_t nOfRows)
00191 {
00192   for (size_t row = 0; row < nOfRows; row++)
00193   {
00194     const int col = assignment[row];
00195     if (col >= 0)
00196     {
00197       cost += distMatrixIn[row + nOfRows * col];
00198     }
00199   }
00200 }
00201 
00202 // --------------------------------------------------------------------------
00203 //
00204 // --------------------------------------------------------------------------
00205 void AssignmentProblemSolver::step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
00206                                      bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
00207                                      size_t nOfRows, size_t nOfColumns, size_t minDim)
00208 {
00209   bool* starMatrixTemp, *columnEnd;
00210   /* cover every column containing a starred zero */
00211   for (size_t col = 0; col < nOfColumns; col++)
00212   {
00213     starMatrixTemp = starMatrix + nOfRows * col;
00214     columnEnd = starMatrixTemp + nOfRows;
00215     while (starMatrixTemp < columnEnd)
00216     {
00217       if (*starMatrixTemp++)
00218       {
00219         coveredColumns[col] = true;
00220         break;
00221       }
00222     }
00223   }
00224   /* move to step 3 */
00225   step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
00226          nOfColumns, minDim);
00227 }
00228 
00229 // --------------------------------------------------------------------------
00230 //
00231 // --------------------------------------------------------------------------
00232 void AssignmentProblemSolver::step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
00233                                      bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
00234                                      size_t nOfRows, size_t nOfColumns, size_t minDim)
00235 {
00236   /* count covered columns */
00237   size_t nOfCoveredColumns = 0;
00238   for (size_t col = 0; col < nOfColumns; col++)
00239   {
00240     if (coveredColumns[col])
00241     {
00242       nOfCoveredColumns++;
00243     }
00244   }
00245   if (nOfCoveredColumns == minDim)
00246   {
00247     /* algorithm finished */
00248     buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
00249   }
00250   else
00251   {
00252     /* move to step 3 */
00253     step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
00254           nOfColumns, minDim);
00255   }
00256 }
00257 
00258 // --------------------------------------------------------------------------
00259 //
00260 // --------------------------------------------------------------------------
00261 void AssignmentProblemSolver::step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
00262                                     bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
00263                                     size_t nOfRows, size_t nOfColumns, size_t minDim)
00264 {
00265   bool zerosFound = true;
00266   while (zerosFound)
00267   {
00268     zerosFound = false;
00269     for (size_t col = 0; col < nOfColumns; col++)
00270     {
00271       if (!coveredColumns[col])
00272       {
00273         for (size_t row = 0; row < nOfRows; row++)
00274         {
00275           if ((!coveredRows[row]) && (distMatrix[row + nOfRows * col] == 0))
00276           {
00277             /* prime zero */
00278             primeMatrix[row + nOfRows * col] = true;
00279             /* find starred zero in current row */
00280             size_t starCol = 0;
00281             for (; starCol < nOfColumns; starCol++)
00282             {
00283               if (starMatrix[row + nOfRows * starCol])
00284               {
00285                 break;
00286               }
00287             }
00288             if (starCol == nOfColumns) /* no starred zero found */
00289             {
00290               /* move to step 4 */
00291               step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows,
00292                     nOfRows, nOfColumns, minDim, row, col);
00293               return;
00294             }
00295             else
00296             {
00297               coveredRows[row] = true;
00298               coveredColumns[starCol] = false;
00299               zerosFound = true;
00300               break;
00301             }
00302           }
00303         }
00304       }
00305     }
00306   }
00307   /* move to step 5 */
00308   step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
00309         nOfColumns, minDim);
00310 }
00311 
00312 // --------------------------------------------------------------------------
00313 //
00314 // --------------------------------------------------------------------------
00315 void AssignmentProblemSolver::step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
00316                                     bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
00317                                     size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col)
00318 {
00319   const size_t nOfElements = nOfRows * nOfColumns;
00320   /* generate temporary copy of starMatrix */
00321   for (size_t n = 0; n < nOfElements; n++)
00322   {
00323     newStarMatrix[n] = starMatrix[n];
00324   }
00325   /* star current zero */
00326   newStarMatrix[row + nOfRows * col] = true;
00327   /* find starred zero in current column */
00328   size_t starCol = col;
00329   size_t starRow = 0;
00330   for (; starRow < nOfRows; starRow++)
00331   {
00332     if (starMatrix[starRow + nOfRows * starCol])
00333     {
00334       break;
00335     }
00336   }
00337   while (starRow < nOfRows)
00338   {
00339     /* unstar the starred zero */
00340     newStarMatrix[starRow + nOfRows * starCol] = false;
00341     /* find primed zero in current row */
00342     size_t primeRow = starRow;
00343     size_t primeCol = 0;
00344     for (; primeCol < nOfColumns; primeCol++)
00345     {
00346       if (primeMatrix[primeRow + nOfRows * primeCol])
00347       {
00348         break;
00349       }
00350     }
00351     /* star the primed zero */
00352     newStarMatrix[primeRow + nOfRows * primeCol] = true;
00353     /* find starred zero in current column */
00354     starCol = primeCol;
00355     for (starRow = 0; starRow < nOfRows; starRow++)
00356     {
00357       if (starMatrix[starRow + nOfRows * starCol])
00358       {
00359         break;
00360       }
00361     }
00362   }
00363   /* use temporary copy as new starMatrix */
00364   /* delete all primes, uncover all rows */
00365   for (size_t n = 0; n < nOfElements; n++)
00366   {
00367     primeMatrix[n] = false;
00368     starMatrix[n] = newStarMatrix[n];
00369   }
00370   for (size_t n = 0; n < nOfRows; n++)
00371   {
00372     coveredRows[n] = false;
00373   }
00374   /* move to step 2a */
00375   step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
00376          nOfColumns, minDim);
00377 }
00378 
00379 // --------------------------------------------------------------------------
00380 //
00381 // --------------------------------------------------------------------------
00382 void AssignmentProblemSolver::step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
00383                                     bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
00384                                     size_t nOfRows, size_t nOfColumns, size_t minDim)
00385 {
00386   /* find smallest uncovered element h */
00387   float h = std::numeric_limits<track_t>::max();
00388   for (size_t row = 0; row < nOfRows; row++)
00389   {
00390     if (!coveredRows[row])
00391     {
00392       for (size_t col = 0; col < nOfColumns; col++)
00393       {
00394         if (!coveredColumns[col])
00395         {
00396           const float value = distMatrix[row + nOfRows * col];
00397           if (value < h)
00398           {
00399             h = value;
00400           }
00401         }
00402       }
00403     }
00404   }
00405   /* add h to each covered row */
00406   for (size_t row = 0; row < nOfRows; row++)
00407   {
00408     if (coveredRows[row])
00409     {
00410       for (size_t col = 0; col < nOfColumns; col++)
00411       {
00412         distMatrix[row + nOfRows * col] += h;
00413       }
00414     }
00415   }
00416   /* subtract h from each uncovered column */
00417   for (size_t col = 0; col < nOfColumns; col++)
00418   {
00419     if (!coveredColumns[col])
00420     {
00421       for (size_t row = 0; row < nOfRows; row++)
00422       {
00423         distMatrix[row + nOfRows * col] -= h;
00424       }
00425     }
00426   }
00427   /* move to step 3 */
00428   step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
00429         nOfColumns, minDim);
00430 }
00431 
00432 // --------------------------------------------------------------------------
00433 // Computes a suboptimal solution. Good for cases without forbidden assignments.
00434 // --------------------------------------------------------------------------
00435 void AssignmentProblemSolver::assignmentsuboptimal2(assignments_t& assignment, track_t& cost,
00436                                                     const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
00437 {
00438   /* make working copy of distance Matrix */
00439   const size_t nOfElements = nOfRows * nOfColumns;
00440   float* distMatrix = (float*)malloc(nOfElements * sizeof(float));
00441   for (size_t n = 0; n < nOfElements; n++)
00442   {
00443     distMatrix[n] = distMatrixIn[n];
00444   }
00445 
00446   /* recursively search for the minimum element and do the assignment */
00447   for (;;)
00448   {
00449     /* find minimum distance observation-to-track pair */
00450     float minValue = std::numeric_limits<track_t>::max();
00451     size_t tmpRow = 0;
00452     size_t tmpCol = 0;
00453     for (size_t row = 0; row < nOfRows; row++)
00454     {
00455       for (size_t col = 0; col < nOfColumns; col++)
00456       {
00457         const float value = distMatrix[row + nOfRows * col];
00458         if (value != std::numeric_limits<track_t>::max() && (value < minValue))
00459         {
00460           minValue = value;
00461           tmpRow = row;
00462           tmpCol = col;
00463         }
00464       }
00465     }
00466 
00467     if (minValue != std::numeric_limits<track_t>::max())
00468     {
00469       assignment[tmpRow] = static_cast<int>(tmpCol);
00470       cost += minValue;
00471       for (size_t n = 0; n < nOfRows; n++)
00472       {
00473         distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
00474       }
00475       for (size_t n = 0; n < nOfColumns; n++)
00476       {
00477         distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
00478       }
00479     }
00480     else
00481     {
00482       break;
00483     }
00484   }
00485 
00486   free(distMatrix);
00487 }
00488 // --------------------------------------------------------------------------
00489 // Computes a suboptimal solution. Good for cases with many forbidden assignments.
00490 // --------------------------------------------------------------------------
00491 void AssignmentProblemSolver::assignmentsuboptimal1(assignments_t& assignment, track_t& cost,
00492                                                     const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
00493 {
00494   /* make working copy of distance Matrix */
00495   const size_t nOfElements = nOfRows * nOfColumns;
00496   float* distMatrix = (float*)malloc(nOfElements * sizeof(float));
00497   for (size_t n = 0; n < nOfElements; n++)
00498   {
00499     distMatrix[n] = distMatrixIn[n];
00500   }
00501 
00502   /* allocate memory */
00503   int* nOfValidObservations = (int*)calloc(nOfRows, sizeof(int));
00504   int* nOfValidTracks = (int*)calloc(nOfColumns, sizeof(int));
00505 
00506   /* compute number of validations */
00507   bool infiniteValueFound = false;
00508   bool finiteValueFound = false;
00509   for (size_t row = 0; row < nOfRows; row++)
00510   {
00511     for (size_t col = 0; col < nOfColumns; col++)
00512     {
00513       if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
00514       {
00515         nOfValidTracks[col] += 1;
00516         nOfValidObservations[row] += 1;
00517         finiteValueFound = true;
00518       }
00519       else
00520       {
00521         infiniteValueFound = true;
00522       }
00523     }
00524   }
00525 
00526   if (infiniteValueFound)
00527   {
00528     if (!finiteValueFound)
00529     {
00530       return;
00531     }
00532     bool repeatSteps = true;
00533 
00534     while (repeatSteps)
00535     {
00536       repeatSteps = false;
00537 
00538       /* step 1: reject assignments of multiply validated tracks to singly validated observations                */
00539       for (size_t col = 0; col < nOfColumns; col++)
00540       {
00541         bool singleValidationFound = false;
00542         for (size_t row = 0; row < nOfRows; row++)
00543         {
00544           if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() &&
00545               (nOfValidObservations[row] == 1))
00546           {
00547             singleValidationFound = true;
00548             break;
00549           }
00550 
00551           if (singleValidationFound)
00552           {
00553             for (size_t row = 0; row < nOfRows; row++)
00554               if ((nOfValidObservations[row] > 1) &&
00555                   distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
00556               {
00557                 distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();
00558                 nOfValidObservations[row] -= 1;
00559                 nOfValidTracks[col] -= 1;
00560                 repeatSteps = true;
00561               }
00562           }
00563         }
00564       }
00565 
00566       /* step 2: reject assignments of multiply validated observations to singly validated tracks */
00567       if (nOfColumns > 1)
00568       {
00569         for (size_t row = 0; row < nOfRows; row++)
00570         {
00571           bool singleValidationFound = false;
00572           for (size_t col = 0; col < nOfColumns; col++)
00573           {
00574             if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() && (nOfValidTracks[col] == 1))
00575             {
00576               singleValidationFound = true;
00577               break;
00578             }
00579           }
00580 
00581           if (singleValidationFound)
00582           {
00583             for (size_t col = 0; col < nOfColumns; col++)
00584             {
00585               if ((nOfValidTracks[col] > 1) && distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
00586               {
00587                 distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();
00588                 nOfValidObservations[row] -= 1;
00589                 nOfValidTracks[col] -= 1;
00590                 repeatSteps = true;
00591               }
00592             }
00593           }
00594         }
00595       }
00596     } /* while(repeatSteps) */
00597 
00598     /* for each multiply validated track that validates only with singly validated  */
00599     /* observations, choose the observation with minimum distance */
00600     for (size_t row = 0; row < nOfRows; row++)
00601     {
00602       if (nOfValidObservations[row] > 1)
00603       {
00604         bool allSinglyValidated = true;
00605         float minValue = std::numeric_limits<track_t>::max();
00606         size_t tmpCol = 0;
00607         for (size_t col = 0; col < nOfColumns; col++)
00608         {
00609           const float value = distMatrix[row + nOfRows * col];
00610           if (value != std::numeric_limits<track_t>::max())
00611           {
00612             if (nOfValidTracks[col] > 1)
00613             {
00614               allSinglyValidated = false;
00615               break;
00616             }
00617             else if ((nOfValidTracks[col] == 1) && (value < minValue))
00618             {
00619               tmpCol = col;
00620               minValue = value;
00621             }
00622           }
00623         }
00624 
00625         if (allSinglyValidated)
00626         {
00627           assignment[row] = static_cast<int>(tmpCol);
00628           cost += minValue;
00629           for (size_t n = 0; n < nOfRows; n++)
00630           {
00631             distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
00632           }
00633           for (size_t n = 0; n < nOfColumns; n++)
00634           {
00635             distMatrix[row + nOfRows * n] = std::numeric_limits<track_t>::max();
00636           }
00637         }
00638       }
00639     }
00640 
00641     // for each multiply validated observation that validates only with singly validated  track, choose the track with
00642     // minimum distance
00643     for (size_t col = 0; col < nOfColumns; col++)
00644     {
00645       if (nOfValidTracks[col] > 1)
00646       {
00647         bool allSinglyValidated = true;
00648         float minValue = std::numeric_limits<track_t>::max();
00649         size_t tmpRow = 0;
00650         for (size_t row = 0; row < nOfRows; row++)
00651         {
00652           const float value = distMatrix[row + nOfRows * col];
00653           if (value != std::numeric_limits<track_t>::max())
00654           {
00655             if (nOfValidObservations[row] > 1)
00656             {
00657               allSinglyValidated = false;
00658               break;
00659             }
00660             else if ((nOfValidObservations[row] == 1) && (value < minValue))
00661             {
00662               tmpRow = row;
00663               minValue = value;
00664             }
00665           }
00666         }
00667 
00668         if (allSinglyValidated)
00669         {
00670           assignment[tmpRow] = static_cast<int>(col);
00671           cost += minValue;
00672           for (size_t n = 0; n < nOfRows; n++)
00673           {
00674             distMatrix[n + nOfRows * col] = std::numeric_limits<track_t>::max();
00675           }
00676           for (size_t n = 0; n < nOfColumns; n++)
00677           {
00678             distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
00679           }
00680         }
00681       }
00682     }
00683   } /* if(infiniteValueFound) */
00684 
00685   /* now, recursively search for the minimum element and do the assignment */
00686   for (;;)
00687   {
00688     /* find minimum distance observation-to-track pair */
00689     float minValue = std::numeric_limits<track_t>::max();
00690     size_t tmpRow = 0;
00691     size_t tmpCol = 0;
00692     for (size_t row = 0; row < nOfRows; row++)
00693     {
00694       for (size_t col = 0; col < nOfColumns; col++)
00695       {
00696         const float value = distMatrix[row + nOfRows * col];
00697         if (value != std::numeric_limits<track_t>::max() && (value < minValue))
00698         {
00699           minValue = value;
00700           tmpRow = row;
00701           tmpCol = col;
00702         }
00703       }
00704     }
00705 
00706     if (minValue != std::numeric_limits<track_t>::max())
00707     {
00708       assignment[tmpRow] = static_cast<int>(tmpCol);
00709       cost += minValue;
00710       for (size_t n = 0; n < nOfRows; n++)
00711       {
00712         distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
00713       }
00714       for (size_t n = 0; n < nOfColumns; n++)
00715       {
00716         distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
00717       }
00718     }
00719     else
00720     {
00721       break;
00722     }
00723   }
00724 
00725   /* free allocated memory */
00726   free(nOfValidObservations);
00727   free(nOfValidTracks);
00728   free(distMatrix);
00729 }


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat Jun 8 2019 20:53:15