1 /*
2  * Software License Agreement (BSD License)
3  *
4  *  Point Cloud Library (PCL) - www.pointclouds.org
5  *  Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  *  All rights reserved.
8  *
9  *  Redistribution and use in source and binary forms, with or without
10  *  modification, are permitted provided that the following conditions
11  *  are met:
12  *
13  *   * Redistributions of source code must retain the above copyright
14  *     notice, this list of conditions and the following disclaimer.
15  *   * Redistributions in binary form must reproduce the above
16  *     copyright notice, this list of conditions and the following
17  *     disclaimer in the documentation and/or other materials provided
18  *     with the distribution.
19  *   * Neither the name of Willow Garage, Inc. nor the names of its
20  *     contributors may be used to endorse or promote products derived
21  *     from this software without specific prior written permission.
22  *
23  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  *  POSSIBILITY OF SUCH DAMAGE.
35  *
36  *  $Id$
37  *
38  */
39 
40 #include <pcl/outofcore/outofcore_node_data.h>
41 
42 #include <pcl/console/print.h>
43 
44 #include <pcl/pcl_macros.h>
45 #include <pcl/exceptions.h> // for PCL_THROW_EXCEPTION, PCLException
46 
47 #include <iostream>
48 #include <fstream>
49 #include <memory>
50 
51 namespace pcl
52 {
53   namespace outofcore
54   {
55 
OutofcoreOctreeNodeMetadata()56     OutofcoreOctreeNodeMetadata::OutofcoreOctreeNodeMetadata ()
57       : outofcore_version_ ()
58     {
59     }
60 
61     ////////////////////////////////////////////////////////////////////////////////
62 
~OutofcoreOctreeNodeMetadata()63     OutofcoreOctreeNodeMetadata::~OutofcoreOctreeNodeMetadata ()
64     {
65     }
66 
67     ////////////////////////////////////////////////////////////////////////////////
68 
OutofcoreOctreeNodeMetadata(const OutofcoreOctreeNodeMetadata & orig)69     OutofcoreOctreeNodeMetadata::OutofcoreOctreeNodeMetadata (const OutofcoreOctreeNodeMetadata& orig)
70     {
71       this->min_bb_ = orig.min_bb_;
72       this->max_bb_ = orig.max_bb_;
73       this->binary_point_filename_ = orig.binary_point_filename_;
74       this->midpoint_xyz_ = orig.midpoint_xyz_;
75       this->directory_ = orig.directory_;
76       this->metadata_filename_ = orig.metadata_filename_;
77       this->outofcore_version_ = orig.outofcore_version_;
78 
79       this->updateVoxelCenter ();
80     }
81 
82     ////////////////////////////////////////////////////////////////////////////////
83 
84     const Eigen::Vector3d&
getBoundingBoxMin() const85     OutofcoreOctreeNodeMetadata::getBoundingBoxMin () const
86     {
87       return (min_bb_);
88     }
89 
90     ////////////////////////////////////////////////////////////////////////////////
91 
92     void
setBoundingBoxMin(const Eigen::Vector3d & min_bb)93     OutofcoreOctreeNodeMetadata::setBoundingBoxMin (const Eigen::Vector3d& min_bb)
94     {
95       min_bb_ = min_bb;
96       this->updateVoxelCenter ();
97     }
98 
99     ////////////////////////////////////////////////////////////////////////////////
100 
101     const Eigen::Vector3d&
getBoundingBoxMax() const102     OutofcoreOctreeNodeMetadata::getBoundingBoxMax () const
103     {
104       return (max_bb_);
105     }
106 
107     ////////////////////////////////////////////////////////////////////////////////
108 
109     void
setBoundingBoxMax(const Eigen::Vector3d & max_bb)110     OutofcoreOctreeNodeMetadata::setBoundingBoxMax (const Eigen::Vector3d& max_bb)
111     {
112       max_bb_ = max_bb;
113       this->updateVoxelCenter ();
114     }
115 
116     ////////////////////////////////////////////////////////////////////////////////
117 
118     void
getBoundingBox(Eigen::Vector3d & min_bb,Eigen::Vector3d & max_bb) const119     OutofcoreOctreeNodeMetadata::getBoundingBox (Eigen::Vector3d &min_bb, Eigen::Vector3d &max_bb) const
120     {
121       min_bb = min_bb_;
122       max_bb = max_bb_;
123     }
124 
125     ////////////////////////////////////////////////////////////////////////////////
126 
127     void
setBoundingBox(const Eigen::Vector3d & min_bb,const Eigen::Vector3d & max_bb)128     OutofcoreOctreeNodeMetadata::setBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb)
129     {
130       min_bb_ = min_bb;
131       max_bb_ = max_bb;
132       this->updateVoxelCenter ();
133     }
134 
135     ////////////////////////////////////////////////////////////////////////////////
136 
137     const boost::filesystem::path&
getDirectoryPathname() const138     OutofcoreOctreeNodeMetadata::getDirectoryPathname () const
139     {
140       return (directory_);
141     }
142 
143     ////////////////////////////////////////////////////////////////////////////////
144     void
setDirectoryPathname(const boost::filesystem::path & directory_pathname)145     OutofcoreOctreeNodeMetadata::setDirectoryPathname (const boost::filesystem::path& directory_pathname)
146     {
147       directory_ = directory_pathname;
148     }
149 
150     ////////////////////////////////////////////////////////////////////////////////
151 
152     const boost::filesystem::path&
getPCDFilename() const153     OutofcoreOctreeNodeMetadata::getPCDFilename () const
154     {
155       return (binary_point_filename_);
156     }
157 
158     ////////////////////////////////////////////////////////////////////////////////
159 
160     void
setPCDFilename(const boost::filesystem::path & point_filename)161     OutofcoreOctreeNodeMetadata::setPCDFilename (const boost::filesystem::path& point_filename)
162     {
163       binary_point_filename_ = point_filename;
164     }
165 
166 
167     ////////////////////////////////////////////////////////////////////////////////
168 
169     int
getOutofcoreVersion() const170     OutofcoreOctreeNodeMetadata::getOutofcoreVersion () const
171     {
172       return (outofcore_version_);
173     }
174 
175     ////////////////////////////////////////////////////////////////////////////////
176 
177     void
setOutofcoreVersion(const int version)178     OutofcoreOctreeNodeMetadata::setOutofcoreVersion (const int version)
179     {
180       outofcore_version_ = version;
181     }
182 
183     ////////////////////////////////////////////////////////////////////////////////
184 
185     const boost::filesystem::path&
getMetadataFilename() const186     OutofcoreOctreeNodeMetadata::getMetadataFilename () const
187     {
188       return (metadata_filename_);
189     }
190 
191     ////////////////////////////////////////////////////////////////////////////////
192 
193     void
setMetadataFilename(const boost::filesystem::path & path_to_metadata)194     OutofcoreOctreeNodeMetadata::setMetadataFilename (const boost::filesystem::path& path_to_metadata)
195     {
196       directory_ = path_to_metadata.parent_path ();
197       metadata_filename_ = path_to_metadata;
198     }
199 
200     ////////////////////////////////////////////////////////////////////////////////
201 
202     const Eigen::Vector3d&
getVoxelCenter() const203     OutofcoreOctreeNodeMetadata::getVoxelCenter () const
204     {
205       return (midpoint_xyz_);
206     }
207 
208     ////////////////////////////////////////////////////////////////////////////////
209 
210     void
serializeMetadataToDisk()211     OutofcoreOctreeNodeMetadata::serializeMetadataToDisk ()
212     {
213       std::shared_ptr<cJSON> idx (cJSON_CreateObject (), cJSON_Delete);
214 
215       cJSON* cjson_outofcore_version = cJSON_CreateNumber (outofcore_version_);
216 
217       double min_array[3];
218       double max_array[3];
219 
220       for(int i=0; i<3; i++)
221       {
222         min_array[i] = min_bb_[i];
223         max_array[i] = max_bb_[i];
224       }
225 
226       cJSON* cjson_bb_min = cJSON_CreateDoubleArray (min_array, 3);
227       cJSON* cjson_bb_max = cJSON_CreateDoubleArray (max_array, 3);
228 
229       std::string binary_point_filename_string = binary_point_filename_.filename ().generic_string ();
230       cJSON* cjson_bin_point_filename = cJSON_CreateString (binary_point_filename_string.c_str ());
231 
232       cJSON_AddItemToObject (idx.get (), "version", cjson_outofcore_version);
233       cJSON_AddItemToObject (idx.get (), "bb_min", cjson_bb_min);
234       cJSON_AddItemToObject (idx.get (), "bb_max", cjson_bb_max);
235       cJSON_AddItemToObject (idx.get (), "bin", cjson_bin_point_filename);
236 
237       char* idx_txt = cJSON_Print (idx.get ());
238 
239       std::ofstream f (metadata_filename_.string ().c_str (), std::ios::out | std::ios::trunc);
240       f << idx_txt;
241       f.close ();
242 
243       free (idx_txt);
244     }
245 
246     ////////////////////////////////////////////////////////////////////////////////
247 
248     int
loadMetadataFromDisk()249     OutofcoreOctreeNodeMetadata::loadMetadataFromDisk ()
250     {
251       if(directory_ != metadata_filename_.parent_path ())
252       {
253         PCL_ERROR ("directory_ is not set correctly\n");
254       }
255 
256       //if the file to load doesn't exist, return failure
257       if (!boost::filesystem::exists (metadata_filename_))
258       {
259         PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeNodeMetadata] Can not find index metadata at %s.\n", metadata_filename_.c_str ());
260         return (0);
261       }
262       if(boost::filesystem::is_directory (metadata_filename_))
263       {
264         PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeNodeMetadata] Got a directory, but no oct_idx metadata?\n");
265         return (0);
266       }
267 
268       //load CJSON
269       std::vector<char> idx_input;
270       std::uintmax_t len = boost::filesystem::file_size (metadata_filename_);
271       idx_input.resize (len + 1);
272 
273       std::ifstream f (metadata_filename_.string ().c_str (), std::ios::in);
274       f.read (&(idx_input.front ()), len);
275       idx_input.back () = '\0';
276 
277       //Parse
278       std::shared_ptr<cJSON> idx (cJSON_Parse (&(idx_input.front ())), cJSON_Delete);
279 
280       cJSON* cjson_outofcore_version = cJSON_GetObjectItem (idx.get (), "version");
281       cJSON* cjson_bb_min = cJSON_GetObjectItem (idx.get (), "bb_min");
282       cJSON* cjson_bb_max = cJSON_GetObjectItem (idx.get (), "bb_max");
283       cJSON* cjson_bin_point_filename = cJSON_GetObjectItem (idx.get (), "bin");
284 
285       bool parse_failure = false;
286 
287       //Sanitize
288       if (!cjson_outofcore_version)
289       {
290         PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeNodeMetadata::%s] Failed to parse \"version\" field of node metadata %s\n", __FUNCTION__, metadata_filename_.c_str ());
291         parse_failure = true;
292       }
293       if (!cjson_bb_min)
294       {
295         PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeNodeMetadata::%s] Failed to parse \"bb_min\" field of node metadata %s\n", __FUNCTION__, metadata_filename_.c_str ());
296         parse_failure = true;
297       }
298       if (!cjson_bb_max)
299       {
300         PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeNodeMetadata::%s] Failed to parse \"bb_max\" field of node metadata %s\n", __FUNCTION__, metadata_filename_.c_str ());
301         parse_failure = true;
302       }
303       if (!cjson_bin_point_filename)
304       {
305         PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeNodeMetadata::%s] Failed to parse \"bin\" field of node metadata %s\n", __FUNCTION__, metadata_filename_.c_str ());
306         parse_failure = true;
307       }
308 
309       if (parse_failure)
310       {
311         PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeNodeMetadata::%s] Outofcore node metadata parse error\n");
312       }
313 
314       for (int i = 0; i < 3; i++)
315       {
316         min_bb_[i] = cJSON_GetArrayItem (cjson_bb_min, i)->valuedouble;
317         max_bb_[i] = cJSON_GetArrayItem (cjson_bb_max, i)->valuedouble;
318       }
319       outofcore_version_ = cjson_outofcore_version->valueint;
320 
321       binary_point_filename_= directory_ / cjson_bin_point_filename->valuestring;
322       midpoint_xyz_ = (max_bb_+min_bb_)/static_cast<double>(2.0);
323 
324       //return success
325       return (1);
326     }
327     ////////////////////////////////////////////////////////////////////////////////
328 
329     int
loadMetadataFromDisk(const boost::filesystem::path & path_to_metadata)330     OutofcoreOctreeNodeMetadata::loadMetadataFromDisk (const boost::filesystem::path& path_to_metadata)
331     {
332       this->setMetadataFilename (path_to_metadata);
333       this->setDirectoryPathname (path_to_metadata.parent_path ());
334       return (this->loadMetadataFromDisk ());
335     }
336 
337     ////////////////////////////////////////////////////////////////////////////////
338 
339     std::ostream&
operator <<(std::ostream &,const OutofcoreOctreeNodeMetadata &)340     operator<<(std::ostream&, const OutofcoreOctreeNodeMetadata&)
341     {
342       //todo: implement me
343       PCL_THROW_EXCEPTION (PCLException, "Not implemented\n");
344     }
345 
346     ////////////////////////////////////////////////////////////////////////////////
347 
348 
349   }//namespace outofcore
350 }//namespace pcl
351 
352 
353 
354 
355