Subversion Repositories seema-scanner

Rev

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

Rev 82 Rev 83
Line 238... Line 238...
238 
    cv::Mat M(6, 6, CV_32F, cv::Scalar(0));
 238 
    cv::Mat M(6, 6, CV_32F, cv::Scalar(0));
239 
 
 239 
 
240 
    for(int k=0; k<l; k++){
 240 
    for(int k=0; k<l; k++){
241 
        for(int idx=0; idx<mn; idx++){
 241 
        for(int idx=0; idx<mn; idx++){
242 
 
 242 
 
243 
            float i = Qobj[idx].x+4;
 243 
//            float i = Qobj[idx].x+4;
244 
            float j = Qobj[idx].y+4;
 244 
//            float j = Qobj[idx].y+4;
- 
 
 245 
            float i = Qobj[idx].x;
- 
 
 246 
            float j = Qobj[idx].y;
245 
 
 247 
 
246 
            float x = Qcam[k][idx].x;
 248 
            float x = Qcam[k][idx].x;
247 
            float y = Qcam[k][idx].y;
 249 
            float y = Qcam[k][idx].y;
248 
            float z = Qcam[k][idx].z;
 250 
            float z = Qcam[k][idx].z;
249 
 
 251 
 
Line 272... Line 274...
272 
            minLambda = lambda[i];
 274 
            minLambda = lambda[i];
273 
            idx = i;
 275 
            idx = i;
274 
        }
 276 
        }
275 
    }
 277 
    }
276 
 
 278 
 
277 
std::cout << "u:" <<  u << std::endl;
 279 
//std::cout << "u:" <<  u << std::endl;
278 
 
 280 
 
279 
    axis = u.row(idx).colRange(0, 3);
 281 
    axis = u.row(idx).colRange(0, 3);
280 
    axis /= cv::norm(axis);
 282 
    axis /= cv::norm(axis);
281 
 
 283 
 
282 
    float nx = u.at<float>(5, 0);
 284 
    float nx = u.at<float>(idx, 0);
283 
    float ny = u.at<float>(5, 1);
 285 
    float ny = u.at<float>(idx, 1);
284 
    float nz = u.at<float>(5, 2);
 286 
    float nz = u.at<float>(idx, 2);
285 
    float d  = u.at<float>(5, 3);
 287 
    float d  = u.at<float>(idx, 3);
286 
    float dh = u.at<float>(5, 4);
 288 
    float dh = u.at<float>(idx, 4);
287 
    float dv = u.at<float>(5, 5);
 289 
    float dv = u.at<float>(idx, 5);
288 
 
 290 
 
289 
    cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));
 291 
    cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));
290 
    cv::Mat b(l*mn, 1, CV_32F);
 292 
    cv::Mat bb(l*mn, 1, CV_32F);
291 
 
 293 
 
292 
    for(int k=0; k<l; k++){
 294 
    for(int k=0; k<l; k++){
293 
        for(int idx=0; idx<mn; idx++){
 295 
        for(int idx=0; idx<mn; idx++){
294 
 
 296 
 
295 
            float i = Qobj[idx].x;
 297 
            float i = Qobj[idx].x;
Line 304... Line 306...
304 
            int row = k*mn+idx;
 306 
            int row = k*mn+idx;
305 
            A.at<float>(row, 0) = 2*x - (2*z*nx)/nz;
 307 
            A.at<float>(row, 0) = 2*x - (2*z*nx)/nz;
306 
            A.at<float>(row, 1) = 2*y - (2*z*ny)/nz;
 308 
            A.at<float>(row, 1) = 2*y - (2*z*ny)/nz;
307 
            A.at<float>(row, idx+2) = 1.0;
 309 
            A.at<float>(row, idx+2) = 1.0;
308 
 
 310 
 
309 
            b.at<float>(row, 0) = f + (2*z*d)/nz;
 311 
            bb.at<float>(row, 0) = f + (2*z*d)/nz;
310 
        }
 312 
        }
311 
    }
 313 
    }
312 
 
 314 
 
313 
    // solve for point
 315 
    // solve for point
314 
    cv::Mat abe;
 316 
    cv::Mat abe;
315 
    cv::solve(A, b, abe, cv::DECOMP_SVD);
 317 
    cv::solve(A, bb, abe, cv::DECOMP_SVD);
316 
 
 318 
 
317 
    point[0] = abe.at<float>(0, 0);
 319 
    float a = abe.at<float>(0, 0);
318 
    point[1] = abe.at<float>(1, 0);
 320 
    float b = abe.at<float>(1, 0);
- 
 
 321 
    float c = -(nx*a+ny*b)/nz;
- 
 
 322 
 
- 
 
 323 
    point[0] = a;
- 
 
 324 
    point[1] = b;
319 
    point[2] = abe.at<float>(2, 0);
 325 
    point[2] = c;
320 
 
 326 
 
321 
}
 327 
}
322 
 
 328 
 
323 
// Function to fit two sets of corresponding pose data.
 329 
// Function to fit two sets of corresponding pose data.
324 
// Finds [Omega | tau], to minimize ||[R_mark | t_mark] - [Omega | tau][R | t]||^2
 330 
// Finds [Omega | tau], to minimize ||[R_mark | t_mark] - [Omega | tau][R | t]||^2