Obtain position of the object data from Homography matrix

edit

Hi community! Im using ros noetic and gazebo 11 in unbuntu 20.04.

Im using rosbot packages and running find_objects_2d package present in its tutorial. The issue is that this package uses reads Homography matrix data received to obtain object's orientation with some processing on the pixels.

We understand that the Homography matrix should bring us to the position of the object refered to robot's base_link, but the question is, how to process the data from this matrix? Is a 9x9 one. Our objective is to obtain object position so we can send a goal to move_base package, because we want move_base to be in charge of the motion of our robot.

Here are some examples, knowing robot's position ( 0,0,0) and object position, the results are wrong, they were obtained with the code below that we were trying to apply. The desired result is the indicated by x and y (that are the real coordenates where our object was located):

//Object 2
//0.6100760698318481, -0.059579864144325256, -0.0003138711617793888, 
//-0.02506314031779766, 0.6501667499542236, -9.795412915991619e-05, 
//188.61143493652344, 101.26304626464844, 1.0

//x = 3
//y= 0
// Result: 
//Distance: 0.396946
// Angle: -0.097351

//Object 3
//0.37898701429367065, -0.0909111276268959, -0.00037208336289040744, 
//0.022685814648866653, 0.5084739327430725, 9.063976176548749e-05, 
//245.93141174316406, 139.2412109375, 1.0
//Result:
//Distance: 0.192897
//Angle: -0.235431

#include <iostream>
#include <vector>
#include <cmath> // Incluir cmath para utilizar la función abs y atan2

using namespace std;

int main() {
  vector<double> matrix = {
    0.37898701429367065, -0.0909111276268959, -0.00037208336289040744, 
    0.022685814648866653, 0.5084739327430725, 9.063976176548749e-05, 
    245.93141174316406, 139.2412109375, 1.0
  };

  double norm = sqrt(matrix[3]*matrix[3] + matrix[4]*matrix[4] + matrix[5]*matrix[5]); // Cálculo de la norma

  double distance = abs(matrix[0]) * norm;
  double angle = atan2(matrix[1], matrix[0]);

  cout << "Distance: " << distance << endl;
  cout << "Angle: " << angle << endl;

  return 0;
}

Could you tell me another option to detect object position referd to my robot using camera, tof sensor or lidar?

Thanks!