I am finding a problem to define the exact position of a simple bluesquare using camera and laserscan, I am using ros noetic and turtlebot for the simulation. I tried a lot of solution in my vision module but I still can't get it right. The best outcomes that I had was that the correct position will be calculated correctly only if the camera is facing the object with angle 0, that means the object has to be in the middle of the image, otherwise it will rotate with it or follow the wall line instead.

I am not sure if the problem is a miscalculation in the x and y values that I defined or is it when setting the position for blueSquareMap

This is how I do the transformation: transformStamped = tfBuffer.lookupTransform("map", "base_scan", ros::Time(0));

and this is the code I have now:

geometry_msgs::PointStamped get_point()
{
     geometry_msgs::PointStamped p;
     p.header.frame_id = "map";
     p.header.stamp = ros::Time::now();

     p.point.x = transformStamped.transform.translation.x;
     p.point.y = transformStamped.transform.translation.y;
     p.point.z = transformStamped.transform.translation.z;

return p;
}


// Extract blue square marker positions
for (const auto &contour : blueContours)
{
    double area = cv::contourArea(contour); // Get how big is the object

    if (area > 100)
    {
        // Calculate coords of the middle
        cv::Rect boundingRect = cv::boundingRect(contour);
        geometry_msgs::Point markerPoint;
        markerPoint.x = boundingRect.x + boundingRect.width / 2;
        markerPoint.y = boundingRect.y + boundingRect.height / 2;
        markerPoint.z = 0.0;

        // Adjust camera image and laser scanner
        double angle_offset = ((markerPoint.x - 320) / 320);
        double adapted_camera_angle = estimated_camera_angle * angle_offset;

        // Calculate the angle in which the blue square is
        double final_angle = fmod((2 * M_PI + adapted_camera_angle), (2 * M_PI));
        // Set the laser index to read from
        int laserscan_index = static_cast<int>(final_angle / laserScan.angle_increment);
        laserscan_index = fmod((360 - laserscan_index), (360));
        // Read distance from the laserscanner
        double laserscan_range = laserScan.ranges.at(laserscan_index);

        ROS_INFO("[BLUE] Angle: %.2f - Index: %i", final_angle * 180 / M_PI, laserscan_index);
        ROS_INFO("[BLUE] Distance: %.2f   |   Contour area: %.2f", laserscan_range, area);

        // Calculate the position of the blue square
        double x = cos(final_angle) * laserscan_range;
        double y = sin(final_angle) * laserscan_range;

        // Point with the postition to transform
        geometry_msgs::PointStamped blueSquareMap = get_point();

        tf2::Quaternion q;
        tf2::convert(transformStamped.transform.rotation, q);
        double rotation = tf2::getYaw(q);

        ROS_INFO("[BLUE] ROTATION1: %.2f", rotation * 180 / M_PI);
        // laserScan.ranges.at(final_angle)
        blueSquareMap.point.x += laserScan.ranges.at(final_angle) * cos(final_angle * laserScan.angle_increment + laserScan.angle_min + rotation);
        blueSquareMap.point.y += laserScan.ranges.at(final_angle) * sin(final_angle * laserScan.angle_increment + laserScan.angle_min + rotation);

        // Set the color of the marker
        std_msgs::ColorRGBA blueColor;
        blueColor.r = 0.0;
        blueColor.g = 0.0;
        blueColor.b = 1.0;
        blueColor.a = 1.0;

        publishMarker(blueSquareMap.point, std::to_string(laserscan_range), blueColor, "map", "blue_markers", 0, visualization_msgs::Marker::CUBE, blueSquarePublisher);
    }
}