Subversion Repositories seema-scanner

Rev

Rev 89 | Rev 91 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 89 Rev 90
Line 286... Line 286...
286 
    float nz = u.at<float>(idx, 2);
 286 
    float nz = u.at<float>(idx, 2);
287 
    float d  = u.at<float>(idx, 3);
 287 
    float d  = u.at<float>(idx, 3);
288 
    float dh = u.at<float>(idx, 4);
 288 
    float dh = u.at<float>(idx, 4);
289 
    float dv = u.at<float>(idx, 5);
 289 
    float dv = u.at<float>(idx, 5);
290 
 
 290 
 
- 
 
 291 
//    // Paper version: c is initially eliminated
- 
 
 292 
//    cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));
- 
 
 293 
//    cv::Mat bb(l*mn, 1, CV_32F);
- 
 
 294 
 
- 
 
 295 
//    for(int k=0; k<l; k++){
- 
 
 296 
//        for(int idx=0; idx<mn; idx++){
- 
 
 297 
 
- 
 
 298 
//            float i = Qobj[idx].x;
- 
 
 299 
//            float j = Qobj[idx].y;
- 
 
 300 
 
- 
 
 301 
//            float x = Qcam[k][idx].x;
- 
 
 302 
//            float y = Qcam[k][idx].y;
- 
 
 303 
//            float z = Qcam[k][idx].z;
- 
 
 304 
 
- 
 
 305 
//            float f = x*x + y*y + z*z + (2*x*nx + 2*y*ny + 2*z*nz)*(i*dh + j*dv);
- 
 
 306 
 
- 
 
 307 
//            int row = k*mn+idx;
- 
 
 308 
//            A.at<float>(row, 0) = 2*x - (2*z*nx)/nz;
- 
 
 309 
//            A.at<float>(row, 1) = 2*y - (2*z*ny)/nz;
- 
 
 310 
//            A.at<float>(row, idx+2) = 1.0;
- 
 
 311 
 
- 
 
 312 
//            bb.at<float>(row, 0) = f + (2*z*d)/nz;
- 
 
 313 
//        }
- 
 
 314 
//    }
- 
 
 315 
 
- 
 
 316 
//    // solve for point
- 
 
 317 
//    cv::Mat abe;
- 
 
 318 
//    cv::solve(A, bb, abe, cv::DECOMP_SVD);
- 
 
 319 
 
- 
 
 320 
//    float a = abe.at<float>(0, 0);
- 
 
 321 
//    float b = abe.at<float>(1, 0);
- 
 
 322 
//    float c = -(nx*a+ny*b+d)/nz;
- 
 
 323 
 
- 
 
 324 
    // Our version: solve simultanously for a,b,c
291 
    cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));
 325 
    cv::Mat A(l*mn, mn+3, CV_32F, cv::Scalar(0.0));
292 
    cv::Mat bb(l*mn, 1, CV_32F);
 326 
    cv::Mat bb(l*mn, 1, CV_32F);
293 
 
 327 
 
294 
    for(int k=0; k<l; k++){
 328 
    for(int k=0; k<l; k++){
295 
        for(int idx=0; idx<mn; idx++){
 329 
        for(int idx=0; idx<mn; idx++){
296 
 
 330 
 
Line 302... Line 336...
302 
            float z = Qcam[k][idx].z;
 336 
            float z = Qcam[k][idx].z;
303 
 
 337 
 
304 
            float f = x*x + y*y + z*z + (2*x*nx + 2*y*ny + 2*z*nz)*(i*dh + j*dv);
 338 
            float f = x*x + y*y + z*z + (2*x*nx + 2*y*ny + 2*z*nz)*(i*dh + j*dv);
305 
 
 339 
 
306 
            int row = k*mn+idx;
 340 
            int row = k*mn+idx;
307 
            A.at<float>(row, 0) = 2*x - (2*z*nx)/nz;
 341 
            A.at<float>(row, 0) = 2*x;
308 
            A.at<float>(row, 1) = 2*y - (2*z*ny)/nz;
 342 
            A.at<float>(row, 1) = 2*y;
- 
 
 343 
            A.at<float>(row, 2) = 2*z;
309 
            A.at<float>(row, idx+2) = 1.0;
 344 
            A.at<float>(row, idx+3) = 1.0;
310 
 
 345 
 
311 
            bb.at<float>(row, 0) = f + (2*z*d)/nz;
 346 
            bb.at<float>(row, 0) = f;
312 
        }
 347 
        }
313 
    }
 348 
    }
314 
 
 349 
 
315 
    // solve for point
 350 
    // solve for point
316 
    cv::Mat abe;
 351 
    cv::Mat abe;
317 
    cv::solve(A, bb, abe, cv::DECOMP_SVD);
 352 
    cv::solve(A, bb, abe, cv::DECOMP_SVD);
318 
 
 353 
 
319 
    float a = abe.at<float>(0, 0);
 354 
    float a = abe.at<float>(0, 0);
320 
    float b = abe.at<float>(1, 0);
 355 
    float b = abe.at<float>(1, 0);
321 
    float c = -(nx*a+ny*b+d)/nz;
 356 
    float c = abe.at<float>(2, 0);
322 
 
 357 
 
323 
    point[0] = a;
 358 
    point[0] = a;
324 
    point[1] = b;
 359 
    point[1] = b;
325 
    point[2] = c;
 360 
    point[2] = c;
326 
 
 361