opencv 如何用c++实现像素坐标转相机坐标

小鱼

@小元不圆 hi，可以看一下别人怎么用的

void triangulate_stereo(const cv::Mat & K1, const cv::Mat & kc1, const cv::Mat & K2, const cv::Mat & kc2,
                                  const cv::Mat & Rt, const cv::Mat & T, const cv::Point2d & p1, const cv::Point2d & p2,
                                  cv::Point3d & p3d, double * distance)
{
    //to image camera coordinates
    cv::Mat inp1(1, 1, CV_64FC2), inp2(1, 1, CV_64FC2);
    inp1.at<cv::Vec2d>(0, 0) = cv::Vec2d(p1.x, p1.y);
    inp2.at<cv::Vec2d>(0, 0) = cv::Vec2d(p2.x, p2.y);
    cv::Mat outp1, outp2;
    cv::undistortPoints(inp1, outp1, K1, kc1);
    cv::undistortPoints(inp2, outp2, K2, kc2);
    assert(outp1.type()==CV_64FC2 && outp1.rows==1 && outp1.cols==1);
    assert(outp2.type()==CV_64FC2 && outp2.rows==1 && outp2.cols==1);
    const cv::Vec2d & outvec1 = outp1.at<cv::Vec2d>(0,0);
    const cv::Vec2d & outvec2 = outp2.at<cv::Vec2d>(0,0);
    cv::Point3d u1(outvec1[0], outvec1[1], 1.0);
    cv::Point3d u2(outvec2[0], outvec2[1], 1.0);

    //to world coordinates
    cv::Point3d w1 = u1;
    cv::Point3d w2 = cv::Point3d(cv::Mat(Rt*(cv::Mat(u2) - T)));

    //world rays
    cv::Point3d v1 = w1;
    cv::Point3d v2 = cv::Point3d(cv::Mat(Rt*cv::Mat(u2)));

    //compute ray-ray approximate intersection
    p3d = approximate_ray_intersection(v1, w1, v2, w2, distance);
}

小元不圆

@小鱼我想请问下，terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(4.2.0) ../modules/core/src/matrix_wrap.cpp:79: error: (-215:Assertion failed) 0 <= i && i < (int)vv.size() in function 'getMat_'
我编译通过但是运行报错，这大概是什么原因
#include "ros/ros.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf/transform_listener.h"
#include "std_msgs/Float32MultiArray.h"
#include <opencv2/opencv.hpp>
#include <iostream>
using namespace cv;
using namespace std;
vector<vector<double>> MK;
vector<double> MD;

void pixel_to_camera(double arr[],float depth){
vector<double> points(arr,arr+2);
vector<Point2f> MP;
cv::undistortPoints(points,MP,MK,MD,cv::noArray(),cv::noArray());
int main(int argc,char *argv[]){
ros::init(argc,argv,"tf_pub_dynamic");
ros::NodeHandle nh;
double c1[3] = {467.973718746897, 0, 303.2622606570868};
double c2[3] = {0, 452.261907941113, 251.781457549611};
double c3[3] = {0, 0, 1};
vector<double> c1_v(c1,c1+3);
vector<double> c2_v(c2,c2+3);
vector<double> c3_v(c3,c3+3);
MK.push_back(c1_v);
MK.push_back(c2_v);
MK.push_back(c3_v);

double d1[5]={0.1162199830512324, -0.1111997240485036, -0.007053489238768723, 0.006914931854427629, 0};
for(int i = 0;i<5;i++){
    MD.push_back(d1[i]);
}   
double arr[2] = {286.0,198.0};
int *result;
pixel_to_camera(arr,1.0);
return 0;

}

小元不圆

@小元不圆我用的是std::vector来作为矩阵的不知道可不可以这么写

小鱼

@小元不圆应该是数据被opencv拿到时是空的，你没有赋值成功。

小元不圆

@小鱼鱼哥，我按照你给的例子把参数类型全部换成了mat类型可是还是报错了，不过报错变了
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(4.2.0) ../modules/calib3d/src/undistort.dispatch.cpp:283: error: (-215:Assertion failed) CV_IS_MAT(_cameraMatrix) && _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 in function 'cvUndistortPointsInternal'

Aborted (core dumped)

#include "ros/ros.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf/transform_listener.h"
#include "std_msgs/Float32MultiArray.h"
#include <opencv2/opencv.hpp>
#include <iostream>
using namespace cv;
using namespace std;

double num1[3][5] = {{467.973718746897, 0, 303.2622606570868},
{0, 452.261907941113, 251.781457549611},
{0,0,1},
};
double num2[5] = {0.1162199830512324, -0.1111997240485036,
-0.007053489238768723, 0.006914931854427629, 0};
void pixel_to_camera(double arr[],float depth){
vector<double> points(arr,arr+2);
vector<Point2f> MP;

cv::Mat inp1(1, 1, CV_64FC2,arr), inp2(1, 1, CV_64FC2);
cv::Mat outp1, outp2; 
cv::Mat m1(3,5,CV_64F,num1);
cv::Mat m2(1,5,CV_64F,num2);
cv::undistortPoints(inp1,outp1,m1,m2);

int main(int argc,char *argv[]){
ros::init(argc,argv,"tf_pub_dynamic");
ros::NodeHandle nh;

double arr[2] = {286.0,198.0};
int *result;
pixel_to_camera(arr,1.0);
return 0;

}

小鱼

@小元不圆在 opencv 中说：

./modules/calib3d/src/undistort.dispatch.cpp:283: error: (-215:Assertion failed) CV_IS_MAT(_cameraMatrix) && _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3

这个矩阵至少是3*3的哈

小元不圆

@小鱼嗯嗯我输入参数输错了，但是我获得的结构是nan是为什么呀

小鱼

@小元不圆 nan是计算中除0了应该是，所以还是输入数据的问题，仔细检查。

小元不圆

@小元不圆
这是我的代码，我把输入和输出全部打印出来后，得到的果全是nan。
#include "ros/ros.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf/transform_listener.h"
#include "std_msgs/Float32MultiArray.h"
#include <opencv2/opencv.hpp>
#include <iostream>
using namespace cv;
using namespace std;
vector<vector<double>> MK;
vector<double> MD;
double num1[3][5] = {{467.973718746897, 0, 303.2622606570868},
{0, 452.261907941113, 251.781457549611},
{0,0,1},
};
double num2[5] = {0.1162199830512324, -0.1111997240485036,
-0.007053489238768723, 0.006914931854427629, 0};

void pixel_to_camera(double arr[],float depth){
vector<double> points(arr,arr+2);
vector<Point2f> MP;

cv::Mat inp1(1, 2, CV_64FC2,arr), inp2(1, 1, CV_64FC2);
cv::Mat outp1, outp2;
cv::Mat m1(3,3,CV_64F,num1);
cv::Mat m2(1,5,CV_64F,num2);
cv::undistortPoints(inp1,outp1,m1,m2);
cout<<"inp1\n"<<inp1<<endl;
cout<<"outp1\n"<<outp1<<endl;
cout<<"m1\n"<<m1<<endl;
cout<<"m2\n"<<m2<<endl;

int main(int argc,char *argv[]){
ros::init(argc,argv,"tf_pub_dynamic");
ros::NodeHandle nh;
double arr[2] = {286.0,198.0};
int *result;
pixel_to_camera(arr,1.0);
return 0;
}

小元不圆

，格式没弄整齐

小元不圆

@小鱼是我输入的值不对，我检查一下，谢了鱼鱼哥

小鱼

@小元不圆 m1第二行全是0，你发现了吗

小元不圆

@小鱼嗯嗯，解决掉了，谢啦，顺便问一下输入的像素坐标是顺序是x和y，输出的相机下的坐标顺序分别也是x和y对吗

小鱼

@小元不圆像素坐标和相机坐标原点和方向是不同的，具体我记不太清了，你找些文章看看，记得把问题标记为已解决，同时改一下标题，这个标题不太合格

@小鱼在提问前必看！一定要看！必须看一下！中说：

1.使用明确的问题标题
好的标题：

一键安装后进不去图形操作系统(能让人一眼看出问题)

不好的标题（看到后感觉莫名其妙不想回答）：

ssh远程登录（远程登录怎么了？）
ROS安装错误(不说什么错误)

小元不圆

@小鱼好嘞，改了