opencv 仅起读图和显示,去畸变部分是纯c。
#include <opencv2/opencv.hpp>
#include <iostream>
#include <cassert>
#include <cmath>
#include<iostream>
using namespace cv;
using namespace std;
int main()
{
Mat image = cv::imread("test.png");
int rows = image.rows, cols = image.cols;
Mat image_undistort = Mat(rows, cols, CV_8UC3);
//begin
double fx = 1761.11838;
double fy = 1764.23318;
double cx = 1252.36929;
double cy = 726.936342;
double k1 = -0.42143775;
double k2 = 0.213623709;
double p1 = 0.000174992651;
double p2 = 0.0000262047557;
double k3 =-0.0564630091;
for (int v = 0; v < rows; v++)
for (int u = 0; u < cols; u++) {
double u_distorted = 0, v_distorted = 0;
double x1, y1, x2, y2;
x1 = (u - cx) / fx;
y1 = (v - cy) / fy;
double r2;
r2 = pow(x1, 2) + pow(y1, 2);
x2 = x1*(1 + k1*r2 + k2*pow(r2, 2)+ k3*pow(r2, 3)) + 2 * p1*x1*y1 + p2*(r2 + 2 * x1*x1);
y2 = y1*(1 + k1*r2 + k2*pow(r2, 2)+ k3*pow(r2, 3)) + p1*(r2 + 2 * y1*y1) + 2 * p2*x1*y1;
u_distorted = fx*x2 + cx;
v_distorted = fy*y2 + cy;
//finish
if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {