Subversion Repositories seema-scanner

Rev

Rev 86 | Rev 111 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 86 Rev 88
Line 188... Line 188...
188 
        std::cout << std::endl;
 188 
        std::cout << std::endl;
189 
    }
 189 
    }
190 
std::cout << "Rr: " << cal.Rr << std::endl;
 190 
std::cout << "Rr: " << cal.Rr << std::endl;
191 
std::cout << "Tr: " << cal.Tr << std::endl;
 191 
std::cout << "Tr: " << cal.Tr << std::endl;
192 
 
 192 
 
193 
    //* Using rotation axis calibration *//
 193 
//    //* Using rotation axis calibration *//
194 
    // full camera matrices
 194 
//    // full camera matrices
195 
    cv::Matx34f P0 = cv::Matx34f::eye();
 195 
//    cv::Matx34f P0 = cal.K0*cv::Matx34f::eye();
196 
    cv::Mat RT1(3, 4, CV_32F);
 196 
//    cv::Mat RT1(3, 4, CV_32F);
197 
    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)));
198 
    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)));
199 
    cv::Matx34f P1 = cv::Matx34f(RT1);
 199 
//    cv::Matx34f P1 = cal.K1*cv::Matx34f(RT1);
200 
 
 200 
 
201 
    // calibration points in camera 0 frame
 201 
//    // calibration points in camera 0 frame
202 
    std::vector< std::vector<cv::Point3f> > Qcam;
 202 
//    std::vector< std::vector<cv::Point3f> > Qcam;
203 
 
 203 
 
204 
    for(int i=0; i<nValidSets; i++){
 204 
//    for(int i=0; i<nValidSets; i++){
205 
        std::vector<cv::Point2f> qc0i, qc1i;
 205 
//        std::vector<cv::Point2f> qc0i, qc1i;
206 
 
 206 
 
207 
        cv::undistortPoints(qc0[i], qc0i, cal.K0, cal.k0);
 207 
////        cv::undistortPoints(qc0[i], qc0i, cal.K0, cal.k0);
208 
        cv::undistortPoints(qc1[i], qc1i, cal.K1, cal.k1);
 208 
////        cv::undistortPoints(qc1[i], qc1i, cal.K1, cal.k1);
- 
 
 209 
//        qc0i = qc0[i];
- 
 
 210 
//        qc1i = qc1[i];
209 
 
 211 
 
210 
        cv::Mat Qhom, Qcami;
 212 
//        cv::Mat Qhom, Qcami;
211 
        cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);
 213 
//        cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);
212 
        cvtools::convertMatFromHomogeneous(Qhom, Qcami);
 214 
//        cvtools::convertMatFromHomogeneous(Qhom, Qcami);
213 
        std::vector<cv::Point3f> QcamiPoints;
 215 
//        std::vector<cv::Point3f> QcamiPoints;
214 
        cvtools::matToPoints3f(Qcami, QcamiPoints);
 216 
//        cvtools::matToPoints3f(Qcami, QcamiPoints);
- 
 
 217 
 
- 
 
 218 
//        Qcam.push_back(QcamiPoints);
- 
 
 219 
//    }
- 
 
 220 
 
- 
 
 221 
//    cv::Vec3f axis, point;
- 
 
 222 
//    cvtools::rotationAxisCalibration(Qcam, Qi, axis, point);
- 
 
 223 
 
- 
 
 224 
//    // construct transformation matrix
- 
 
 225 
//    cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,-1.0));
- 
 
 226 
//    cv::Vec3f ez = ex.cross(axis);
- 
 
 227 
 
- 
 
 228 
//    cv::Mat RrMat(3, 3, CV_32F);
- 
 
 229 
//    cv::Mat(ex).copyTo(RrMat.col(0));
- 
 
 230 
//    cv::Mat(axis).copyTo(RrMat.col(1));
- 
 
 231 
//    cv::Mat(ez).copyTo(RrMat.col(2));
215 
 
 232 
 
216 
        Qcam.push_back(QcamiPoints);
 - 
 
217 
    }
 - 
 
218 
 
 - 
 
219 
    cv::Vec3f axis, point;
 - 
 
220 
    cvtools::rotationAxisCalibration(Qcam, Qi, axis, point);
 - 
 
221 
 
 - 
 
222 
    // construct transformation matrix
 - 
 
223 
    cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,1.0));
 - 
 
224 
    cv::Vec3f ez = axis.cross(ex);
 - 
 
225 
 
 - 
 
226 
    cv::Mat RrMat(3, 3, CV_32F);
 - 
 
227 
    cv::Mat(ex).copyTo(RrMat.col(0));
 - 
 
228 
    cv::Mat(axis).copyTo(RrMat.col(1));
 - 
 
229 
    cv::Mat(ez).copyTo(RrMat.col(2));
 - 
 
230 
 
 - 
 
231 
    cal.Rr = cv::Mat(RrMat.t());
 233 
//    cal.Rr = cv::Mat(RrMat.t());
232 
    cal.Tr = -point;
 234 
//    cal.Tr = point;
233 
 
 235 
 
234 
    // Print to std::cout
 236 
    // Print to std::cout
235 
    cal.print();
 237 
    cal.print();
236 
 
 238 
 
237 
    // save to (reentrant qsettings object)
 239 
    // save to (reentrant qsettings object)