outofcore_base_data.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  *  $Id$
00037  *
00038  */
00039 
00040 
00041 #include <pcl/outofcore/outofcore_base_data.h>
00042 
00043 #include <pcl/pcl_macros.h>
00044 #include <pcl/exceptions.h>
00045 #include <pcl/console/print.h>
00046 
00047 #include <fstream>
00048 
00049 #define __PCL_OUTOFCORE_VERSION__ 3
00050 
00051 namespace pcl
00052 {
00053   namespace outofcore
00054   {
00055     OutofcoreOctreeBaseMetadata::OutofcoreOctreeBaseMetadata () 
00056       : metadata_filename_ ()
00057       , outofcore_version_ ()
00058       , coordinate_system_ ()
00059       , tree_name_ ()
00060       , point_type_ ("urp")
00061       , levels_of_depth_ ()
00062       , LOD_num_points_ ()
00063     {
00064     }
00065       
00067 
00068     OutofcoreOctreeBaseMetadata::OutofcoreOctreeBaseMetadata (const boost::filesystem::path& metadata_filename) 
00069       : metadata_filename_ (metadata_filename)
00070       , outofcore_version_ ()
00071       , coordinate_system_ ()
00072       , tree_name_ ()
00073       , point_type_ ("urp")
00074       , levels_of_depth_ ()
00075       , LOD_num_points_ ()
00076     {
00077       //read metadata from file and store in fields
00078       loadMetadataFromDisk ();
00079     }
00080     
00082       
00083     OutofcoreOctreeBaseMetadata::~OutofcoreOctreeBaseMetadata ()
00084     {
00085       this->serializeMetadataToDisk ();
00086     }
00087 
00089 
00090     OutofcoreOctreeBaseMetadata::OutofcoreOctreeBaseMetadata (const OutofcoreOctreeBaseMetadata& orig) 
00091       : OutofcoreAbstractMetadata ()
00092       , metadata_filename_ (orig.metadata_filename_)
00093       , outofcore_version_ (orig.outofcore_version_)
00094       , coordinate_system_ (orig.coordinate_system_)
00095       , tree_name_ (orig.tree_name_)
00096       , point_type_ (orig.point_type_)
00097       , levels_of_depth_ (orig.levels_of_depth_)
00098       , LOD_num_points_ (orig.LOD_num_points_)
00099     {
00100 
00101     }
00102 
00104 
00105     int
00106     OutofcoreOctreeBaseMetadata::getOutofcoreVersion () const
00107     {
00108       return (outofcore_version_);
00109     }
00110       
00112 
00113     void 
00114     OutofcoreOctreeBaseMetadata::setOutofcoreVersion (const int version)
00115     {
00116       outofcore_version_ = version;
00117     }
00118       
00120 
00121     boost::filesystem::path
00122     OutofcoreOctreeBaseMetadata::getMetadataFilename () const
00123     {
00124       return (metadata_filename_);
00125     }
00126       
00128 
00129     void
00130     OutofcoreOctreeBaseMetadata::setMetadataFilename (const boost::filesystem::path& path_to_metadata)
00131     {
00132       metadata_filename_ = path_to_metadata;
00133     }
00134 
00136 
00137     void
00138     OutofcoreOctreeBaseMetadata::serializeMetadataToDisk ()
00139     {
00140       // Create JSON object
00141       boost::shared_ptr<cJSON> idx (cJSON_CreateObject (), cJSON_Delete);
00142   
00143       cJSON* name = cJSON_CreateString (tree_name_.c_str ());
00144       cJSON* version = cJSON_CreateNumber ( __PCL_OUTOFCORE_VERSION__ );
00145       cJSON* pointtype = cJSON_CreateString (point_type_.c_str ());
00146       cJSON* lod = cJSON_CreateNumber (static_cast<double> (levels_of_depth_));
00147 
00148       // cJSON does not allow 64 bit ints.  Have to put the points in a double to
00149       // use this api, will allow counts up to 2^52 points to be stored correctly
00150       //or split into LSB MSB?
00151       std::vector<double> lodPoints_db;
00152       lodPoints_db.insert (lodPoints_db.begin (), LOD_num_points_.begin (), LOD_num_points_.end ());
00153 
00154       cJSON* numpts = cJSON_CreateDoubleArray ( &(lodPoints_db.front ()), static_cast<int>(lodPoints_db.size ()));
00155 
00156       cJSON_AddItemToObject (idx.get (), "name", name);
00157       cJSON_AddItemToObject (idx.get (), "version", version);
00158       cJSON_AddItemToObject (idx.get (), "pointtype", pointtype);
00159       cJSON_AddItemToObject (idx.get (), "lod", lod);
00160       cJSON_AddItemToObject (idx.get (), "numpts", numpts);
00161       cJSON_AddStringToObject(idx.get(), "coord_system", coordinate_system_.c_str());
00162 
00163       char* idx_txt = cJSON_Print (idx.get ());
00164 
00165       std::ofstream f (metadata_filename_.string ().c_str (), std::ios::out | std::ios::trunc);
00166       f << idx_txt;
00167       f.close ();
00168 
00169       free (idx_txt);
00170     }
00171 
00173 
00174     int
00175     OutofcoreOctreeBaseMetadata::loadMetadataFromDisk ()
00176     {
00177       // Open JSON
00178       std::vector<char> idx_input;
00179       boost::uintmax_t len = boost::filesystem::file_size (metadata_filename_);
00180       idx_input.resize (len + 1);
00181 
00182       std::ifstream f (metadata_filename_.string ().c_str (), std::ios::in);
00183       f.read (&(idx_input.front ()), len);
00184       idx_input.back () = '\0';
00185 
00186       // Parse JSON
00187       boost::shared_ptr<cJSON> idx (cJSON_Parse (&(idx_input.front ())), cJSON_Delete);
00188       cJSON* name = cJSON_GetObjectItem (idx.get (), "name");
00189       cJSON* version = cJSON_GetObjectItem (idx.get (), "version");
00190       cJSON* pointtype = cJSON_GetObjectItem (idx.get (), "pointtype");
00191       cJSON* lod = cJSON_GetObjectItem (idx.get (), "lod");
00192       cJSON* numpts = cJSON_GetObjectItem (idx.get (), "numpts");
00193       cJSON* coord = cJSON_GetObjectItem (idx.get (), "coord_system");
00194 
00195       bool parse_failure = false;
00196 
00197       // Validate JSON
00198       if (!((name) && (version) && (pointtype) && (lod) && (numpts) && (coord)))
00199       {
00200         PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseMetadata::loadMetadataFromDisk] One of expected metadata fields does not exist in %s\n", metadata_filename_.c_str ());
00201         parse_failure = true;
00202       }
00203       if ((name->type != cJSON_String) || (version->type != cJSON_Number) || (pointtype->type != cJSON_String)
00204           || (lod->type != cJSON_Number) || (numpts->type != cJSON_Array) || (coord->type != cJSON_String))
00205       {
00206         PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseMetadata::loadMetadataFromDisk] One of metadata fields does not contain its expected type in %s\n",metadata_filename_.c_str ());
00207         parse_failure = true;
00208       }
00209       if (version->valuedouble != 2.0 && version->valuedouble != 3.0)//only support version 2.0 and 3.0
00210       {
00211         PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseMetadata::loadMetadataFromDisk] Outofcore version field (just read version:number = %.1lf) in %s does not match the current supported versions\n",metadata_filename_.c_str (), version->valuedouble);
00212         parse_failure = true;
00213       }
00214       if ((lod->valueint + 1) != cJSON_GetArraySize (numpts))
00215       {
00216         PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseMetadata::loadMetadataFromDisk] lod:num+1=%d+1 (i.e. height of tree) does not match size of LOD array (%d) in %s\n", lod->valueint, cJSON_GetArraySize (numpts), metadata_filename_.c_str ());
00217         parse_failure = true;
00218       }
00219 
00220       if (parse_failure)
00221       {
00222         PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseMetadata::loadMetadataFromDisk] Parse Failure\n");
00223       }
00224       
00225 
00226       // Get Data
00227       LOD_num_points_.resize (lod->valueint + 1);
00228       for (int i = 0; i < (lod->valueint + 1); i++)
00229       {
00230         //cJSON doesn't have explicit 64bit int, have to use double, get up to 2^52
00231         LOD_num_points_[i] = static_cast<boost::uint64_t> (cJSON_GetArrayItem (numpts, i)->valuedouble );
00232       }
00233       levels_of_depth_ = lod->valueint;
00234       coordinate_system_ = coord->valuestring;
00235 
00236       //return success
00237       return (1);
00238     }
00239 
00241 
00242     int
00243     OutofcoreOctreeBaseMetadata::loadMetadataFromDisk (const boost::filesystem::path& path_to_metadata)
00244     {
00245       this->setMetadataFilename (path_to_metadata);
00246       return (this->loadMetadataFromDisk ());
00247     }
00248     
00250 
00251     std::string
00252     OutofcoreOctreeBaseMetadata::getOctreeName ()
00253     {
00254       return (this->tree_name_);
00255     }
00256 
00258 
00259     void
00260     OutofcoreOctreeBaseMetadata::setOctreeName (const std::string& name_arg)
00261     {
00262       this->tree_name_ = name_arg;
00263     }
00264     
00266 
00267     std::string
00268     OutofcoreOctreeBaseMetadata::getPointType ()
00269     {
00270       return (this->point_type_);
00271     }
00272 
00274     
00275     void
00276     OutofcoreOctreeBaseMetadata::setPointType (const std::string& point_type_arg)
00277     {
00278       this->point_type_ = point_type_arg;
00279     }
00280   
00282 
00283     std::vector<boost::uint64_t>&
00284     OutofcoreOctreeBaseMetadata::getLODPoints ()
00285     {
00286       return (LOD_num_points_);
00287     }
00288     
00290 
00291     std::vector<boost::uint64_t>
00292     OutofcoreOctreeBaseMetadata::getLODPoints () const
00293     {
00294       return (LOD_num_points_);
00295     }
00296 
00298 
00299     boost::uint64_t
00300     OutofcoreOctreeBaseMetadata::getLODPoints (const boost::uint64_t& depth_index) const
00301     {
00302       return (LOD_num_points_[depth_index]);
00303     }
00304 
00306 
00307     void
00308     OutofcoreOctreeBaseMetadata::setLODPoints (const boost::uint64_t& depth)
00309     {
00310       LOD_num_points_.clear ();
00311       LOD_num_points_.resize (depth);
00312       assert (LOD_num_points_.size () == this->levels_of_depth_+1);
00313     }
00314 
00316 
00317     void
00318     OutofcoreOctreeBaseMetadata::setLODPoints (std::vector<boost::uint64_t>& lod_points_arg)
00319     {
00320       assert (this->LOD_num_points_.size () == lod_points_arg.size ());
00321       
00322       this->LOD_num_points_ = lod_points_arg;
00323     }
00324     
00326 
00327     void
00328     OutofcoreOctreeBaseMetadata::setLODPoints (const boost::uint64_t& lod_index_arg, const boost::uint64_t& num_points_arg, const bool increment)
00329     {
00330       assert (lod_index_arg < LOD_num_points_.size ());
00331 
00332       if (increment)
00333         LOD_num_points_[lod_index_arg] += num_points_arg;
00334       else
00335         LOD_num_points_[lod_index_arg] = num_points_arg;
00336     }
00337     
00339     
00340     void
00341     OutofcoreOctreeBaseMetadata::setCoordinateSystem (const std::string& coord_sys_arg)
00342     {
00343       this->coordinate_system_ = coord_sys_arg;
00344     }
00345     
00347 
00348     std::string
00349     OutofcoreOctreeBaseMetadata::getCoordinateSystem () const
00350     {
00351       return (this->coordinate_system_);
00352     }
00353 
00355 
00356     void
00357     OutofcoreOctreeBaseMetadata::setDepth (const boost::uint64_t& depth_arg)
00358     {
00359       this->levels_of_depth_ = depth_arg;
00360     }
00361     
00362     boost::uint64_t
00363     OutofcoreOctreeBaseMetadata::getDepth () const
00364     {
00365       return (levels_of_depth_);
00366     }
00367     
00369     //Protected Member Functions
00371     void
00372     OutofcoreOctreeBaseMetadata::writeMetadataString (std::vector<char>& buf)
00373     {
00374       (void)buf;
00375       PCL_THROW_EXCEPTION (PCLException, "Not implemented\n");
00376     }
00377 
00379 
00380     std::ostream& 
00381     operator<<(std::ostream& os, const OutofcoreOctreeBaseMetadata& metadata_arg)
00382     {
00383       (void) metadata_arg;
00384       return (os);
00385     }
00386 
00388 
00389   }//namespace outofcore
00390 }//namespace pcl
00391 #undef __PCL_OUTOFCORE_VERSION__


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:27:30