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