planar.cpp
Go to the documentation of this file.
00001 /*
00002  *  planar.cpp
00003  *  outlet_model
00004  *
00005  *  Created by Victor  Eruhimov on 1/16/09.
00006  *  Copyright 2009 Argus Corp. All rights reserved.
00007  *
00008  */
00009 
00010 //*****************************************************************************************
00011 // Warning: this is research code with poor architecture, performance and no documentation!
00012 //*****************************************************************************************
00013 
00014 #include <time.h>
00015 
00016 #include "outlet_pose_estimation/detail/planar.h"
00017 #include <highgui.h>
00018 #include <stdio.h>
00019 
00020 #include <limits>
00021 
00022 void gen_3x3_matrix(CvMat* in, CvMat* out)
00023 {
00024         for(int r = 0; r < 2; r++)
00025         {
00026                 for(int c = 0; c < 3; c++)
00027                 {
00028                         float value = cvmGet(in, r, c);
00029                         cvmSet(out, r, c, value);
00030                 }
00031         }
00032 
00033         cvmSet(out, 2, 0, 0.0f);
00034         cvmSet(out, 2, 1, 0.0f);
00035         cvmSet(out, 2, 2, 1.0f);
00036 }
00037 
00038 CvPoint2D32f apply_mat(CvMat* mat, CvPoint2D32f vec)
00039 {
00040         CvPoint2D32f res = cvPoint2D32f(cvmGet(mat, 0, 0)*vec.x + cvmGet(mat, 0, 1)*vec.y,
00041                                                                         cvmGet(mat, 1, 0)*vec.x + cvmGet(mat, 1, 1)*vec.y);
00042         return(res);
00043 }
00044 
00045 CvRect calc_mapped_rectangle(CvMat* mat, CvRect roi)
00046 {
00047         CvPoint2D32f p[4];
00048         p[0] = cvPoint2D32f(roi.x, roi.y);
00049         p[1] = cvPoint2D32f(roi.x + roi.width, roi.y);
00050         p[2] = cvPoint2D32f(roi.x + roi.width, roi.y + roi.height);
00051         p[3] = cvPoint2D32f(roi.x, roi.y + roi.height);
00052         
00053         CvPoint2D32f r[4];
00054         for(int i = 0; i < 4; i++)
00055         {
00056                 r[i] = apply_mat(mat, p[i]);
00057         }
00058         
00059         float xmin = FLT_MAX, xmax = -FLT_MAX, ymin = FLT_MAX, ymax = -FLT_MAX;
00060         for(int i = 0; i < 4; i++)
00061         {
00062                 xmin = MIN(xmin, r[i].x);
00063                 xmax = MAX(xmax, r[i].x);
00064                 ymin = MIN(ymin, r[i].y);
00065                 ymax = MAX(ymax, r[i].y);
00066         }
00067         
00068         return(cvRect(xmin, ymin, xmax - xmin, ymax - ymin));
00069 }
00070 
00071 void copy_cols(CvMat* src, CvMat* dst, int start_col, int end_col)
00072 {
00073         for(int r = 0; r < src->rows; r++)
00074         {
00075                 for(int c = start_col; c < end_col; c++)
00076                 {
00077                         float val = cvmGet(src, r, c);
00078                         cvmSet(dst, r, c, val);
00079                 }
00080         }
00081 }
00082 
00083 // generates a 2x3 affine transform 
00084 CvSize gen_random_homog_transform(CvRect roi, CvMat* mat)
00085 {
00086         CvMat* rt = cvCreateMat(2, 2, CV_32FC1);
00087         CvMat* rp = cvCreateMat(2, 2, CV_32FC1);
00088         CvMat* rpi = cvCreateMat(2, 2, CV_32FC1);
00089         CvMat* s = cvCreateMat(2, 2, CV_32FC1);
00090         CvMat* final = cvCreateMat(2, 2, CV_32FC1);
00091         
00092         CvMat* temp = cvCreateMat(2, 3, CV_32FC1);
00093                 
00094         float phi = (float(rand())/RAND_MAX*2 - 1)*60;
00095         cv2DRotationMatrix(cvPoint2D32f(0, 0), phi, 1, temp);
00096         copy_cols(temp, rp, 0, 2);
00097 
00098         cv2DRotationMatrix(cvPoint2D32f(0, 0), -phi, 1, temp);
00099         copy_cols(temp, rpi, 0, 2);
00100         
00101         const float max_angle = 30;
00102         float theta = (float(rand())/RAND_MAX*2 - 1)*max_angle;
00103         cv2DRotationMatrix(cvPoint2D32f(0, 0), theta, 1, temp);
00104         copy_cols(temp, rt, 0, 2);
00105         
00106         const float smin = 0.8f;
00107         const float smax = 1.3f;
00108         float l1 = float(rand())/RAND_MAX*(smax - smin) + smin;
00109         float l2 = float(rand())/RAND_MAX*(smax - smin) + smin;
00110         cvmSet(s, 0, 0, l1);
00111         cvmSet(s, 0, 1, 0.0f);
00112         cvmSet(s, 1, 0, 0.0f);
00113         cvmSet(s, 1, 1, l2);
00114         
00115         cvMatMul(rt, rpi, final);
00116         cvMatMul(final, s, final);
00117         cvMatMul(final, rp, final);
00118         
00119         CvRect bound = calc_mapped_rectangle(final, roi);
00120         
00121         for(int r = 0; r < 2; r++)
00122         {
00123                 for(int c = 0; c < 2; c++)
00124                 {
00125                         float value = cvmGet(final, r, c);
00126                         cvmSet(mat, r, c, value);
00127                 }
00128         }
00129         
00130         int length = MAX(bound.width, bound.height);
00131         bound.x -= (length - bound.width)/2;
00132         bound.y -= (length - bound.height)/2;
00133         
00134         cvmSet(mat, 0, 2, -bound.x);
00135         cvmSet(mat, 1, 2, -bound.y);
00136         
00137         cvReleaseMat(&rt);
00138         cvReleaseMat(&rp);
00139         cvReleaseMat(&s);
00140         cvReleaseMat(&rpi);
00141         cvReleaseMat(&temp);
00142         cvReleaseMat(&final);
00143         
00144         
00145         return cvSize(length, length);
00146 }
00147 
00148 void gen_random_homog_patches(IplImage* src, int count, IplImage** dst)
00149 {
00150         srand(clock());
00151         
00152         CvRect roi = cvGetImageROI(src);
00153         cvResetImageROI(src);
00154         
00155         CvMat* transform = cvCreateMat(2, 3, CV_32FC1);
00156         for(int i = 0; i < count; i++)
00157         {
00158                 CvSize size = gen_random_homog_transform(roi, transform);
00159                 dst[i] = cvCreateImage(size, IPL_DEPTH_8U, 1);
00160                 cvWarpAffine(src, dst[i], transform);
00161                 cvEqualizeHist(dst[i], dst[i]);
00162         }
00163         cvSetImageROI(src, roi);
00164         
00165         cvReleaseMat(&transform);
00166 }
00167 
00168 void save_image_array(const char* folder, const char* filename, int count, IplImage** images)
00169 {
00170         for(int i = 0; i < count; i++)
00171         {
00172                 char buf[1024];
00173                 sprintf(buf, "%s/%s_%d.jpg", folder, filename, i);
00174                 cvSaveImage(buf, images[i]);
00175         }
00176 }
00177 
00178 void release_image_array(int count, IplImage** images)
00179 {
00180         for(int i = 0; i < count; i++)
00181         {
00182                 cvReleaseImage(&images[i]);
00183         }
00184 }
00185 void test_homog_transform(IplImage* src)
00186 {
00187         const int count = 30;
00188         IplImage* images[count];
00189         gen_random_homog_patches(src, count, images);
00190         save_image_array("../../patches", "", count, images);
00191         release_image_array(count, images);
00192 }
00193 
00194 namespace cv{
00195     
00196     
00197 // Algorithm:
00198 // plane equation: P*N + c = 0
00199 // we find point rays in 3D from image points and camera parameters
00200 // then we fit c by minimizing average L2 distance between rotated and translated object points
00201 // and points found by crossing point rays with plane. We use the fact that center of mass 
00202 // of object points and fitted points should coincide.
00203 void findPlanarObjectPose(const Mat& object_points, const Mat& image_points, const Point3f& normal, 
00204                               const Mat& intrinsic_matrix, const Mat& distortion_coeffs, std::vector<Point3f>& object_points_crf)
00205 {
00206     vector<Point2f> _rays;
00207     undistortPoints(image_points, _rays, intrinsic_matrix, distortion_coeffs);
00208     
00209     // filter rays that are parallel to the plane
00210     vector<Point3f> rays;
00211     for(size_t i = 0; i < _rays.size(); i++)
00212     {
00213         double proj = rays[i].dot(normal);
00214         if(fabs(proj) > std::numeric_limits<double>::epsilon())
00215         {
00216             rays.push_back(Point3f(_rays[i].x, _rays[i].y, 1.0f));
00217         }
00218     }
00219     
00220     Point3f pm(0.0f, 0.0f, 0.0f);
00221     for(size_t i = 0; i < rays.size(); i++)
00222     {
00223         pm = pm + rays[i]*(1.0/rays[i].dot(normal));
00224     }
00225     
00226     pm = -pm*(1.0/rays.size());
00227     
00228     double dev = 0.0;
00229     for(size_t i = 0; i < rays.size(); i++)
00230     {
00231         Point3f pdev = rays[i]*(1.0/rays[i].dot(normal)) - pm;
00232         dev += pdev.dot(pdev);
00233     }
00234     
00235     double C = -1.0/sqrt(dev);
00236     
00237     object_points_crf.resize(rays.size());
00238     for(size_t i = 0; i < rays.size(); i++)
00239     {
00240         object_points_crf[i] = -rays[i]*(C/rays[i].dot(normal));
00241     }
00242 }
00243 
00244 };


outlet_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Thu Aug 27 2015 14:29:52