C++ OpenCV findHomography 与 json 的问题

Question

我正在开发一个机器人尝试的ROS项目，我用python编写了代码并且运行良好，现在我尝试将python转换为c++，但我在校准函数方面遇到了一些问题，该函数使用json和openCV 查找单应性 这是我的Python代码，运行良好（仅load_calibrate()函数）

import sys
import rospy
import numpy as np
import scipy.optimize
import cv2
import json
#import InvKin

THETA_EXT = 0.27
THETA_RET = np.pi/4

def pol2cart(rho, phi):
    """helper,convert polar to cartesian"""
    x = rho*np.cos(phi)
    y = rho*np.sin(phi)
    return(x, y)

def load_calibrate(self):
    """load mapping points from gazebo to robot frame, estimate l and L, generate homography map"""
    try:
      with open('calibration.json', 'r') as f:
        calib = json.load(f)
      src_pts = calib['src_pts']
      dst_angs = calib['dst_angs']

      s_ret_pts = src_pts[1::2]
      s_ext_pts = src_pts[2::2]
      arr = np.array(s_ret_pts)-np.array(s_ext_pts)
      self.L = np.sqrt((arr*arr).sum(axis=1)).mean()/(np.cos(THETA_EXT)-np.cos(THETA_RET))
      arr = np.array(s_ret_pts)-np.array(src_pts[0])
      l1 = np.sqrt((arr*arr).sum(axis=1)).mean() - self.L*np.cos(THETA_RET)
      arr = np.array(s_ext_pts)-np.array(src_pts[0])
      l2 = np.sqrt((arr*arr).sum(axis=1)).mean() - self.L*np.cos(THETA_EXT)
      self.l = (l1+l2)/2

      dst_pts = [[0,0]]
      for i in range(len(dst_angs)):
        phi = dst_angs[i][0]
        rho = self.L*np.cos(dst_angs[i][1]) + self.l
        x, y = pol2cart(rho, phi)
        dst_pts.append([x,y])

      src_pts = np.array(src_pts)
      dst_pts = np.array(dst_pts)

      h, status = cv2.findHomography(src_pts, dst_pts)
      self.homography = h

      #self.kinematics = InvKin.Arm3Link(L=[self.L/2,self.L/2,self.l+L_FUDGE])
      print('calibration loaded.')
      print('estimated l = ' + str(self.l))
      print('estimated L = ' + str(self.L))
      cv2.destroyAllWindows()
    except:
      print('calibration.json not in current directory, run calibration first')

这是我的C++代码

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <cmath>

#include <opencv2/opencv.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <nlohmann/json.hpp>


//#include "InvKin.hpp"

// Constants
const double THETA_EXT = 0.27;
const double THETA_RET = M_PI / 4;

cv::Mat homography_;

//Arm3Link InvKin;

using json = nlohmann::json;

//function which transforms polar coord into cartesian
std::pair<double, double> pol2cart(double rho, double phi) {
  // helper, convert polar to cartesian coordinates
  double x = rho * std::cos(phi);
  double y = rho * std::sin(phi);
  return std::make_pair(x, y);
}

void loadCalibrate() {
    try {
        std::ifstream file("calibration.json");
        if (!file.is_open()) {
            std::cerr << "calibration.json not in current directory, run calibration first" << std::endl;
            return;
        }

        json calib = json::parse(file);
        file.close();

        // const auto s = calib.dump(); // serialize to std::string
        // std::cout << "JSON string: " << s << '\n';
        // std::cout << "JSON size  : " << s.size() << '\n';

        const json& src_pts_js = calib["src_pts"];
        const json& dst_angs_js = calib["dst_angs"];


        std::vector<std::vector<double>> src_pts;
        std::vector<std::vector<double>> dst_angs;

        for (const auto& point : src_pts_js) {
            src_pts.push_back({point[0].get<double>(), point[1].get<double>()});
        }

        std::vector<std::vector<double>> s_ret_pts, s_ext_pts;
        for (int i = 1; i < src_pts.size(); i += 2) {
            s_ret_pts.push_back(src_pts[i]);
            s_ext_pts.push_back(src_pts[i + 1]);
        }

        std::vector<std::vector<double>> arr;//(s_ret_pts.size());
        for (int i = 0; i < s_ret_pts.size(); i++) {
            // arr[i] = s_ret_pts[i] - s_ext_pts[i];
            std::vector<double> arr_i(2);
            arr_i[0] = s_ret_pts[i][0] - s_ext_pts[i][0];
            arr_i[1] = s_ret_pts[i][1] - s_ext_pts[i][1];
            arr.push_back(arr_i);
        }

        double sum_of_squares = 0.0;
        for (const auto& v : arr) {
            sum_of_squares += v[0] * v[0] + v[1] * v[1];
        }

        double L = sqrt(sum_of_squares) / (cos(THETA_EXT) - cos(THETA_RET));


        std::vector<std::vector<double>> arr1;
        for (size_t i = 0; i < s_ret_pts.size(); ++i) {
            std::vector<double> arr1_i(2);
            arr1_i[0] = s_ret_pts[i][0] - src_pts[0][0];
            arr1_i[1] = s_ret_pts[i][1] - src_pts[0][1];
            arr1.push_back(arr1_i);
        }

        double l1 = 0.0;
        for (const auto& v : arr1) {
            l1 += sqrt(v[0] * v[0] + v[1] * v[1]);
        }
        l1 /= s_ret_pts.size();

        std::vector<std::vector<double>> arr2;
        for (size_t i = 0; i < s_ext_pts.size(); ++i) {
            std::vector<double> arr2_i(2);
            arr2_i[0] = s_ext_pts[i][0] - src_pts[0][0];
            arr2_i[1] = s_ext_pts[i][1] - src_pts[0][1];
            arr2.push_back(arr2_i);
        }

        double l2 = 0.0;
        for (const auto& v : arr2) {
            l2 += sqrt(v[0] * v[0] + v[1] * v[1]);
        }
        l2 /= s_ext_pts.size();

        double l = (l1 + l2) / 2;

        std::vector<std::vector<double>> dst_pts = {{0, 0}};

        for (const std::vector<double>& dst_ang : dst_angs) {
            double phi = dst_ang[0];
            double rho = L * cos(dst_ang[1]) + l;
            double x = rho * cos(phi);
            double y = rho * sin(phi);
            dst_pts.push_back({x, y});
        }

        cv::Mat src_pts_mat(src_pts.size(), 2, CV_64F);
        for (int i = 0; i < src_pts.size(); i++) {
            src_pts_mat.at<double>(i, 0) = src_pts[i][0];
            src_pts_mat.at<double>(i, 1) = src_pts[i][1];
        }

        cv::Mat dst_pts_mat(dst_pts.size(), 2, CV_64F);
        for (int i = 0; i < dst_pts.size(); i++) {
            dst_pts_mat.at<double>(i, 0) = dst_pts[i][0];
            dst_pts_mat.at<double>(i, 1) = dst_pts[i][1];
        }

        cv::Mat h = cv::findHomography(src_pts_mat, dst_pts_mat);//, cv::RANSAC);
        homography_ = h;

        std::vector<double> arm_vect = {L / 2, L / 2, l + L_FUDGE};
        // InvKin = arm_vect;

        std::cout << "calibration loaded." << std::endl;
        std::cout << "estimated l = " << l << std::endl;
        std::cout << "estimated L = " << L << std::endl;

        cv::destroyAllWindows();
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        std::cerr << "calibration.json not in current directory, run calibration first" << std::endl;
    }
}

这是我的calibration.json 文件

{
  "src_pts": [
    [-4.121986384758449e-08, 1.0276103843932543e-07],
    [0.22931348423505332, 0.13237949523521805],
    [0.2848558233143873, 0.16448791146824454],
    [0.2647778545473112, -9.064312885519177e-08],
    [0.32894016015086663, 1.900568451782686e-05],
    [0.22930849325858083, -0.13237986474748129],
    [0.2848843440686785, -0.16444564748063195]
  ],
  "dst_angs": [
    [1.0471975511965979, 0.7853981633974483, 0.0, 2.356194490192345],
    [1.0471975511965979, 0.27, 1.0307963267948965, 1.8407963267948966],
    [1.5707963267948966, 0.7853981633974483, 0.0, 2.356194490192345],
    [1.5707963267948966, 0.27, 1.0307963267948965, 1.8407963267948966],
    [2.0943951023931957, 0.7853981633974483, 0.0, 2.356194490192345],
    [2.0943951023931957, 0.27, 1.0307963267948965, 1.8407963267948966]
  ]
}

这是我得到的错误

Error: OpenCV(4.2.0) ../modules/calib3d/src/fundam.cpp:380: error: (-215:Assertion failed) src.checkVector(2) == dst.checkVector(2) in function 'findHomography'

P.S.：我注释掉了 InvKin 调用和库，因为这不是这里的问题

Answer 1

我解决了这个问题，L、l1和l2的计算以及计算矩阵范数的方法存在一些问题，我添加了一个函数来计算这个问题并相应地调整了代码。 这是代码

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <cmath>

#include <opencv2/opencv.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <nlohmann/json.hpp>


#include "../InvKin.hpp"

// Constants
const double THETA_EXT = 0.27;
const double THETA_RET = M_PI / 4;
const double L_FUDGE = 0.08;

cv::Mat homography_;

Arm3Link InvKin;

using json = nlohmann::json;

//function which transforms cartesian coord into polar coord
std::pair<double, double> cart2pol(double x, double y) {
  // helper, convert cartesian to polar coordinates
  double rho = std::sqrt(x * x + y * y);
  double phi = std::atan2(y, x);
  return {rho, phi};
}

//function which transforms polar coord into cartesian
std::pair<double, double> pol2cart(double rho, double phi) {
  // helper, convert polar to cartesian coordinates
  double x = rho * std::cos(phi);
  double y = rho * std::sin(phi);
  return std::make_pair(x, y);
}

//function that, getting the angle of the shoulder, returns the angles of the wrist and of the elbow
std::pair<double, double> get_other_angles(double theta_shoulder) {
  // helper, converting some angles
  double theta_wrist = theta_shoulder + M_PI / 2;
  double theta_elbow = M_PI / 2 - 2 * theta_shoulder;
  return std::make_pair(theta_wrist, theta_elbow);
}

double calculateMeanNorm(const std::vector<std::vector<double>>& arr) {
    double sum = 0.0;

    for (const std::vector<double>& vec : arr) {
        double squaredSum = 0.0;

        for (double val : vec) {
            squaredSum += val * val;
        }

        sum += std::sqrt(squaredSum);
    }

    return sum / arr.size();
}


void loadCalibrate() {
    try {
        std::ifstream file("calibration.json");
        if (!file.is_open()) {
            std::cerr << "calibration.json not in current directory, run calibration first" << std::endl;
            return;
        }

        json calib = json::parse(file);
        file.close();

        const auto s = calib.dump(); // serialize to std::string
        std::cout << "JSON string: " << s << '\n';
        std::cout << "JSON size  : " << s.size() << '\n';

        const json& src_pts_js = calib["src_pts"];
        const json& dst_angs_js = calib["dst_angs"];


        std::vector<std::vector<double>> src_pts;
        std::vector<std::vector<double>> dst_angs;

        for (const auto& point : src_pts_js) {
            src_pts.push_back({point[0].get<double>(), point[1].get<double>()});
        }

        for (const auto& point2 : dst_angs_js) {
            dst_angs.push_back({point2[0].get<double>(), point2[1].get<double>(), point2[2].get<double>(),point2[3].get<double>()});
        }

        std::cout << "dst_angs is " << std::endl;
        for(int a = 0; a < dst_angs.size(); a++){
            for(int b = 0; b < dst_angs[a].size(); b++){
                std::cout << dst_angs[a][b] << ", ";
                }
            std::cout << std::endl;
            }

        std::vector<std::vector<double>> s_ret_pts, s_ext_pts;
        for (int i = 1; i < src_pts.size(); i += 2) {
            s_ret_pts.push_back(src_pts[i]);
            s_ext_pts.push_back(src_pts[i + 1]);
        }

        std::vector<std::vector<double>> arr;//(s_ret_pts.size());
        for (int i = 0; i < s_ret_pts.size(); i++) {
            // arr[i] = s_ret_pts[i] - s_ext_pts[i];
            std::vector<double> arr_i(2);
            arr_i[0] = s_ret_pts[i][0] - s_ext_pts[i][0];
            arr_i[1] = s_ret_pts[i][1] - s_ext_pts[i][1];
            arr.push_back(arr_i);
        }


        double L = calculateMeanNorm(arr) / (cos(THETA_EXT) - cos(THETA_RET));
        std::cout << "L is" << L << std::endl;

        std::vector<std::vector<double>> arr1;
        for (size_t i = 0; i < s_ret_pts.size(); ++i) {
            std::vector<double> arr1_i(2);
            arr1_i[0] = s_ret_pts[i][0] - src_pts[0][0];
            arr1_i[1] = s_ret_pts[i][1] - src_pts[0][1];
            arr1.push_back(arr1_i);
        }


        double l1 = calculateMeanNorm(arr1);
        std::cout << "l1 before is" << l1 << std::endl;
        l1 = l1 - L*cos(THETA_RET);
        std::cout << "l1 after is" << l1 << std::endl;

        std::vector<std::vector<double>> arr2;
        for (size_t i = 0; i < s_ext_pts.size(); ++i) {
            std::vector<double> arr2_i(2);
            arr2_i[0] = s_ext_pts[i][0] - src_pts[0][0];
            arr2_i[1] = s_ext_pts[i][1] - src_pts[0][1];
            arr2.push_back(arr2_i);
        }

        double l2 = calculateMeanNorm(arr2);
        std::cout << "l2 before is" << l2 << std::endl;
        l2 = l2 - L*cos(THETA_EXT);
        std::cout << "l2 after is" << l2 << std::endl;

        float l = (l1 + l2)/ 2;
        std::cout << "l is" << l << std::endl;

        std::vector<std::vector<double>> dst_pts = {{0, 0}};

        for (const std::vector<double>& dst_ang : dst_angs) {
            double phi = dst_ang[0];
            std::cout << "phi is" << phi << std::endl;
            double rho = L * cos(dst_ang[1]) + l;
            std::cout << "rho is" << rho << std::endl;
            std::pair<double,double> coord = pol2cart(rho, phi);
            dst_pts.push_back({coord.first, coord.second});
        }

        cv::Mat src_pts_mat(src_pts.size(), 2, CV_64F);
        for (int i = 0; i < src_pts.size(); i++) {
            src_pts_mat.at<double>(i, 0) = src_pts[i][0];
            src_pts_mat.at<double>(i, 1) = src_pts[i][1];
        }

        cv::Mat dst_pts_mat(dst_pts.size(), 2, CV_64F);
        for (int i = 0; i < dst_pts.size(); i++) {
            dst_pts_mat.at<double>(i, 0) = dst_pts[i][0];
            dst_pts_mat.at<double>(i, 1) = dst_pts[i][1];
        }

        int rows_src = sizeof(src_pts)/sizeof(src_pts[0]);
        int cols_src = sizeof(src_pts[0])/sizeof(src_pts[0][0]);
        std::cout<< "Shape of src_pts is "<< rows_src <<" "<<cols_src<<std::endl;

        int rows_dst = sizeof(dst_pts)/sizeof(dst_pts[0]);
        int cols_dst = sizeof(dst_pts[0])/sizeof(dst_pts[0][0]);
        std::cout<< "Shape of dst_pts is "<< rows_dst <<" "<< cols_dst <<std::endl;

        std::cout << "src_pts array is " << std::endl;
        for(int i = 0; i < src_pts.size(); i++){
            for(int j = 0; j < src_pts[i].size(); j++){
                std::cout << src_pts[i][j] << ", ";
                }
            std::cout << std::endl;
            }

        std::cout << "dst_pts array is " << std::endl;
        for(int h = 0; h < dst_pts.size(); h++){
            for(int k = 0; k < dst_pts[h].size(); k++){
                std::cout <<  dst_pts[h][k] << ", ";
                }
            std::cout << std::endl;
            }

        cv::Mat h = cv::findHomography(src_pts_mat, dst_pts_mat, cv::RANSAC);
        homography_ = h;

        std::vector<double> arm_vect = {L / 2, L / 2, l + L_FUDGE};
        InvKin = arm_vect;

        std::cout << "calibration loaded." << std::endl;
        std::cout << "estimated l = " << l << std::endl;
        std::cout << "estimated L = " << L << std::endl;

        cv::destroyAllWindows();
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        std::cerr << "calibration.json not in current directory, run calibration first" << std::endl;
    }
}

int main(){
    loadCalibrate();
    return 0;
}