affine_transform.cpp
Go to the documentation of this file.
00001 /*
00002  *  affine_transform.cpp
00003  *  online_patch
00004  *
00005  *  Created by Victor  Eruhimov on 5/16/09.
00006  *  Copyright 2009 Argus Corp. All rights reserved.
00007  *
00008  */
00009 
00010 #include "outlet_pose_estimation/detail/affine_transform.h"
00011 #include "outlet_pose_estimation/detail/one_way_descriptor_base.h"
00012 
00013 using namespace std;
00014 
00015 void cvmSet6(CvMat* m, int row, int col, float val1, float val2, float val3, float val4, float val5, float val6)
00016 {
00017     cvmSet(m, row, col, val1);
00018     cvmSet(m, row, col + 1, val2);
00019     cvmSet(m, row, col + 2, val3);
00020     cvmSet(m, row, col + 3, val4);
00021     cvmSet(m, row, col + 4, val5);
00022     cvmSet(m, row, col + 5, val6);
00023 }
00024 
00025 void FindAffineTransform(const std::vector<CvPoint>& p1, const std::vector<CvPoint>& p2, CvMat* affine)
00026 {
00027     int eq_num = 2*(int)p1.size();
00028     CvMat* A = cvCreateMat(eq_num, 6, CV_32FC1);
00029     CvMat* B = cvCreateMat(eq_num, 1, CV_32FC1);
00030     CvMat* X = cvCreateMat(6, 1, CV_32FC1);
00031 
00032     for(int i = 0; i < (int)p1.size(); i++)
00033     {
00034         cvmSet6(A, 2*i, 0, p1[i].x, p1[i].y, 1, 0, 0, 0);
00035         cvmSet6(A, 2*i + 1, 0, 0, 0, 0, p1[i].x, p1[i].y, 1);
00036         cvmSet(B, 2*i, 0, p2[i].x);
00037         cvmSet(B, 2*i + 1, 0, p2[i].y);
00038     }
00039 
00040     cvSolve(A, B, X, CV_SVD);
00041 
00042     cvmSet(affine, 0, 0, cvmGet(X, 0, 0));
00043     cvmSet(affine, 0, 1, cvmGet(X, 1, 0));
00044     cvmSet(affine, 0, 2, cvmGet(X, 2, 0));
00045     cvmSet(affine, 1, 0, cvmGet(X, 3, 0));
00046     cvmSet(affine, 1, 1, cvmGet(X, 4, 0));
00047     cvmSet(affine, 1, 2, cvmGet(X, 5, 0));
00048 
00049     cvReleaseMat(&A);
00050     cvReleaseMat(&B);
00051     cvReleaseMat(&X);
00052 }
00053 
00054 void MapVectorAffine(const std::vector<CvPoint>& p1, std::vector<CvPoint>& p2, CvMat* transform)
00055 {
00056     float a = cvmGet(transform, 0, 0);
00057     float b = cvmGet(transform, 0, 1);
00058     float c = cvmGet(transform, 0, 2);
00059     float d = cvmGet(transform, 1, 0);
00060     float e = cvmGet(transform, 1, 1);
00061     float f = cvmGet(transform, 1, 2);
00062 
00063     for(int i = 0; i < (int)p1.size(); i++)
00064     {
00065         float x = a*p1[i].x + b*p1[i].y + c;
00066         float y = d*p1[i].x + e*p1[i].y + f;
00067         p2.push_back(cvPoint(x, y));
00068     }
00069 }
00070 
00071 void MapFeaturesAffine(const std::vector<feature_t>& features, std::vector<feature_t>& mapped_features,
00072     CvMat* transform)
00073 {
00074     float a = cvmGet(transform, 0, 0);
00075     float b = cvmGet(transform, 0, 1);
00076     float c = cvmGet(transform, 0, 2);
00077     float d = cvmGet(transform, 1, 0);
00078     float e = cvmGet(transform, 1, 1);
00079     float f = cvmGet(transform, 1, 2);
00080 
00081     for(int i = 0; i < (int)features.size(); i++)
00082     {
00083         float x = a*features[i].pt.x + b*features[i].pt.y + c;
00084         float y = d*features[i].pt.x + e*features[i].pt.y + f;
00085         mapped_features.push_back(feature_t(cvPoint(x, y), features[i].size, features[i].class_id));
00086     }
00087 }
00088 
00089 float CalcAffineReprojectionError(const std::vector<CvPoint>& p1, const std::vector<CvPoint>& p2,
00090     CvMat* transform)
00091 {
00092     vector<CvPoint> mapped_p1;
00093     MapVectorAffine(p1, mapped_p1, transform);
00094     float error = 0;
00095     for(int i = 0; i < (int)p2.size(); i++)
00096     {
00097         float l = length(p2[i] - mapped_p1[i]);
00098         error += l*l;
00099     }
00100 
00101     error /= p2.size();
00102 
00103     return error;
00104 }


outlet_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Mon Dec 2 2013 13:21:49