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
00014 res[2] = sqrt(point[0] * point[0] +
00015 point[1] * point[1] +
00016 point[2] * point[2]);
00017
00018
00019 res[0] = asin(point[2]/res[2]);
00020
00021
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
00070
00071
00072
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
00083
00084
00085
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
00111 transform(camera_point, viewpoint, point);
00112
00113
00114 color[0] = msg_point.r;
00115 color[1] = msg_point.g;
00116 color[2] = msg_point.b;
00117
00118
00119
00120
00121
00122
00123
00124 }
00125
00126
00127 }
00128
00129 #endif