| Line 92... |
Line 92... |
| 92 |
cal.frameWidth = calibrationData[0].frame0.cols;
|
92 |
cal.frameWidth = calibrationData[0].frame0.cols;
|
| 93 |
cal.frameHeight = calibrationData[0].frame0.rows;
|
93 |
cal.frameHeight = calibrationData[0].frame0.rows;
|
| 94 |
cv::Size frameSize(cal.frameWidth, cal.frameHeight);
|
94 |
cv::Size frameSize(cal.frameWidth, cal.frameHeight);
|
| 95 |
|
95 |
|
| 96 |
// determine only k1, k2 for lens distortion
|
96 |
// determine only k1, k2 for lens distortion
|
| 97 |
int flags = 0; //cv::CALIB_FIX_K3;
|
97 |
int flags = cv::CALIB_FIX_K3;
|
| 98 |
// Note: several of the output arguments below must be cv::Mat, otherwise segfault
|
98 |
// Note: several of the output arguments below must be cv::Mat, otherwise segfault
|
| 99 |
std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
|
99 |
std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
|
| 100 |
cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);
|
100 |
cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);
|
| 101 |
|
101 |
|
| - |
|
102 |
// refine extrinsics for camera 0
|
| - |
|
103 |
for(int i=0; i<Q.size(); i++)
|
| - |
|
104 |
cv::solvePnPRansac(Q[i], qc0[i], cal.K0, cal.k0, cam_rvecs0[i], cam_tvecs0[i], true, 100, 0.05, 100, cv::noArray(), CV_ITERATIVE);
|
| - |
|
105 |
|
| 102 |
std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
|
106 |
std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
|
| 103 |
cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags);
|
107 |
cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags);
|
| 104 |
|
108 |
|
| 105 |
// stereo calibration (fix K0, K1, k0, k1)
|
109 |
// stereo calibration (fix K0, K1, k0, k1)
|
| 106 |
int flags_stereo = cv::CALIB_FIX_INTRINSIC;
|
110 |
int flags_stereo = cv::CALIB_FIX_INTRINSIC;
|
| Line 181... |
Line 185... |
| 181 |
cv::Mat Tcr = -cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t())*cv::Mat(cal.Tr) + cv::Mat(cal.Rr)*cv::Mat(Tc[i]) + cv::Mat(cal.Tr);
|
185 |
cv::Mat Tcr = -cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t())*cv::Mat(cal.Tr) + cv::Mat(cal.Rr)*cv::Mat(Tc[i]) + cv::Mat(cal.Tr);
|
| 182 |
std::cout << "Rcr[i]" << Rcr << std::endl;
|
186 |
std::cout << "Rcr[i]" << Rcr << std::endl;
|
| 183 |
std::cout << "Tcr[i]" << Tcr << std::endl;
|
187 |
std::cout << "Tcr[i]" << Tcr << std::endl;
|
| 184 |
std::cout << std::endl;
|
188 |
std::cout << std::endl;
|
| 185 |
}
|
189 |
}
|
| - |
|
190 |
std::cout << "Rr: " << cal.Rr << std::endl;
|
| - |
|
191 |
std::cout << "Tr: " << cal.Tr << std::endl;
|
| 186 |
|
192 |
|
| 187 |
//* Using rotation axis calibration *//
|
193 |
//* Using rotation axis calibration *//
|
| 188 |
// full camera matrices
|
194 |
// full camera matrices
|
| 189 |
cv::Matx34f P0 = cal.K0*cv::Matx34f::eye();
|
195 |
cv::Matx34f P0 = cv::Matx34f::eye();
|
| 190 |
cv::Mat RT1(3, 4, CV_32F);
|
196 |
cv::Mat RT1(3, 4, CV_32F);
|
| 191 |
cv::Mat(cal.R1).copyTo(RT1(cv::Range(0, 3), cv::Range(0, 3)));
|
197 |
cv::Mat(cal.R1).copyTo(RT1(cv::Range(0, 3), cv::Range(0, 3)));
|
| 192 |
cv::Mat(cal.T1).copyTo(RT1(cv::Range(0, 3), cv::Range(3, 4)));
|
198 |
cv::Mat(cal.T1).copyTo(RT1(cv::Range(0, 3), cv::Range(3, 4)));
|
| 193 |
cv::Matx34f P1 = cal.K1*cv::Matx34f(RT1);
|
199 |
cv::Matx34f P1 = cv::Matx34f(RT1);
|
| 194 |
|
200 |
|
| 195 |
// calibration points in camera 0 frame
|
201 |
// calibration points in camera 0 frame
|
| 196 |
std::vector< std::vector<cv::Point3f> > Qcam;
|
202 |
std::vector< std::vector<cv::Point3f> > Qcam;
|
| 197 |
|
203 |
|
| 198 |
for(int i=0; i<nValidSets; i++){
|
204 |
for(int i=0; i<nValidSets; i++){
|
| 199 |
std::vector<cv::Point3f> qc0i, qc1i;
|
205 |
std::vector<cv::Point2f> qc0i, qc1i;
|
| - |
|
206 |
|
| 200 |
cv::undistortPoints(qc0[i], qc0i, cal.K0, cal.k0);
|
207 |
cv::undistortPoints(qc0[i], qc0i, cal.K0, cal.k0);
|
| 201 |
cv::undistortPoints(qc1[i], qc1i, cal.K0, cal.k0);
|
208 |
cv::undistortPoints(qc1[i], qc1i, cal.K1, cal.k1);
|
| 202 |
|
209 |
|
| 203 |
cv::Mat Qhom, Qcami;
|
210 |
cv::Mat Qhom, Qcami;
|
| 204 |
cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);
|
211 |
cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);
|
| 205 |
cvtools::convertMatFromHomogeneous(Qhom, Qcami);
|
212 |
cvtools::convertMatFromHomogeneous(Qhom, Qcami);
|
| 206 |
std::vector<cv::Point3f> QcamiPoints;
|
213 |
std::vector<cv::Point3f> QcamiPoints;
|
| Line 214... |
Line 221... |
| 214 |
|
221 |
|
| 215 |
// construct transformation matrix
|
222 |
// construct transformation matrix
|
| 216 |
cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,1.0));
|
223 |
cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,1.0));
|
| 217 |
cv::Vec3f ez = axis.cross(ex);
|
224 |
cv::Vec3f ez = axis.cross(ex);
|
| 218 |
|
225 |
|
| 219 |
cal.Rr.col(0) = ex;
|
226 |
cv::Mat RrMat(3, 3, CV_32F);
|
| 220 |
cal.Rr.col(1) = axis;
|
227 |
cv::Mat(ex).copyTo(RrMat.col(0));
|
| - |
|
228 |
cv::Mat(axis).copyTo(RrMat.col(1));
|
| 221 |
cal.Rr.col(2) = ez;
|
229 |
cv::Mat(ez).copyTo(RrMat.col(2));
|
| 222 |
|
230 |
|
| - |
|
231 |
cal.Rr = cv::Mat(RrMat.t());
|
| 223 |
cal.Tr = point;
|
232 |
cal.Tr = -point;
|
| 224 |
|
233 |
|
| 225 |
// Print to std::cout
|
234 |
// Print to std::cout
|
| 226 |
cal.print();
|
235 |
cal.print();
|
| 227 |
|
236 |
|
| 228 |
// save to (reentrant qsettings object)
|
237 |
// save to (reentrant qsettings object)
|