bool IPLUndistort::processInputData(IPLImage* image, int, bool)
{
// get properties
float k1 = getProcessPropertyDouble("k1");
float k2 = getProcessPropertyDouble("k2");
float k3 = getProcessPropertyDouble("k3");
float p1 = getProcessPropertyDouble("p1");
float p2 = getProcessPropertyDouble("p2");
float c1 = getProcessPropertyInt("f");
float c2 = c1;
notifyProgressEventHandler(-1);
cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) << c1, 0, image->width()*0.5, 0, c2, image->height()*0.5, 0, 0, 1);
cv::Mat distCoeffs(5, 1, CV_32FC1);
distCoeffs.at<float>(0,0) = k1;
distCoeffs.at<float>(1,0) = k2;
distCoeffs.at<float>(2,0) = p1;
distCoeffs.at<float>(3,0) = p2;
distCoeffs.at<float>(4,0) = k3;
cv::Mat result;
cv::undistort(image->toCvMat(), result, cameraMatrix, distCoeffs);
delete _result;
_result = new IPLImage(result);
return true;
}
int main( int argc, char* argv[])
{
// Read points
std::vector<cv::Point2f> imagePoints = Generate2DPoints();
std::vector<cv::Point3f> objectPoints = Generate3DPoints();
std::cout << "There are " << imagePoints.size() << " imagePoints and " << objectPoints.size() << " objectPoints." << std::endl;
//camera parameters
double fx = 525.0; //focal length x
double fy = 525.0; //focal le
double cx = 319.5; //optical centre x
double cy = 239.5; //optical centre y
cv::Mat cameraMatrix(3,3,cv::DataType<double>::type);
cameraMatrix.at<double>(0,0)=fx;
cameraMatrix.at<double>(1,1)=fy;
cameraMatrix.at<double>(2,2)=1;
cameraMatrix.at<double>(0,2)=cx;
cameraMatrix.at<double>(1,2)=cy;
cameraMatrix.at<double>(0,1)=0;
cameraMatrix.at<double>(1,0)=0;
cameraMatrix.at<double>(2,0)=0;
cameraMatrix.at<double>(2,1)=0;
std::cout << "Initial cameraMatrix: " << cameraMatrix << std::endl;
cv::Mat distCoeffs(4,1,cv::DataType<double>::type);
distCoeffs.at<double>(0) = 0;
distCoeffs.at<double>(1) = 0;
distCoeffs.at<double>(2) = 0;
distCoeffs.at<double>(3) = 0;
cv::Mat rvec(3,1,cv::DataType<double>::type);
cv::Mat tvec(3,1,cv::DataType<double>::type);
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
std::cout << "rvec: " << rvec << std::endl;
std::cout << "tvec: " << tvec << std::endl;
std::vector<cv::Point2f> projectedPoints;
cv::projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedPoints);
for(unsigned int i = 0; i < projectedPoints.size(); ++i)
{
std::cout << "Image point: " << imagePoints[i] << " Projected to " << projectedPoints[i] << std::endl;
}
return 0;
}
void MonoDvsCalibration::addPattern(int id)
{
// add detection
image_points_.push_back(transition_maps_[id].pattern);
object_points_.push_back(world_pattern_);
num_detections_++;
// compute and publish camera pose if camera is calibrated
if (got_camera_info_)
{
cv::Mat rvec, tvec;
cv::Mat cameraMatrix(3, 3, CV_64F);
cv::Mat distCoeffs(1, 5, CV_64F);
// convert to OpenCV
for (int i = 0; i < 5; i++)
distCoeffs.at<double>(i) = camera_info_external_.D[i];
for (int i = 0; i < 9; i++)
cameraMatrix.at<double>(i) = camera_info_external_.K[i];
cv::solvePnP(world_pattern_, transition_maps_[id].pattern, cameraMatrix, distCoeffs, rvec, tvec);
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.stamp = ros::Time::now();
pose_msg.header.frame_id = "dvs";
pose_msg.pose.position.x = tvec.at<double>(0);
pose_msg.pose.position.y = tvec.at<double>(1);
pose_msg.pose.position.z = tvec.at<double>(2);
double angle = cv::norm(rvec);
cv::normalize(rvec, rvec);
pose_msg.pose.orientation.x = rvec.at<double>(0) * sin(angle/2.0);
pose_msg.pose.orientation.y = rvec.at<double>(1) * sin(angle/2.0);
pose_msg.pose.orientation.z = rvec.at<double>(2) * sin(angle/2.0);
pose_msg.pose.orientation.w = cos(angle/2.0);
camera_pose_pub_.publish(pose_msg);
}
}
//\fn Eigen::Vector3d ExtrinsicParam::undistortPoint(Eigen::Vector3d distortedPoint);
///\brief This function undistorts a distorted point on the image coordinates. After calling this function, the first value of the returned vector should be multiplied by fx and added to cx and the second value should be multiplied by fy and added to cy.
///\param distortedPoint the distorted point to undistort.
///\return The value of the undistorted point in the image coordinates.
Eigen::Vector3d ExtrinsicParam::undistortPoint(Eigen::Vector3d distortedPoint)
{
Eigen::Vector3d res;
// Convert the camera matrix to an OpenCV matrix
cv::Mat K1(3,3,CV_64F);
K1.at<double>(0,0) = K(0,0);
K1.at<double>(0,1) = K(0,1);
K1.at<double>(0,2) = K(0,2);
K1.at<double>(1,0) = K(1,0);
K1.at<double>(1,1) = K(1,1);
K1.at<double>(1,2) = K(1,2);
K1.at<double>(2,0) = K(2,0);
K1.at<double>(2,1) = K(2,1);
K1.at<double>(2,2) = K(2,2);
// Convert the distortion vector to an OpenCV vector
cv::Mat distCoeffs(5,1,CV_64F);
distCoeffs.at<double>(0,0) = dist(0);
distCoeffs.at<double>(1,0) = dist(1);
distCoeffs.at<double>(2,0) = dist(2);
distCoeffs.at<double>(3,0) = dist(3);
distCoeffs.at<double>(4,0) = dist(4);
// Convert the distorted point vector to an OpenCV vector
cv::Mat src(1,1,CV_64FC2);
src.at<double>(0,0) = distortedPoint(0);
src.at<double>(0,1) = distortedPoint(1);
cv::Mat dst;
// Undistort the point
cv::undistortPoints(src, dst, K1, distCoeffs);
// Fill the undistorted point vector and return it
res(0) = dst.at<double>(0,0);
res(1) = dst.at<double>(0,1);
res(2) = 1.;
return res;
}
void RaspiVoice::processImage(cv::Mat rawImage)
{
cv::Mat processedImage = rawImage;
if (verbose)
{
printtime("ProcessImage start");
}
if ((image_source > 0) || (opt.input_filename != ""))
{
if (opt.foveal_mapping)
{
cv::Matx33f cameraMatrix(100, 0, processedImage.cols / 2, 0, 100, processedImage.rows / 2, 0, 0, 1);
cv::Matx41f distCoeffs(5.0, 5.0, 0, 0);
cv::Mat processedImage2;
cv::undistort(processedImage, processedImage2, cameraMatrix, distCoeffs);
float clipzoom = 1.8; //horizontal zoom to remove blinders, decreases resolution if > 1.0
cv::Rect roi(processedImage.cols / 2 - columns / 2 / clipzoom, processedImage.rows / 2 - rows / 2, columns / clipzoom, rows);
processedImage = processedImage2(roi);
}
if (opt.zoom > 1.0)
{
int w = processedImage.cols;
int h = processedImage.rows;
float z = opt.zoom;
cv::Rect roi((w / 2.0) - w / (2.0*z), (h / 2.0) - h / (2.0*z), w/z, h/z);
processedImage = processedImage(roi);
}
//Bring to size needed by ImageToSoundscape:
if (processedImage.rows != rows || processedImage.cols != columns)
{
cv::resize(processedImage, processedImage, cv::Size(columns, rows));
}
if ((opt.blinders > 0) && (opt.blinders < columns/2))
{
processedImage(cv::Rect(0, 0, opt.blinders, rows - 1)).setTo(0);
processedImage(cv::Rect(columns - 1 - opt.blinders, 0, opt.blinders, rows - 1)).setTo(0);
}
if (opt.contrast != 0.0)
{
float avg = 0.0;
for (int y = 0; y < processedImage.rows; y++)
{
for (int x = 0; x < processedImage.cols; x++)
{
avg += processedImage.at<uchar>(y, x);
}
}
avg = avg / (processedImage.rows * processedImage.cols);
for (int y = 0; y < processedImage.rows; y++)
{
for (int x = 0; x < processedImage.cols; x++)
{
int mVal = processedImage.at<uchar>(y, x);
processedImage.at<uchar>(y, x) = cv::saturate_cast<uchar>(mVal + opt.contrast*(mVal - avg));
}
}
}
if (opt.threshold > 0)
{
if (opt.threshold < 255)
{
cv::threshold(processedImage, processedImage, opt.threshold, 255, cv::THRESH_BINARY);
}
else
{
//Auto threshold:
cv::threshold(processedImage, processedImage, 127, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);
}
}
if (opt.negative_image)
{
cv::Mat sub_mat = cv::Mat::ones(processedImage.size(), processedImage.type()) * 255;
cv::subtract(sub_mat, processedImage, processedImage);
}
if (opt.edge_detection_opacity > 0.0)
{
cv::Mat blurImage;
cv::Mat edgesImage;
int ratio = 3;
int kernel_size = 3;
int lowThreshold = opt.edge_detection_threshold;
if (lowThreshold <= 0)
{
lowThreshold = 127;
}
cv::blur(processedImage, blurImage, cv::Size(3, 3));
cv::Canny(blurImage, edgesImage, lowThreshold, lowThreshold*ratio, kernel_size);
double alpha = opt.edge_detection_opacity;
if (alpha > 1.0)
{
alpha = 1.0;
}
double beta = (1.0 - alpha);
cv::addWeighted(edgesImage, alpha, processedImage, beta, 0.0, processedImage);
}
if ((opt.flip >= 1) && (opt.flip <= 3))
{
int flipCode;
if (opt.flip == 1) //h
{
flipCode = 1;
}
else if (opt.flip == 2) //v
{
flipCode = 0;
}
else if (opt.flip == 3) //h+v
{
flipCode = -1;
}
cv::flip(processedImage, processedImage, flipCode);
}
if (preview)
{
//Screen views
//imwrite("raspivoice_capture_raw.jpg", rawImage);
//imwrite("raspivoice_capture_scaled_gray.jpg", processedImage);
cv::imshow("RaspiVoice Preview", processedImage);
cv::waitKey(200);
}
/* Set live camera image */
for (int j = 0; j < columns; j++)
{
for (int i = 0; i < rows; i++)
{
int mVal = processedImage.at<uchar>(rows - 1 - i, j) / 16;
if (mVal == 0)
{
(*image)[IDX2D(i, j)] = 0;
}
else
{
(*image)[IDX2D(i, j)] = pow(10.0, (mVal - 15) / 10.0); // 2dB steps
}
}
}
}
}
int main(int argc, char *argv[]) {
gflags::ParseCommandLineFlags(&argc, &argv, true);
cv::vector<cv::Mat> lefts, rights;
const cv::Size patternSize(9, 6);
cv::vector<cv::vector<cv::Point3f>> worldPoints;
cv::vector<cv::vector<cv::vector<cv::Point2f>>> imagePoints(2);
for (size_t i = 0; i < 2; i++)
imagePoints[i].resize(FLAGS_size);
loadImages(lefts, rights, FLAGS_size);
FLAGS_size = findChessboards(lefts, rights, imagePoints, patternSize, FLAGS_size);
std::cout << "number of correct files = " << FLAGS_size << std::endl;
setWorldPoints(worldPoints, patternSize, 0.024, FLAGS_size);
std::cout << "calibrate stereo cameras" << std::endl;
cv::vector<cv::Mat> cameraMatrix(2);
cv::vector<cv::Mat> distCoeffs(2);
cameraMatrix[0] = cv::Mat::eye(3, 3, CV_64FC1);
cameraMatrix[1] = cv::Mat::eye(3, 3, CV_64FC1);
distCoeffs[0] = cv::Mat(8, 1, CV_64FC1);
distCoeffs[1] = cv::Mat(8, 1, CV_64FC1);
cv::Mat R, T, E, F;
double rms = stereoCalibrate(
worldPoints, imagePoints[0], imagePoints[1], cameraMatrix[0],
distCoeffs[0], cameraMatrix[1], distCoeffs[1], lefts[0].size(),
R, T, E, F, cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5),
CV_CALIB_FIX_ASPECT_RATIO +
CV_CALIB_ZERO_TANGENT_DIST +
CV_CALIB_SAME_FOCAL_LENGTH +
CV_CALIB_RATIONAL_MODEL +
CV_CALIB_FIX_K3 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5);
std::cout << "done with RMS error = " << rms << std::endl;
double err = 0;
int npoints = 0;
for (int i = 0; i < FLAGS_size; i++) {
int size = (int) imagePoints[0][i].size();
cv::vector<cv::Vec3f> lines[2];
cv::Mat imgpt[2];
for (int k = 0; k < 2; k++) {
imgpt[k] = cv::Mat(imagePoints[k][i]);
cv::undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k],
cv::Mat(), cameraMatrix[k]);
cv::computeCorrespondEpilines(imgpt[k], k + 1, F, lines[k]);
}
for (int j = 0; j < size; j++) {
double errij =
std::fabs(imagePoints[0][i][j].x * lines[1][j][0] +
imagePoints[0][i][j].y * lines[1][j][1] +
lines[1][j][2]) +
std::fabs(imagePoints[1][i][j].x * lines[0][j][0] +
imagePoints[1][i][j].y * lines[0][j][1] +
lines[0][j][2]);
err += errij;
}
npoints += size;
}
std::cout << "average reprojection error = " << err / npoints << std::endl;
cv::Mat R1, R2, P1, P2, Q;
cv::Rect validROI[2];
stereoRectify(cameraMatrix[0], distCoeffs[0], cameraMatrix[1],
distCoeffs[1], lefts[0].size(), R, T, R1, R2, P1, P2, Q,
cv::CALIB_ZERO_DISPARITY, 1, lefts[0].size(),
&validROI[0], &validROI[1]);
{
cv::FileStorage fs(FLAGS_intrinsics.c_str(), cv::FileStorage::WRITE);
if (fs.isOpened()) {
fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0]
<< "M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
fs.release();
}
}
cv::Mat rmap[2][2];
cv::initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1,
lefts[0].size(),
CV_16SC2,
rmap[0][0], rmap[0][1]);
cv::initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2,
lefts[0].size(),
CV_16SC2,
rmap[1][0], rmap[1][1]);
{
cv::FileStorage fs(FLAGS_extrinsics.c_str(), cv::FileStorage::WRITE);
if (fs.isOpened()) {
fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2
<< "P1" << P1 << "P2" << P2 << "Q" << Q
<< "V1" << validROI[0] << "V2" << validROI[1];
fs.release();
}
}
cv::Mat canvas;
double sf;
int w, h;
sf = 600. / MAX(lefts[0].size().width, lefts[0].size().height);
w = cvRound(lefts[0].size().width * sf);
h = cvRound(lefts[0].size().height * sf);
canvas.create(h, w * 2, CV_8UC3);
cv::namedWindow("Rectified", CV_WINDOW_AUTOSIZE | CV_WINDOW_FREERATIO);
for (int i = 0; i < FLAGS_size; i++) {
for (int k = 0; k < 2; k++) {
if (k == 0) {
cv::Mat img = lefts[i].clone(), rimg, cimg;
cv::remap(img, rimg, rmap[k][0], rmap[k][1], CV_INTER_LINEAR);
cv::cvtColor(rimg, cimg, CV_GRAY2BGR);
cv::Mat canvasPart = canvas(cv::Rect(w * k, 0, w, h));
cv::resize(cimg, canvasPart, canvasPart.size(), 0, 0, CV_INTER_AREA);
cv::Rect vroi(cvRound(validROI[k].x * sf),
cvRound(validROI[k].y * sf),
cvRound(validROI[k].width * sf),
cvRound(validROI[k].height * sf));
cv::rectangle(canvasPart, vroi, cv::Scalar(0, 0, 255), 3, 8);
} else {
cv::Mat img = rights[i].clone(), rimg, cimg;
cv::remap(img, rimg, rmap[k][0], rmap[k][1], CV_INTER_LINEAR);
cvtColor(rimg, cimg, CV_GRAY2BGR);
cv::Mat canvasPart = canvas(cv::Rect(w * k, 0, w, h));
cv::resize(cimg, canvasPart, canvasPart.size(), 0, 0, CV_INTER_AREA);
cv::Rect vroi(cvRound(validROI[k].x * sf),
cvRound(validROI[k].y * sf),
cvRound(validROI[k].width * sf),
cvRound(validROI[k].height * sf));
cv::rectangle(canvasPart, vroi, cv::Scalar(0, 0, 255), 3, 8);
}
}
for (int j = 0; j < canvas.rows; j += 16)
cv::line(canvas, cv::Point(0, j), cv::Point(canvas.cols, j),
cv::Scalar(0, 255, 0), 1, 8);
cv::imshow("Rectified", canvas);
if (cv::waitKey(0) == 'q')
break;
}
cv::destroyAllWindows();
return 0;
}
void Calib::computeCameraPosePnP()
{
std::cout << "There are " << imagePoints.size() << " imagePoints and " << objectPoints.size() << " objectPoints." << std::endl;
cv::Mat cameraMatrix(3,3,cv::DataType<double>::type);
cv::setIdentity(cameraMatrix);
for (unsigned i = 0; i < 3; i++)
{
for (unsigned int j = 0; j < 3; j++)
{
cameraMatrix.at<double> (i, j) = M(i, j);
}
}
std::cout << "Initial cameraMatrix: " << cameraMatrix << std::endl;
cv::Mat distCoeffs(4,1,cv::DataType<double>::type);
distCoeffs.at<double>(0) = 0;
distCoeffs.at<double>(1) = 0;
distCoeffs.at<double>(2) = 0;
distCoeffs.at<double>(3) = 0;
cv::Mat rvec(3,1,cv::DataType<double>::type);
cv::Mat tvec(3,1,cv::DataType<double>::type);
std::vector<cv::Point3d> objectPoints_cv;
std::vector<cv::Point2d> imagePoints_cv;
for(int point_id = 0;point_id < objectPoints.size();point_id++)
{
cv::Point3d object_point;
cv::Point2d image_point;
object_point.x = objectPoints[point_id](0);
object_point.y = objectPoints[point_id](1);
object_point.z = objectPoints[point_id](2);
image_point.x = imagePoints[point_id](0);
image_point.y = imagePoints[point_id](1);
objectPoints_cv.push_back(object_point);
imagePoints_cv.push_back(image_point);
}
cv::solvePnP(objectPoints_cv, imagePoints_cv, cameraMatrix, distCoeffs, rvec, tvec);
std::cout << "rvec: " << rvec << std::endl;
std::cout << "tvec: " << tvec << std::endl;
std::vector<cv::Point2d> projectedPoints;
cv::projectPoints(objectPoints_cv, rvec, tvec, cameraMatrix, distCoeffs, projectedPoints);
for(unsigned int i = 0; i < projectedPoints.size(); ++i)
{
std::cout << "Image point: " << imagePoints[i] << " Projected to " << projectedPoints[i] << std::endl;
}
//inverting the R|t to get camera position in world co-ordinates
cv::Mat Rot(3,3,cv::DataType<double>::type);
cv::Rodrigues(rvec, Rot);
Rot = Rot.t();
tvec = -Rot*tvec;
this->R << Rot.at<double>(0,0), Rot.at<double>(0,1), Rot.at<double>(0,2), Rot.at<double>(1,0), Rot.at<double>(1,1), Rot.at<double>(1,2), Rot.at<double>(2,0), Rot.at<double>(2,1), Rot.at<double>(2,2);
this->T << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
}
