Mat fuseImg(Mat &_leftImg, Mat &_rightImg, double _ratio)
{
if (_leftImg.size() != _rightImg.size())
{
return Mat();
}
if (_ratio == 1.0)
{
return _leftImg.clone();
}
else if (_ratio == 0.0)
{
return _rightImg.clone();
}
else
{
size_t imgWidth = _leftImg.size().width;
size_t cropPoint = imgWidth * _ratio;
Range leftRange = Range(0, cropPoint);
Range rightRange = Range(cropPoint, imgWidth - 1);
Mat retImg(_leftImg.size(), _leftImg.type());
_leftImg(Range::all(), leftRange).copyTo(retImg(Range::all(), leftRange));
_rightImg(Range::all(), rightRange).copyTo(retImg(Range::all(), rightRange));
return retImg;
}
}
Mat ImageHandler::convertImageToOneDim(Mat img)
{
Mat img1;
if(img.empty())
throw std::invalid_argument( "The image is empty!" );
else if(img.channels()>1)
cvtColor(img, img1, COLOR_RGB2GRAY);
else img1 = img;
std::vector<uchar> oneDimData1;
oneDimData1.assign(img1.datastart, img1.dataend);
Mat retImg(oneDimData1);
return retImg;
}
Image<T> rotate(const Image<T>& srcImg, const int x, const int y,
const float ang)
{
GVX_TRACE(__PRETTY_FUNCTION__);
// make sure the source image is valid
ASSERT(srcImg.initialized());
// create and clear the return image
int w = srcImg.getWidth(), h = srcImg.getHeight();
Image<T> retImg(w, h, ZEROS);
// create temporary working image, put srcImg in the middle
// put some padding (values are 0) around the image
// so we won't need to check out of bounds when rotating
int pad = int(ceil(sqrt(w * w + h * h)));
int wR = 2 * pad + w;
int hR = 2 * pad + h;
Image<T> tempImg(wR, hR, ZEROS);
inplacePaste(tempImg, srcImg, Point2D<int>(pad,pad));
typename Image<T>::iterator rBPtr = retImg.beginw();
float cAng = cos(ang), sAng = sin(ang);
// fill in the return image with the appropriate values
float xR = 0.0; float yR = 0.0;
int dx = pad + x, dy = pad + y;
for(int j = -y; j < h-y; j++)
for(int i = -x; i < w-x; i++)
{
xR = dx + i*cAng + j*sAng;
yR = dy - i*sAng + j*cAng;
*rBPtr++ = T(tempImg.getValInterp(xR,yR));
}
return retImg;
}
/*********************w is variable********************************/
Mat corrector::latitudeCorrection4(Mat imgOrg, Point2i center, int radius, double w_longtitude, double w_latitude, distMapMode distMap, double theta_left, double phi_up, double camerFieldAngle, camMode camProjMode)
{
if (!(camerFieldAngle > 0 && camerFieldAngle <= PI))
{
cout << "The parameter \"camerFieldAngle\" must be in the interval (0,PI]." << endl;
return Mat();
}
double rateOfWindow = 0.9;
//int width = imgOrg.size().width*rateOfWindow;
//int height = width;
//int width = max(imgOrg.cols, imgOrg.rows);
int width = 512;
int height = width;
//int height = imgOrg.rows;
Size imgSize(width, height);
int center_x = imgSize.width / 2;
int center_y = imgSize.height / 2;
Mat retImg(imgSize, CV_8UC3, Scalar(0, 0, 0));
double dx = camerFieldAngle / imgSize.width;
double dy = camerFieldAngle / imgSize.height;
//coordinate for latitude map
double latitude;
double longitude;
//unity sphere coordinate
double x, y, z, r;
//parameter cooradinate of sphere coordinate
double Theta_sphere;
double Phi_sphere;
//polar cooradinate for fish-eye Image
double p;
double theta;
//cartesian coordinate
double x_cart, y_cart;
//Image cooradinate of imgOrg
double u, v;
Point pt, pt1, pt2, pt3, pt4;
//Image cooradinate of imgRet
int u_latitude, v_latitude;
Rect imgArea(0, 0, imgOrg.cols, imgOrg.rows);
//offset of imgRet Origin
double longitude_offset, latitude_offset;
longitude_offset = (PI - camerFieldAngle) / 2;
latitude_offset = (PI - camerFieldAngle) / 2;
double foval = 0.0;//焦距
cv::Mat_<Vec3b> _retImg = retImg;
cv::Mat_<Vec3b> _imgOrg = imgOrg;
//according to the camera type to do the calibration
double limi_latitude = 2 * auxFunc(w_latitude, 0);
double limi_longtitude = 2 * auxFunc(w_longtitude, 0);
for (int j = 0; j < imgSize.height; j++)
{
for (int i = 0; i < imgSize.width; i++)
{
Point3f tmpPt(i - center_x, center_y - j, 600);//最后一个参数用来修改成像面的焦距
double normPt = norm(tmpPt);
switch (distMap)
{
case PERSPECTIVE:
tmpPt.x /= normPt;
tmpPt.y /= normPt;
tmpPt.z /= normPt;
x = tmpPt.x;
y = tmpPt.y;
z = tmpPt.z;
break;
case LATITUDE_LONGTITUDE:
//latitude = latitude_offset + j*dy;
latitude = getPhi1((double)j*limi_latitude / imgSize.height, w_latitude);
//longitude = getPhi1((double)i * limi_longtitude / imgSize.width,w_longtitude);
//latitude = latitude_offset + j*dy;
longitude = longitude_offset + i*dx;
//Convert from latitude cooradinate to the sphere cooradinate
x = -sin(latitude)*cos(longitude);
y = cos(latitude);
z = sin(latitude)*sin(longitude);
break;
default:
break;
}
if (distMap == PERSPECTIVE)
{
//double theta = PI/4;
//double phi = -PI/2;
cv::Mat curPt(cv::Point3f(x, y, z));
std::vector<cv::Point3f> pts;
//向东旋转地球
//pts.push_back(cv::Point3f(cos(theta), 0, -sin(theta)));
//pts.push_back(cv::Point3f(0, 1, 0));
//pts.push_back(cv::Point3f(sin(theta), 0, cos(theta)));
//向南旋转地球
//pts.push_back(cv::Point3f(1, 0, 0));
//pts.push_back(cv::Point3f(0, cos(phi), sin(phi)));
//pts.push_back(cv::Point3f(0, -sin(phi), cos(phi)));
//两个方向旋转
pts.push_back(cv::Point3f(cos(theta_left), 0, sin(theta_left)));
pts.push_back(cv::Point3f(sin(phi_up)*sin(theta_left), cos(phi_up), -sin(phi_up)*cos(theta_left)));
pts.push_back(cv::Point3f(-cos(phi_up)*sin(theta_left), sin(phi_up), cos(phi_up)*cos(theta_left)));
cv::Mat revert = cv::Mat(pts).reshape(1).t();
cv::Mat changed(revert*curPt);
cv::Mat_<double> changed_double;
changed.convertTo(changed_double, CV_64F);
x = changed_double.at<double>(0, 0);
y = changed_double.at<double>(1, 0);
z = changed_double.at<double>(2, 0);
//std::cout << curPt << std::endl
// <<revert<<std::endl;
}
//Convert from unit sphere cooradinate to the parameter sphere cooradinate
Theta_sphere = acos(z);
Phi_sphere = cvFastArctan(y, x);//return value in Angle
Phi_sphere = Phi_sphere*PI / 180;//Convert from Angle to Radian
switch (camProjMode)
{
case STEREOGRAPHIC:
foval = radius / (2 * tan(camerFieldAngle / 4));
p = 2 * foval*tan(Theta_sphere / 2);
break;
case EQUIDISTANCE:
foval = radius / (camerFieldAngle / 2);
p = foval*Theta_sphere;
break;
case EQUISOLID:
foval = radius / (2 * sin(camerFieldAngle / 4));
p = 2 * foval*sin(Theta_sphere / 2);
break;
case ORTHOGONAL:
foval = radius / sin(camerFieldAngle / 2);
p = foval*sin(Theta_sphere);
break;
default:
cout << "The camera mode hasn't been choose!" << endl;
}
//Convert from parameter sphere cooradinate to fish-eye polar cooradinate
//p = sin(Theta_sphere);
theta = Phi_sphere;
//Convert from fish-eye polar cooradinate to cartesian cooradinate
x_cart = p*cos(theta);
y_cart = p*sin(theta);
//double R = radius / sin(camerFieldAngle / 2);
//Convert from cartesian cooradinate to image cooradinate
u = x_cart + center.x;
v = -y_cart + center.y;
pt = Point(u, v);
if (!pt.inside(imgArea))
{
continue;
}
else
{
_retImg.at<Vec3b>(j, i) = _imgOrg.at<Vec3b>(pt);
}
}
}
//imshow("org", _imgOrg);
//imshow("ret", _retImg);
//cv::waitKey();
#ifdef _DEBUG_
cv::namedWindow("Corrected Image", CV_WINDOW_AUTOSIZE);
imshow("Corrected Image", retImg);
cv::waitKey();
#endif
imwrite("ret.jpg", retImg);
return retImg;
}
//对于向天和向地拍摄的鱼眼图片校正
Mat corrector::heavenAndEarthCorrect(Mat imgOrg, Point center, int radius, double startRadian, CorrectType type)
{
//设定展开图的高度，因为鱼眼图像不能灰复高度信息，所以这里可以跟根实际来
//进行调节
int heightOfPanorama = radius * 2;
//设定展开图的宽度，这里设定为鱼眼图像圆形有效区域的周长
int widthOfPanorama = 2 * PI*radius;
double dx = 2 * PI / widthOfPanorama;
double dy = radius / (double)heightOfPanorama;
double p;
double theta;
double x, y;
int u, v;
//展开图的变量分配
Mat retImg(heightOfPanorama, widthOfPanorama, CV_8UC3, Scalar(0, 0, 0));
cv::Mat_<Vec3b> _retImg = retImg;
cv::Mat_<Vec3b> _imgOrg = imgOrg.clone();
switch (type)
{
case Reverse: //使用反向映射来填充展开图
for (int j = 0; j < heightOfPanorama; j++)
{
for (int i = 0; i < widthOfPanorama; i++)
{
p = j*dy;
theta = i*dx + startRadian;
x = p*cos(theta);
y = p*sin(theta);
u = x + center.x;
v = y + center.y;
_retImg(j, i)[0] = _imgOrg(v, u)[0];
_retImg(j, i)[1] = _imgOrg(v, u)[1];
_retImg(j, i)[2] = _imgOrg(v, u)[2];
}
}
break;
case Forward:
int left, top;
left = center.x - radius;
top = center.y - radius;
for (int j = top; j < top + 2 * radius; j++)
{
for (int i = left; i < left + 2 * radius; i++)
{
if (pow(i - center.x, 2) + pow(j - center.y, 2) > pow(radius, 2))
continue;
u = i;
v = j;
x = (u - center.x);
y = -(v - center.y);
//convert to polar axes
theta = cvFastArctan(y, x)*PI / 180;
p = sqrt(pow(x, 2) + pow(y, 2));
theta -= startRadian;
theta = theta < 0 ? theta + 2 * PI : theta;
int u_ret = theta / dx;
int v_ret = p / dy;
if (u_ret < 0 || u_ret >= widthOfPanorama || v_ret < 0 || v_ret >= heightOfPanorama)
continue;
//perform the map from the origin image to the latitude map image
_retImg(v_ret, u_ret)[0] = _imgOrg(j, i)[0];
_retImg(v_ret, u_ret)[1] = _imgOrg(j, i)[1];
_retImg(v_ret, u_ret)[2] = _imgOrg(j, i)[2];
}
}
break;
}
#ifdef _DEBUG_
cv::namedWindow("expand fish-eye image", CV_WINDOW_AUTOSIZE);
imshow("expand fish-eye image", retImg);
cv::waitKey();
#endif
return retImg;
}
//longitude-latitude reverse or forward map correction method
Mat corrector::latitudeCorrection(Mat imgOrg, Point2i center, int radius, double camerFieldAngle, CorrectType type)
{
if (!(camerFieldAngle > 0 && camerFieldAngle <= PI))
{
cout << "The parameter \"camerFieldAngle\" must be in the interval (0,PI]." << endl;
return Mat();
}
double rateOfWindow = 0.9;
int width = imgOrg.size().width*rateOfWindow;
int height = width;
Size imgSize(width, height);
Mat retImg(imgSize, CV_8UC3, Scalar(0, 0, 0));
double dx = camerFieldAngle / imgSize.width;
double dy = dx;
//coordinate for latitude map
double latitude;
double longitude;
//unity sphere coordinate
double x, y, z, r;
//parameter cooradinate of sphere coordinate
double Theta_sphere;
double Phi_sphere;
//polar cooradinate for fish-eye Image
double p;
double theta;
//cartesian coordinate
double x_cart, y_cart;
//Image cooradinate of imgOrg
int u, v;
//Image cooradinate of imgRet
int u_latitude, v_latitude;
//offset of imgRet Origin
double longitude_offset, latitude_offset;
longitude_offset = (PI - camerFieldAngle) / 2;
latitude_offset = (PI - camerFieldAngle) / 2;
cv::Mat_<Vec3b> _retImg = retImg;
cv::Mat_<Vec3b> _imgOrg = imgOrg;
//according to the correct type to do the calibration
switch (type)
{
case Forward:
int left, top;
left = center.x - radius;
top = center.y - radius;
for (int j = top; j < top + 2 * radius; j++)
{
for (int i = left; i < left + 2 * radius; i++)
{
if (pow(i - center.x, 2) + pow(j - center.y, 2) > pow(radius, 2))
continue;
//Origin image cooradinate in pixel
u = i;
v = j;
double R = radius / sin(camerFieldAngle / 2);
//Convert to cartiesian cooradinate in unity circle
x_cart = (u - center.x) / R;
y_cart = -(v - center.y) / R;
//convert to polar axes
theta = cvFastArctan(y_cart, x_cart)*PI / 180;
p = sqrt(pow(x_cart, 2) + pow(y_cart, 2));
//convert to sphere surface parameter cooradinate
Theta_sphere = asin(p);
Phi_sphere = theta;
//convert to sphere surface 3D cooradinate
x = sin(Theta_sphere)*cos(Phi_sphere);
y = sin(Theta_sphere)*sin(Phi_sphere);
z = cos(Theta_sphere);
//convert to latitiude cooradinate
latitude = acos(y);
longitude = cvFastArctan(z, -x)*PI / 180;
//transform the latitude to pixel cooradinate
u_latitude = ((longitude - longitude_offset) / dx);
v_latitude = ((latitude - latitude_offset) / dy);
if (u_latitude < 0 || u_latitude >= imgSize.height || v_latitude < 0 || v_latitude >= imgSize.width)
continue;
//perform the map from the origin image to the latitude map image
_retImg(v_latitude, u_latitude)[0] = _imgOrg(j, i)[0];
_retImg(v_latitude, u_latitude)[1] = _imgOrg(j, i)[1];
_retImg(v_latitude, u_latitude)[2] = _imgOrg(j, i)[2];
}
}
break;
case Reverse:
for (int j = 0; j < imgSize.height; j++)
{
latitude = latitude_offset + j*dy;
for (int i = 0; i < imgSize.width; i++)
{
longitude = longitude_offset + i*dx;
//Convert from latitude cooradinate to the sphere cooradinate
x = -sin(latitude)*cos(longitude);
y = cos(latitude);
z = sin(latitude)*sin(longitude);
//Convert from sphere cooradinate to the parameter sphere cooradinate
Theta_sphere = acos(z);
Phi_sphere = cvFastArctan(y, x);//return value in Angle
Phi_sphere = Phi_sphere*PI / 180;//Convert from Angle to Radian
//Convert from parameter sphere cooradinate to fish-eye polar cooradinate
p = sin(Theta_sphere);
theta = Phi_sphere;
//Convert from fish-eye polar cooradinate to cartesian cooradinate
x_cart = p*cos(theta);
y_cart = p*sin(theta);
//double R = radius / sin(camerFieldAngle / 2);
double R = radius;
//Convert from cartesian cooradinate to image cooradinate
u = x_cart*R + center.x;
v = -y_cart*R + center.y;
//if (pow(u - center.x, 2) + pow(v - center.y, 2) > pow(radius, 2))
//{
// _imgOrg(v, u)[0] = 255;
// _imgOrg(v, u)[1] = 255;
// _imgOrg(v, u)[2] = 255;
// continue;
//}
_retImg.at<Vec3b>(j, i) = _imgOrg.at<Vec3b>(v, u);
}
}
break;
default:
cout << "The CorrectType is Wrong! It should be \"Forward\" or \"Reverse\"." << endl;
return Mat();
}
//imwrite("C:\\Users\\Joker\\Desktop\\ret4.jpg", retImg);
//imshow("org", _imgOrg);
//imshow("ret", _retImg);
//cv::waitKey();
#ifdef _DEBUG_
cv::namedWindow("Corrected Image", CV_WINDOW_AUTOSIZE);
imshow("Corrected Image", retImg);
cv::waitKey();
#endif
return retImg;
}
/************************w is variable, and Forward map*/
Mat corrector::latitudeCorrection5(Mat imgOrg, Point2i center, int radius, double w_longtitude, double w_latitude, distMapMode distMap, double theta_left, double phi_up, double camerFieldAngle, camMode camProjMode)
{
if (!(camerFieldAngle > 0 && camerFieldAngle <= PI))
{
cout << "The parameter \"camerFieldAngle\" must be in the interval (0,PI]." << endl;
return Mat();
}
double rateOfWindow = 0.9;
//int width = imgOrg.size().width*rateOfWindow;
//int height = width;
//int width = max(imgOrg.cols, imgOrg.rows);
int width = 512;
int height = width;
//int height = imgOrg.rows;
Size imgSize(width, height);
int center_x = imgSize.width / 2;
int center_y = imgSize.height / 2;
Mat retImg(imgSize, CV_8UC3, Scalar(0, 0, 0));
double dx = camerFieldAngle / imgSize.width;
double dy = camerFieldAngle / imgSize.height;
//coordinate for latitude map
double latitude;
double longitude;
//unity sphere coordinate
double x, y, z, r;
//parameter cooradinate of sphere coordinate
double Theta_sphere;
double Phi_sphere;
//polar cooradinate for fish-eye Image
double p;
double theta;
//cartesian coordinate
double x_cart, y_cart;
//Image cooradinate of imgOrg
double u, v;
Point pt, pt1, pt2, pt3, pt4;
//Image cooradinate of imgRet
int u_longtitude, v_latitude;
Rect imgArea(0, 0, imgOrg.cols, imgOrg.rows);
//offset of imgRet Origin
double longitude_offset, latitude_offset;
longitude_offset = (PI - camerFieldAngle) / 2;
latitude_offset = (PI - camerFieldAngle) / 2;
double foval = 0.0;//焦距
cv::Mat_<Vec3b> _retImg = retImg;
cv::Mat_<Vec3b> _imgOrg = imgOrg;
int left, top;
left = center.x - radius;
top = center.y - radius;
for (int j = top; j < top + 2 * radius; j++)
{
for (int i = left; i < left + 2 * radius; i++)
{
if (pow(i - center.x, 2) + pow(j - center.y, 2) > pow(radius, 2))
continue;
//Origin image cooradinate in pixel
u = i;
v = j;
//Convert to cartiesian cooradinate in unity circle
x_cart = (u - center.x);
y_cart = -(v - center.y);
//convert to polar axes
theta = cvFastArctan(y_cart, x_cart)*PI / 180;
p = sqrt(pow(x_cart, 2) + pow(y_cart, 2));
//convert to sphere surface parameter cooradinate
Theta_sphere = p*(camerFieldAngle / 2) / radius;
Phi_sphere = theta;
//convert to sphere surface 3D cooradinate
x = sin(Theta_sphere)*cos(Phi_sphere);
y = sin(Theta_sphere)*sin(Phi_sphere);
z = cos(Theta_sphere);
//convert to latitiude cooradinate
latitude = acos(y);
longitude = cvFastArctan(z, -x)*PI / 180;
//transform the latitude to pixel cooradinate
double limi_latitude = auxFunc(w_latitude, 0);
double l = 0;
if (latitude >= 0 && latitude < PI / 2)
{
l = limi_latitude - sin(w_latitude)*sqrt(cos(latitude)*cos(latitude) + (1 - sin(latitude))*(1 - sin(latitude))) / sin(PI - w_latitude - atan((1 - sin(latitude)) / abs(cos(latitude))));
}
else
{
l = limi_latitude + sin(w_latitude)*sqrt(cos(latitude)*cos(latitude) + (1 - sin(latitude))*(1 - sin(latitude))) / sin(PI - w_latitude - atan((1 - sin(latitude)) / abs(cos(latitude))));
}
u_longtitude = ((longitude - longitude_offset) / dx);
// = (latitude - latitude_offset) / dy;
v_latitude = l*imgSize.height / (2 * limi_latitude);
if (u_longtitude < 0 || u_longtitude >= imgSize.height || v_latitude < 0 || v_latitude >= imgSize.width)
continue;
//perform the map from the origin image to the latitude map image
_retImg.at<cv::Vec3b>(v_latitude, u_longtitude) = imgOrg.at<cv::Vec3b>(j, i);
}
}
//imshow("org", _imgOrg);
//imshow("ret", _retImg);
//cv::waitKey();
#ifdef _DEBUG_
cv::namedWindow("Corrected Image", CV_WINDOW_AUTOSIZE);
imshow("Corrected Image", retImg);
cv::waitKey();
#endif
imwrite("ret.jpg", retImg);
return retImg;
}
