00001 /** 00002 \file Robots/LoBot/slam/LoCoords.C 00003 \brief This file defines the functions in the lobot::Coords namespace. 00004 */ 00005 00006 // //////////////////////////////////////////////////////////////////// // 00007 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2005 // 00008 // by the University of Southern California (USC) and the iLab at USC. // 00009 // See http://iLab.usc.edu for information about this project. // 00010 // //////////////////////////////////////////////////////////////////// // 00011 // Major portions of the iLab Neuromorphic Vision Toolkit are protected // 00012 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency // 00013 // in Visual Environments, and Applications'' by Christof Koch and // 00014 // Laurent Itti, California Institute of Technology, 2001 (patent // 00015 // pending; application number 09/912,225 filed July 23, 2001; see // 00016 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status). // 00017 // //////////////////////////////////////////////////////////////////// // 00018 // This file is part of the iLab Neuromorphic Vision C++ Toolkit. // 00019 // // 00020 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can // 00021 // redistribute it and/or modify it under the terms of the GNU General // 00022 // Public License as published by the Free Software Foundation; either // 00023 // version 2 of the License, or (at your option) any later version. // 00024 // // 00025 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope // 00026 // that it will be useful, but WITHOUT ANY WARRANTY; without even the // 00027 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR // 00028 // PURPOSE. See the GNU General Public License for more details. // 00029 // // 00030 // You should have received a copy of the GNU General Public License // 00031 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write // 00032 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, // 00033 // Boston, MA 02111-1307 USA. // 00034 // //////////////////////////////////////////////////////////////////// // 00035 // 00036 // Primary maintainer for this file: mviswana usc edu 00037 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Robots/LoBot/slam/LoCoords.C $ 00038 // $Id: LoCoords.C 13560 2010-06-11 12:58:57Z mviswana $ 00039 // 00040 00041 //------------------------------ HEADERS -------------------------------- 00042 00043 // lobot headers 00044 #include "Robots/LoBot/slam/LoCoords.H" 00045 #include "Robots/LoBot/slam/LoSlamParams.H" 00046 #include "Robots/LoBot/util/LoMath.H" 00047 00048 //----------------------------- NAMESPACE ------------------------------- 00049 00050 namespace lobot { 00051 namespace Coords { 00052 00053 //------------------- COORDINATE SYSTEM CONVERSIONS --------------------- 00054 00055 // The following function converts real or physical robot coordinates 00056 // into grid coordinates. The occupancy grid is a discretized version of 00057 // the underlying real/world coordinate system. The world coordinate 00058 // system goes from left (L) to right (R) in the x direction and B 00059 // (bottom) to T (top) in the y direction. The occupancy grid OTOH 00060 // extends from 0 to W-1 in x and 0 to H-1 in y with the origin being at 00061 // the top-left corner of the grid. 00062 // 00063 // Thus, to convert from world coordinates to grid coordinates, we apply 00064 // the following formulae: 00065 // 00066 // x - L gx - 0 y - B gy - H-1 00067 // ----- = ------- and ----- = -------- 00068 // R - L W-1 - 0 T - B 0 - H-1 00069 // 00070 // Rearranging the terms in the above two equations and simplifying gives 00071 // us: 00072 // 00073 // (x - L)(W - 1) (T - y)(H - 1) 00074 // gx = -------------- and gy = -------------- 00075 // (R - L) (T - B) 00076 // 00077 // If we let Sx = (W - 1)/(R - L) and Sy = (H - 1)/(T - B), then the 00078 // above expressions become: 00079 // gx = (x - L) * Sx 00080 // gy = (T - y) * Sy 00081 // 00082 // The following function uses the above two equations to convert world 00083 // coordinates (x, y) to grid coordinates (gx, gy). 00084 // 00085 // As an aside, we note that the grid's width W = (R - L)/S and its 00086 // height H = (T - B)/S where S is the size of each cell. For example, if 00087 // the map extents are [0 5000 -2500 2500] and cell size is 10mm, then 00088 // the grid will have (5000 - 0)/10 = 500 cells in the x direction and 00089 // (2500 - (-2500))/10 = 500 cells in the y direction. 00090 // 00091 // Substituting W = (R - L)/S and H = (T - B)/S in the expressions for Sx 00092 // and Sy yields the following: 00093 // 00094 // 1 1 1 1 00095 // Sx = - - ----- and Sy = - - ----- 00096 // S R - L S T - B 00097 // 00098 // where S is the cell size. 00099 // 00100 // The above expressions are used in the MapParams constructor for 00101 // initializing Sx and Sy. 00102 void to_grid(float x, float y, int* gx, int* gy) 00103 { 00104 const int W = SlamParams::map_width() ; 00105 const int H = SlamParams::map_height(); 00106 00107 float L, T ; 00108 SlamParams::map_extents(&L, 0, 0, &T) ; 00109 const float Sx = SlamParams::Sx() ; 00110 const float Sy = SlamParams::Sy() ; 00111 00112 *gx = clamp(round((x - L) * Sx), 0, W - 1) ; 00113 *gy = clamp(round((T - y) * Sy), 0, H - 1) ; 00114 } 00115 00116 // This function converts grid coordinates to world coordinates. We 00117 // simply rearrange the terms in: 00118 // gx = (x - L) * Sx 00119 // gy = (T - y) * Sy 00120 // to get: 00121 // x = L + gx/Sx 00122 // y = T - gy/Sy 00123 void to_real(int gx, int gy, float* x, float* y) 00124 { 00125 float L, R, B, T ; 00126 SlamParams::map_extents(&L, &R, &B, &T) ; 00127 const float Sx = SlamParams::Sx() ; 00128 const float Sy = SlamParams::Sy() ; 00129 00130 *x = clamp(L + gx/Sx, L, R) ; 00131 *y = clamp(T - gy/Sy, B, T) ; 00132 } 00133 00134 //----------------------------------------------------------------------- 00135 00136 } // end of Coords namespace 00137 } // end of lobot namespace 00138 00139 /* So things look consistent in everyone's emacs... */ 00140 /* Local Variables: */ 00141 /* indent-tabs-mode: nil */ 00142 /* End: */