hogman_minimal: hogman_minimal: axes.h Source File

00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 //
00004 // This file is part of HOG-Man.
00005 // 
00006 // HOG-Man is free software: you can redistribute it and/or modify
00007 // it under the terms of the GNU General Public License as published by
00008 // the Free Software Foundation, either version 3 of the License, or
00009 // (at your option) any later version.
00010 // 
00011 // HOG-Man is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU General Public License for more details.
00015 // 
00016 // You should have received a copy of the GNU General Public License
00017 // along with HOG-Man.  If not, see <http://www.gnu.org/licenses/>.
00018 
00019 #ifndef AXES_HH
00020 #define AXES_HH
00021 
00022 #include <iostream>
00023 #include <fstream>
00024 
00025 // forward decl
00026 class Axes;
00027 std::ostream& operator<<(std::ostream &stream, const Axes& axes);
00028 
00029 class Axes
00030 {
00031   public:
00032     Axes(double len = 1.) : 
00033       _len(len)
00034     {}
00035 
00036     double getLen() const { return _len; }
00037 
00038     void saveToFile(const char* filename, bool append = false)
00039     {
00040       if (append) {
00041         std::ofstream fout(filename, std::ios::app);
00042         fout << *this;
00043         fout.close();
00044       } else {
00045         std::ofstream fout(filename);
00046         fout << *this;
00047         fout.close();
00048       }
00049     }
00050 
00051   private:
00052     double _len;
00053 };
00054 
00055 inline std::ostream& operator<<(std::ostream &stream, const Axes& axes)
00056 {
00057   double len = axes.getLen();
00058 
00059   // top-level transform around everything
00060   stream << "#coordinate axes" << std::endl;
00061   stream << "Transform {\n";
00062   stream << "   translation 0 0 0\n";
00063   stream << "   children [\n";
00064 
00065   stream << "Shape {\n";
00066   stream << "  appearance Appearance {" << std::endl;
00067   stream << "    material Material { " << std::endl;
00068   stream << "         diffuseColor 0.8 0.8 0.8 " << std::endl;
00069   stream << "         ambientIntensity 0.2 " << std::endl;
00070   stream << "         emissiveColor 0.0 0.0 0.0 " << std::endl;
00071   stream << "         specularColor 0.0 0.0 0.0 " << std::endl;
00072   stream << "         shininess 0.2 " << std::endl;
00073   stream << "         transparency 0.0 " << std::endl;
00074   stream << "    } " << std::endl;
00075   stream << "  } " << std::endl;
00076   stream << "  geometry IndexedLineSet {\n";
00077   stream << "     coord Coordinate {\n";
00078   stream << "        point [ 0 0 0, " << len << " 0 0, 0 " << len << " 0, 0 0 " << len << "]\n";
00079   stream << "     }\n";
00080   stream << "     coordIndex [ 0 1 -1 0 2 -1 0 3 -1]\n";
00081   stream << "     color Color {\n"
00082          << "       color [0.8 0.0 0.0, 0.0 0.8 0.0, 0.0 0.0 0.8]\n"
00083          << "     }\n";
00084   stream << "     colorPerVertex FALSE\n";
00085   stream << "    }\n";
00086   stream << "}\n";
00087 
00088   // X Arrow
00089   stream << "Transform {\n";
00090   stream << "   rotation 0 0 1 -1.5707963\n";
00091   stream << "   translation " <<  len << " 0 0\n";
00092   stream << "   children [\n";
00093   stream << "   Shape {\n";
00094   stream << "     appearance Appearance {" << std::endl;
00095   stream << "       material Material { " << std::endl;
00096   stream << "          diffuseColor 0.8 0.0 0.0 " << std::endl;
00097   stream << "          ambientIntensity 0.2 " << std::endl;
00098   stream << "          emissiveColor 0.0 0.0 0.0 " << std::endl;
00099   stream << "          specularColor 0.0 0.0 0.0 " << std::endl;
00100   stream << "          shininess 0.2 " << std::endl;
00101   stream << "          transparency 0.0 " << std::endl;
00102   stream << "       } " << std::endl;
00103   stream << "     } " << std::endl;
00104   stream << "     geometry Cone {\n";
00105   stream << "        bottomRadius " << len / 100. << "\n";
00106   stream << "        height " << len /10. << "\n";
00107   stream << "        }\n";
00108   stream << "     }\n";
00109   stream << "     ]\n";
00110   stream << "}\n";
00111 
00112   // X Label
00113   stream << "Transform {\n";
00114   stream << "   translation " <<  .95 * len << " " << -len/40. << " " << len/ 30. << "\n";
00115   stream << "   children [\n";
00116   stream << "   Shape {\n";
00117   stream << "     appearance Appearance {" << std::endl;
00118   stream << "       material Material { " << std::endl;
00119   stream << "          diffuseColor 0.8 0.0 0.0 " << std::endl;
00120   stream << "          ambientIntensity 0.2 " << std::endl;
00121   stream << "          emissiveColor 0.0 0.0 0.0 " << std::endl;
00122   stream << "          specularColor 0.0 0.0 0.0 " << std::endl;
00123   stream << "          shininess 0.2 " << std::endl;
00124   stream << "          transparency 0.0 " << std::endl;
00125   stream << "       } " << std::endl;
00126   stream << "     } " << std::endl;
00127   stream << "     geometry Text {\n";
00128   stream << "       string \"X\"";
00129   stream << "       fontStyle FontStyle {\n";
00130   stream << "         size " <<  len /15. << "\n";
00131   stream << "           }\n";
00132   stream << "        }\n";
00133   stream << "       }\n";
00134   stream << "     ]\n";
00135   stream << "    }\n";
00136 
00137   // Y Arrow
00138   stream << "Transform {\n";
00139   stream << "   rotation 0 0 1 0\n";
00140   stream << "   translation 0 " <<  len << " 0\n";
00141   stream << "   children [\n";
00142   stream << "   Shape {\n";
00143   stream << "     appearance Appearance {" << std::endl;
00144   stream << "       material Material { " << std::endl;
00145   stream << "          diffuseColor 0.0 0.8 0.0 " << std::endl;
00146   stream << "          ambientIntensity 0.2 " << std::endl;
00147   stream << "          emissiveColor 0.0 0.0 0.0 " << std::endl;
00148   stream << "          specularColor 0.0 0.0 0.0 " << std::endl;
00149   stream << "          shininess 0.2 " << std::endl;
00150   stream << "          transparency 0.0 " << std::endl;
00151   stream << "       } " << std::endl;
00152   stream << "     } " << std::endl;
00153   stream << "     geometry Cone {\n";
00154   stream << "        bottomRadius " << len / 100. << "\n";
00155   stream << "        height " << len /10. << "\n";
00156   stream << "        }\n";
00157   stream << "     }\n";
00158   stream << "     ]\n";
00159   stream << "}\n";
00160 
00161   // Y Label
00162   stream << "Transform {\n";
00163   stream << "   translation " << -len/40. << " " <<  .95 * len << " " << len/ 30. << "\n";
00164   stream << "   children [\n";
00165   stream << "   Shape {\n";
00166   stream << "     appearance Appearance {" << std::endl;
00167   stream << "       material Material { " << std::endl;
00168   stream << "          diffuseColor 0.0 0.8 0.0 " << std::endl;
00169   stream << "          ambientIntensity 0.2 " << std::endl;
00170   stream << "          emissiveColor 0.0 0.0 0.0 " << std::endl;
00171   stream << "          specularColor 0.0 0.0 0.0 " << std::endl;
00172   stream << "          shininess 0.2 " << std::endl;
00173   stream << "          transparency 0.0 " << std::endl;
00174   stream << "       } " << std::endl;
00175   stream << "     } " << std::endl;
00176   stream << "     geometry Text {\n";
00177   stream << "       string \"Y\"";
00178   stream << "       fontStyle FontStyle {\n";
00179   stream << "         size " <<  len /15. << "\n";
00180   stream << "           }\n";
00181   stream << "        }\n";
00182   stream << "       }\n";
00183   stream << "     ]\n";
00184   stream << "    }\n";
00185 
00186   // Z arrow
00187   stream << "Transform {\n";
00188   stream << "   rotation 1 0 0 1.5707963 \n";
00189   stream << "   translation 0 0 " <<  .96 * len << " \n";
00190   stream << "   children [\n";
00191   stream << "   Shape {\n";
00192   stream << "     appearance Appearance {" << std::endl;
00193   stream << "       material Material { " << std::endl;
00194   stream << "          diffuseColor 0.0 0.0 0.8 " << std::endl;
00195   stream << "          ambientIntensity 0.2 " << std::endl;
00196   stream << "          emissiveColor 0.0 0.0 0.0 " << std::endl;
00197   stream << "          specularColor 0.0 0.0 0.0 " << std::endl;
00198   stream << "          shininess 0.2 " << std::endl;
00199   stream << "          transparency 0.0 " << std::endl;
00200   stream << "       } " << std::endl;
00201   stream << "     } " << std::endl;
00202   stream << "     geometry Cone {\n";
00203   stream << "        bottomRadius " << len / 100. << "\n";
00204   stream << "        height " << len /10. << "\n";
00205   stream << "        }\n";
00206   stream << "     }\n";
00207   stream << "     ]\n";
00208   stream << "}\n";
00209 
00210   // Z label
00211   stream << "Transform {\n";
00212   stream << "   rotation 0 1 0 -1.5707963 \n";
00213   stream << "   translation "  << -len/30. << " " << -len/ 40. <<  " " << .95 * len << "\n";
00214   stream << "   children [\n";
00215   stream << "   Shape {\n";
00216   stream << "     appearance Appearance {" << std::endl;
00217   stream << "       material Material { " << std::endl;
00218   stream << "          diffuseColor 0.0 0.0 0.8 " << std::endl;
00219   stream << "          ambientIntensity 0.2 " << std::endl;
00220   stream << "          emissiveColor 0.0 0.0 0.0 " << std::endl;
00221   stream << "          specularColor 0.0 0.0 0.0 " << std::endl;
00222   stream << "          shininess 0.2 " << std::endl;
00223   stream << "          transparency 0.0 " << std::endl;
00224   stream << "       } " << std::endl;
00225   stream << "     } " << std::endl;
00226   stream << "     geometry Text {\n";
00227   stream << "       string \"Z\"";
00228   stream << "       fontStyle FontStyle {\n";
00229   stream << "         size " <<  len /15. << "\n";
00230   stream << "           }\n";
00231   stream << "        }\n";
00232   stream << "       }\n";
00233   stream << "     ]\n";
00234   stream << "    }\n";
00235 
00236   // close top-level transform
00237   stream << "  ]\n";
00238   stream << "}\n";
00239   
00240   return stream;
00241 }
00242 
00243 #endif