Minimize matrix in Equation using OpenCV

Question

I need to minimize H in following equation:

Where H is 3x3 Matrix.
Pn is 3x1 matrix (point).
Euclidean() gives distance between 2 points.
Dn is the actual distance.

I have one initial estimate of H and m points(P0 to Pm)
I need optimize value of H such that for all m points error is minimized. (All the values in the expression are known) How can I implement this using opencv or dlib (or using boost/NLopt).

Answer 1

Although the documentation of find_optimal_parameters function of dlib library was really not enough, there is a unit test that you can find on github which shows how to use the function.

I saw the other question you've asked and seems that the solution was something different than in this question. However, here is an example, how to use the library (this is the first time I'm hearing about it) to calculate what you need or something very close to that. Probably you will need to change the DistanceQuality() function (by replacing the existing loop with two nested ones) and I'll let you do it yourself.

Please note, that everything all over the code is hardcoded, no error handling is done and the testing is done right in the main() function. There's lots of work to be done, though you can find the code working for illustration purposes.

Here we go:

#include <iostream>
#include <dlib/optimization.h>
#include <dlib/optimization/find_optimal_parameters.h>

using namespace dlib;

typedef matrix<double, 3, 1> MyPoint;

std::vector<MyPoint> points;
std::vector<double> distances;

double MyDistance(MyPoint point1, MyPoint point2)
{
   double sum = 0;
   for (int i = 0; i < 3; i++)
   {
      sum += (point1(i, 0) - point2(i, 0)) * (point1(i, 0) - point2(i, 0));
   }
   return sqrt(sum);
}

double DistanceQuality(const matrix<double, 3, 3>& H)
{
   double sum = 0;

   for (int i = 0; i < points.size() - 1; i++)
   {
      auto proj1 = H*points[i];
      auto proj2 = H*points[i+1];
      sum += abs(MyDistance(proj1, proj2) - distances[i]);
   }
   return sum;
}

matrix<double, 3, 3> VecToMatrix(matrix<double, 0, 1> vec)
{
   matrix<double, 3, 3> matrix;
   for (int i = 0; i < 9; i++)
   {
      matrix(i / 3, i % 3) = vec(i);
   }
   return matrix;
}

double test_function(matrix<double, 0, 1> H)
{
   matrix<double, 3, 3> newH = VecToMatrix(H);
   auto result = DistanceQuality(newH);
   return result;
}

int main()
{
   matrix<double, 3, 1> p1;
   matrix<double, 3, 1> p2;
   matrix<double, 3, 1> p3;

   p1 = { 1, 1, 1 };
   p2 = { 2, 2, 3 };
   p3 = { 3, 1.6, 7};

   points.push_back(p1);
   points.push_back(p2);
   points.push_back(p3);

   double d1 = 2.44949;
   double d2 = 4.142463;

   distances.push_back(d1);
   distances.push_back(d2);

   matrix<double, 0, 1> H;
   H = { 3, 1, 1,
         1, 1, 6,
         1, 4, 1 };

   matrix<double, 0, 1> H_min;
   matrix<double, 0, 1> H_max;

   H_min = { 0.5, 0.6, 0.5,
             0.5, 0.7, 0.5,
             0.8, 0.3, 0.5, };

   H_max = { 10, 10, 10,
             10, 10, 10,
             10, 10, 10, };

   dlib::find_optimal_parameters(4, 0.001, 1000, H, H_min, H_max, test_function);
   std::cout << "new H: " << std::endl << VecToMatrix(H) << std::endl;

   return 0;
}

Hope you can adapt the parameters for you specific case.