compression.h
Go to the documentation of this file.
00001 #ifndef MEGATREE_COMPRESSION_H
00002 #define MEGATREE_COMPRESSION_H
00003 
00004 #include <Eigen/Geometry>
00005 #include <megatree_storage/VizPoint.h>
00006 
00007 
00008 namespace megatree
00009 {
00010 
00011 void toSphericalCoordinates(const double point[3], double res[3])
00012 {
00013   // distance 
00014   res[2] = sqrt(point[0] * point[0] +
00015                 point[1] * point[1] +
00016                 point[2] * point[2]);
00017 
00018   // up-down angle
00019   res[0] = asin(point[2]/res[2]);
00020 
00021   // left-right angle
00022   res[1] = atan2(point[1], point[0]);
00023 }
00024 
00025 void fromSphericalCoordinates(const double sph_point[3], double res[3])
00026 {
00027   res[0] = cos(sph_point[0]) * cos(sph_point[1]) * sph_point[2];
00028   res[1] = cos(sph_point[0]) * sin(sph_point[1]) * sph_point[2];
00029   res[2] = sin(sph_point[0]) * sph_point[2];
00030 }
00031 
00032 
00033 
00034 
00035 
00036 void transform(const double point[3], const Eigen::Affine3d &transform, double res[3])
00037 {
00038   Eigen::Vector3d tmp_res =  transform * Eigen::Vector3d(point[0], point[1], point[2]);  
00039 
00040   res[0] = tmp_res[0];
00041   res[1] = tmp_res[1];
00042   res[2] = tmp_res[2];
00043 }
00044 
00045 
00046 
00047 void compress(const double& value, const double& min_value, const double& max_value, uint16_t& res)
00048 {
00049   res = (value - min_value) * 65536.0 / (max_value - min_value) - 0.5;
00050 }
00051 
00052 
00053 void extract(const uint16_t& value, const double& min_value, const double& max_value, double& res)
00054 {
00055   res = ((double)(value) + 0.5) * (max_value - min_value) / 65536.0 + min_value;
00056 }
00057 
00058 
00059 
00060 
00061 
00062 void compressSphericalPoint(const double sph_point[3], const double min_value[3], const double max_value[3], uint16_t res[3])
00063 {
00064   compress(sph_point[0], min_value[0], max_value[0], res[0]);
00065   compress(sph_point[1], min_value[1], max_value[1], res[1]);
00066   compress(log(sph_point[2]), log(min_value[2]), log(max_value[2]), res[2]);
00067 
00068   /*
00069     printf("compress:   dist %f %f,  min %f %f,   max %f %f\n", 
00070     sph_point[2], log(sph_point[2]),
00071     min_value[2], log(min_value[2]),
00072     max_value[2], log(max_value[2]));
00073   */           
00074 }
00075 
00076 void extractSphericalPoint(const uint16_t compressed_point[3], const double min_value[3], const double max_value[3], double res[3])
00077 {
00078   extract(compressed_point[0], min_value[0], max_value[0], res[0]);
00079   extract(compressed_point[1], min_value[1], max_value[1], res[1]);
00080   extract(compressed_point[2], log(min_value[2]), log(max_value[2]), res[2]);
00081   /*
00082     printf("extract:   dist %f %f,  min %f %f,   max %f %f\n", 
00083     res[2], exp(res[2]),
00084     min_value[2], log(min_value[2]),
00085     max_value[2], log(max_value[2]));
00086   */
00087   res[2] = exp(res[2]);
00088 }
00089 
00090 
00091 
00092 
00093 void extractPoint(const megatree_storage::VizPoint& msg_point, 
00094                   const Eigen::Affine3d &viewpoint, double min_cell_size, double max_cell_size, 
00095                   double point[3], double color[3])
00096 {
00097   uint16_t compressed_point[3];
00098   compressed_point[0] = msg_point.angle_vertical;
00099   compressed_point[1] = msg_point.angle_horizontal;
00100   compressed_point[2] = msg_point.distance_log;
00101 
00102   double sph_point[3];
00103   double min_value[3] = {-M_PI, -M_PI, min_cell_size};
00104   double max_value[3] = { M_PI,  M_PI, max_cell_size};
00105   extractSphericalPoint(compressed_point, min_value, max_value, sph_point);
00106 
00107   double camera_point[3];
00108   fromSphericalCoordinates(sph_point, camera_point);
00109 
00110   // convert point from camera frame to absolute frame
00111   transform(camera_point, viewpoint, point);
00112 
00113   // get color
00114   color[0] = msg_point.r;
00115   color[1] = msg_point.g;
00116   color[2] = msg_point.b;
00117 
00118   /*
00119     printf("Compressed point %d %d %d\n", compressed_point[0], compressed_point[1], compressed_point[2]);
00120     printf("Spherical point %f %f %f\n", sph_point[0], sph_point[1], sph_point[2]);
00121     printf("Camera point %f %f %f\n", camera_point[0], camera_point[1], camera_point[2]);
00122     printf("World point %f %f %f\n", point[0], point[1], point[2]);  
00123   */
00124 }
00125 
00126 
00127 }
00128 
00129 #endif


megatree_storage
Author(s): Wim Meeussen
autogenerated on Mon Dec 2 2013 13:01:18