changeScaleFactor.cpp
Go to the documentation of this file.
00001 
00046 #include "ros/ros.h"
00047 
00048 #include <iostream>
00049 #include <iomanip>
00050 #include <vector>
00051 #include <string>
00052 #include <sstream>
00053 
00054 #include <opencv/cv.h>
00055 #include <opencv/highgui.h>
00056 
00057 #include "../MetaFile.h"
00058 #include "../ObjectModel.h"
00059 
00060 #include "DUtils.h"
00061 #include "DUtilsCV.h"
00062 #include "DVision.h"
00063 
00064 typedef DVision::PMVS::PLYFile PLYFile;
00065 typedef DVision::PMVS::PLYFile::PLYPoint PLYPoint;
00066 typedef DVision::SurfSet SurfSet;
00067 
00068 using namespace std;
00069 
00070 // ---------------------------------------------------------------------------
00071 
00072 
00078 void updatePointCloud(std::vector<PLYPoint> &plypoints, float s);
00079 
00080 // ---------------------------------------------------------------------------
00081 
00082 int main(int argc, char **argv)
00083 {
00084   ros::init(argc, argv, "changeScaleFactor");
00085  
00086   if(argc < 3)
00087   {
00088     ROS_WARN("Usage: %s <model dir> <relative scale factor>", argv[0]);
00089     return 1;
00090   }
00091   
00092   string model_dir = argv[1];
00093   float relative_scale_factor = atof(argv[2]);
00094   
00095   if(!ObjectModel::checkDirectory(model_dir))
00096   {
00097     ROS_ERROR("Directory %s does not seem to contain a valid object model",
00098       model_dir.c_str());
00099     return 2;
00100   }
00101   
00102   string meta_file = model_dir + "/meta.xml";
00103   ROS_DEBUG("Reading meta file %s...", meta_file.c_str());
00104   MetaFile::MetaData meta;
00105   MetaFile::readFile(meta_file, meta);
00106   
00107   if(meta.Type != "3D")
00108   {
00109     ROS_WARN("Object %s is of type %s. Only \"3D\" models are supported",
00110       meta.Name.c_str(), meta.Type.c_str());
00111     return 3;
00112   }
00113   
00114   float current_scale_factor = meta.Dimensions.Volume.Scale;
00115   float final_scale_factor = current_scale_factor * relative_scale_factor;
00116   
00117   ROS_INFO("Changing scale factor of object %s from %f to %f (%f x %f = %f)...",
00118     meta.Name.c_str(), current_scale_factor, final_scale_factor,
00119     current_scale_factor, relative_scale_factor, final_scale_factor);
00120   
00121   // dense model  
00122   vector<PLYPoint> plypoints;
00123   
00124   string drawing_model_file = model_dir + "/drawing_model.ply";
00125   PLYFile::readFile(drawing_model_file, plypoints);  
00126   updatePointCloud(plypoints, relative_scale_factor);
00127   PLYFile::saveFile(drawing_model_file, plypoints);
00128 
00129   // face points
00130   for(int i = 0; i < meta.NFaces; ++i)
00131   {
00132     stringstream ply_file;
00133     ply_file << model_dir << "/face_" << setw(3) << setfill('0') << i
00134       << ".ply";
00135     
00136     PLYFile::readFile(ply_file.str(), plypoints);  
00137     updatePointCloud(plypoints, relative_scale_factor);
00138     PLYFile::saveFile(ply_file.str(), plypoints);
00139   }
00140   
00141   // meta file
00142   meta.Dimensions.Volume.Scale = final_scale_factor;
00143   MetaFile::saveFile(meta_file, meta);
00144   
00145   // done
00146   ROS_INFO("Done. Drawing model, %d faces and meta file modified", meta.NFaces);
00147   
00148   return 0;
00149 }
00150         
00151 // ---------------------------------------------------------------------------
00152 
00153 void updatePointCloud(std::vector<PLYPoint> &plypoints, float s)
00154 {
00155   vector<PLYPoint>::iterator pit;
00156   for(pit = plypoints.begin(); pit != plypoints.end(); ++pit)
00157   {
00158     pit->x *= s;
00159     pit->y *= s;
00160     pit->z *= s;
00161   }
00162 }
00163 
00164 // ---------------------------------------------------------------------------
00165 


re_vision
Author(s): Dorian Galvez-Lopez
autogenerated on Sun Jan 5 2014 11:30:49