gpc_test.c
Go to the documentation of this file.
00001 // GPC: A library for the solution of General Point Correspondence problems.
00002 // Copyright (C) 2006 Andrea Censi (andrea at censi dot org)
00003 
00004 // This program is free software; you can redistribute it and/or
00005 // modify it under the terms of the GNU General Public License
00006 // as published by the Free Software Foundation; either version 2
00007 // of the License, or (at your option) any later version.
00008 
00009 // This program is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU General Public License for more details.
00013 
00014 // You should have received a copy of the GNU General Public License
00015 // along with this program; if not, write to the Free Software
00016 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00017 
00018 #include <stdio.h>
00019 #include <math.h>
00020 #include "gpc.h"
00021 
00022 #define deg2rad(x) (x*M_PI/180)
00023 
00024 int main() {
00025         // This is the true roto-translation
00026         double theta = deg2rad(4);
00027         double t[2] = {0.3,-0.2};
00028         printf("x_true =  %f  %f  %f deg\n", t[0], t[1], theta*180/M_PI);       
00029         
00030         double p[5][2] = {{1,0},{0,1},{-1,0},{2,1},{4,2}};
00031         double alpha[5] = {deg2rad(0), 
00032                 deg2rad(10), deg2rad(20), deg2rad(50),deg2rad(-20)};
00033 
00034         struct gpc_corr c[5];
00035         int k;
00036         for(k=0;k<5;k++) {
00037                 c[k].p[0] = p[k][0];
00038                 c[k].p[1] = p[k][1];
00039                 c[k].q[0] = t[0] + p[k][0]*cos(theta) - p[k][1]*sin(theta);
00040                 c[k].q[1] = t[1] + p[k][0]*sin(theta) + p[k][1]*cos(theta);
00041                 c[k].C[0][0] = cos(alpha[k])*cos(alpha[k]);
00042                 c[k].C[0][1] = c[k].C[1][0] = cos(alpha[k])*sin(alpha[k]);
00043                 c[k].C[1][1] = sin(alpha[k])*sin(alpha[k]);
00044         }
00045 
00046         double x[3];
00047         gpc_solve(5,c,x);
00048 
00049         printf("estimated x =  %f  %f  %f deg\n", x[0], x[1],x[2]*180/M_PI);
00050         return 0;
00051 }
00052 


csm_ros
Author(s): Gaƫl Ecorchard , Ivan Dryanovski, William Morris, Andrea Censi
autogenerated on Sat Jun 8 2019 19:52:42