root / rgbdslam / src / misc.cpp @ 9240aaa3
History | View | Annotate | Download (15.1 KB)
1 | 9240aaa3 | Alex | /* This file is part of RGBDSLAM.
|
---|---|---|---|
2 | *
|
||
3 | * RGBDSLAM is free software: you can redistribute it and/or modify
|
||
4 | * it under the terms of the GNU General Public License as published by
|
||
5 | * the Free Software Foundation, either version 3 of the License, or
|
||
6 | * (at your option) any later version.
|
||
7 | *
|
||
8 | * RGBDSLAM is distributed in the hope that it will be useful,
|
||
9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
11 | * GNU General Public License for more details.
|
||
12 | *
|
||
13 | * You should have received a copy of the GNU General Public License
|
||
14 | * along with RGBDSLAM. If not, see <http://www.gnu.org/licenses/>.
|
||
15 | */
|
||
16 | #include <ros/ros.h> |
||
17 | #include <tf/transform_datatypes.h> |
||
18 | #include <Eigen/Core> |
||
19 | #include <QString> |
||
20 | #include <QMatrix4x4> |
||
21 | #include <ctime> |
||
22 | #include <limits> |
||
23 | #include "parameter_server.h" |
||
24 | #include <cv.h> |
||
25 | |||
26 | #include <g2o/math_groups/se3quat.h> |
||
27 | |||
28 | #include <pcl_ros/transforms.h> |
||
29 | |||
30 | void printQMatrix4x4(const char* name, const QMatrix4x4& m){ |
||
31 | ROS_DEBUG("QMatrix %s:", name);
|
||
32 | ROS_DEBUG("%f\t%f\t%f\t%f", m(0,0), m(0,1), m(0,2), m(0,3)); |
||
33 | ROS_DEBUG("%f\t%f\t%f\t%f", m(1,0), m(1,1), m(1,2), m(1,3)); |
||
34 | ROS_DEBUG("%f\t%f\t%f\t%f", m(2,0), m(2,1), m(2,2), m(2,3)); |
||
35 | ROS_DEBUG("%f\t%f\t%f\t%f", m(3,0), m(3,1), m(3,2), m(3,3)); |
||
36 | } |
||
37 | |||
38 | void printTransform(const char* name, const tf::Transform t) { |
||
39 | ROS_INFO_STREAM(name << ": Translation " << t.getOrigin().x() << " " << t.getOrigin().y() << " " << t.getOrigin().z()); |
||
40 | ROS_INFO_STREAM(name << ": Rotation " << t.getRotation().getX() << " " << t.getRotation().getY() << " " << t.getRotation().getZ() << " " << t.getRotation().getW()); |
||
41 | } |
||
42 | |||
43 | void logTransform(QTextStream& out, const tf::Transform& t, double timestamp, const char* label) { |
||
44 | if(label) out << label << ": "; |
||
45 | out << timestamp << " " << t.getOrigin().x() << " " << t.getOrigin().y() << " " << t.getOrigin().z() << " " << t.getRotation().getX() << " " << t.getRotation().getY() << " " << t.getRotation().getZ() << " " << t.getRotation().getW() << "\n"; |
||
46 | } |
||
47 | |||
48 | |||
49 | QMatrix4x4 g2o2QMatrix(const g2o::SE3Quat se3) {
|
||
50 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
51 | Eigen::Matrix<double, 4, 4> m = se3.to_homogenious_matrix(); //_Matrix< 4, 4, double > |
||
52 | ROS_DEBUG_STREAM("Eigen Matrix:\n" << m);
|
||
53 | QMatrix4x4 qmat( static_cast<qreal*>( m.data() ) );
|
||
54 | // g2o/Eigen seems to use a different row-major/column-major array layout
|
||
55 | printQMatrix4x4("from conversion", qmat.transposed());//thus the transposes |
||
56 | return qmat.transposed();//thus the transposes |
||
57 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
58 | } |
||
59 | |||
60 | tf::Transform g2o2TF(const g2o::SE3Quat se3) {
|
||
61 | tf::Transform result; |
||
62 | tf::Vector3 translation; |
||
63 | translation.setX(se3.translation().x()); |
||
64 | translation.setY(se3.translation().y()); |
||
65 | translation.setZ(se3.translation().z()); |
||
66 | |||
67 | tf::Quaternion rotation; |
||
68 | rotation.setX(se3.rotation().x()); |
||
69 | rotation.setY(se3.rotation().y()); |
||
70 | rotation.setZ(se3.rotation().z()); |
||
71 | rotation.setW(se3.rotation().w()); |
||
72 | |||
73 | result.setOrigin(translation); |
||
74 | result.setRotation(rotation); |
||
75 | //printTransform("from conversion", result);
|
||
76 | return result;
|
||
77 | } |
||
78 | //From: /opt/ros/unstable/stacks/perception_pcl/pcl/src/pcl/registration/transforms.hpp
|
||
79 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
80 | /** \brief Apply an affine transform defined by an Eigen Transform
|
||
81 | * \param cloud_in the input point cloud
|
||
82 | * \param cloud_to_append_to the transformed cloud will be appended to this one
|
||
83 | * \param transform a tf::Transform stating the transformation of cloud_to_append_to relative to cloud_in
|
||
84 | * \note The density of the point cloud is lost, since density implies that the origin is the point of view
|
||
85 | * \note Can not(?) be used with cloud_in equal to cloud_to_append_to
|
||
86 | */
|
||
87 | //template <typename PointT> void
|
||
88 | //transformAndAppendPointCloud (const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_to_append_to,
|
||
89 | // const tf::Transform transformation)
|
||
90 | void transformAndAppendPointCloud (const pointcloud_type &cloud_in, |
||
91 | pointcloud_type &cloud_to_append_to, |
||
92 | const tf::Transform transformation, float max_Depth) |
||
93 | { |
||
94 | bool compact = !ParameterServer::instance()->get<bool>("preserve_raster_on_save"); |
||
95 | Eigen::Matrix4f eigen_transform; |
||
96 | pcl_ros::transformAsMatrix(transformation, eigen_transform); |
||
97 | unsigned int cloud_to_append_to_original_size = cloud_to_append_to.size(); |
||
98 | if(cloud_to_append_to.points.size() ==0){ |
||
99 | cloud_to_append_to.header = cloud_in.header; |
||
100 | cloud_to_append_to.width = 0;
|
||
101 | cloud_to_append_to.height = 0;
|
||
102 | cloud_to_append_to.is_dense = false;
|
||
103 | } |
||
104 | |||
105 | ROS_DEBUG("max_Depth = %f", max_Depth);
|
||
106 | ROS_DEBUG("cloud_to_append_to_original_size = %i", cloud_to_append_to_original_size);
|
||
107 | |||
108 | //Append all points untransformed
|
||
109 | cloud_to_append_to += cloud_in; |
||
110 | |||
111 | Eigen::Matrix3f rot = eigen_transform.block<3, 3> (0, 0); |
||
112 | Eigen::Vector3f trans = eigen_transform.block<3, 1> (0, 3); |
||
113 | point_type origin = point_type(); |
||
114 | origin.x = 0;
|
||
115 | origin.y = 0;
|
||
116 | origin.z = 0;
|
||
117 | int j = 0; |
||
118 | for (size_t i = 0; i < cloud_in.points.size (); ++i) |
||
119 | { |
||
120 | Eigen::Map<Eigen::Vector3f> p_in (const_cast<float*>(&cloud_in.points[i].x), 3, 1); |
||
121 | Eigen::Map<Eigen::Vector3f> p_out (&cloud_to_append_to.points[j+cloud_to_append_to_original_size].x, 3, 1); |
||
122 | if(compact){ cloud_to_append_to.points[j+cloud_to_append_to_original_size] = cloud_in.points[i]; }
|
||
123 | //filter out points with a range greater than the given Parameter or do nothing if negativ
|
||
124 | if(max_Depth >= 0){ |
||
125 | if(pcl::squaredEuclideanDistance(cloud_in.points[i], origin) > max_Depth*max_Depth){
|
||
126 | p_out[0]= std::numeric_limits<float>::quiet_NaN(); |
||
127 | p_out[1]= std::numeric_limits<float>::quiet_NaN(); |
||
128 | p_out[2]= std::numeric_limits<float>::quiet_NaN(); |
||
129 | if(!compact) j++;
|
||
130 | continue;
|
||
131 | } |
||
132 | } |
||
133 | if (pcl_isnan (cloud_in.points[i].x) || pcl_isnan (cloud_in.points[i].y) || pcl_isnan (cloud_in.points[i].z)){
|
||
134 | if(!compact) j++;
|
||
135 | continue;
|
||
136 | } |
||
137 | p_out = rot * p_in + trans; |
||
138 | j++; |
||
139 | } |
||
140 | if(compact){
|
||
141 | cloud_to_append_to.points.resize(j+cloud_to_append_to_original_size); |
||
142 | cloud_to_append_to.width = 1;
|
||
143 | cloud_to_append_to.height = j+cloud_to_append_to_original_size; |
||
144 | } |
||
145 | } |
||
146 | |||
147 | //do spurious type conversions
|
||
148 | geometry_msgs::Point pointInWorldFrame(const Eigen::Vector4f& point3d, g2o::SE3Quat transf){
|
||
149 | Eigen::Vector3d tmp(point3d[0], point3d[1], point3d[2]); |
||
150 | tmp = transf * tmp; //transform to world frame
|
||
151 | geometry_msgs::Point p; |
||
152 | p.x = tmp.x(); |
||
153 | p.y = tmp.y(); |
||
154 | p.z = tmp.z(); |
||
155 | return p;
|
||
156 | } |
||
157 | void mat2dist(const Eigen::Matrix4f& t, double &dist){ |
||
158 | dist = sqrt(t(0,3)*t(0,3)+t(1,3)*t(1,3)+t(2,3)*t(2,3)); |
||
159 | } |
||
160 | ///Get euler angles from affine matrix (helper for isBigTrafo)
|
||
161 | void mat2RPY(const Eigen::Matrix4f& t, double& roll, double& pitch, double& yaw) { |
||
162 | roll = atan2(t(2,1),t(2,2)); |
||
163 | pitch = atan2(-t(2,0),sqrt(t(2,1)*t(2,1)+t(2,2)*t(2,2))); |
||
164 | yaw = atan2(t(1,0),t(0,0)); |
||
165 | } |
||
166 | void mat2components(const Eigen::Matrix4f& t, double& roll, double& pitch, double& yaw, double& dist){ |
||
167 | |||
168 | mat2RPY(t, roll,pitch,yaw); |
||
169 | mat2dist(t, dist); |
||
170 | |||
171 | roll = roll/M_PI*180;
|
||
172 | pitch = pitch/M_PI*180;
|
||
173 | yaw = yaw/M_PI*180;
|
||
174 | |||
175 | } |
||
176 | |||
177 | // true iff edge qualifies for generating a new vertex
|
||
178 | bool isBigTrafo(const Eigen::Matrix4f& t){ |
||
179 | double roll, pitch, yaw, dist;
|
||
180 | |||
181 | mat2RPY(t, roll,pitch,yaw); |
||
182 | mat2dist(t, dist); |
||
183 | |||
184 | roll = roll/M_PI*180;
|
||
185 | pitch = pitch/M_PI*180;
|
||
186 | yaw = yaw/M_PI*180;
|
||
187 | |||
188 | double max_angle = std::max(roll,std::max(pitch,yaw));
|
||
189 | |||
190 | // at least 10cm or 5deg
|
||
191 | return (dist > ParameterServer::instance()->get<double>("min_translation_meter") |
||
192 | || max_angle > ParameterServer::instance()->get<int>("min_rotation_degree")); |
||
193 | } |
||
194 | |||
195 | bool isBigTrafo(const g2o::SE3Quat& t){ |
||
196 | float angle_around_axis = 2.0*acos(t.rotation().w()) *180.0 / M_PI; |
||
197 | float dist = t.translation().norm();
|
||
198 | QString infostring; |
||
199 | ROS_INFO("Rotation:% 4.2f, Distance: % 4.3fm", angle_around_axis, dist);
|
||
200 | infostring.sprintf("Rotation:% 4.2f, Distance: % 4.3fm", angle_around_axis, dist);
|
||
201 | //Q_EMIT setGUIInfo2(infostring);
|
||
202 | return (dist > ParameterServer::instance()->get<double>("min_translation_meter") |
||
203 | || angle_around_axis > ParameterServer::instance()->get<int>("min_rotation_degree")); |
||
204 | } |
||
205 | |||
206 | |||
207 | /*
|
||
208 | bool overlappingViews(LoadedEdge3D edge){
|
||
209 | //opening angles
|
||
210 | double alpha = 57.0/180.0*M_PI;
|
||
211 | double beta = 47.0/180.0*M_PI;
|
||
212 | //assumes robot coordinate system (x is front, y is left, z is up)
|
||
213 | Eigen::Matrix<double, 4,3> cam1;
|
||
214 | cam1 << 1.0, std::tan(alpha), std::tan(beta),//upper left
|
||
215 | 1.0, -std::tan(alpha), std::tan(beta),//upper right
|
||
216 | 1.0, std::tan(alpha), -std::tan(beta),//lower left
|
||
217 | 1.0, -std::tan(alpha),-std::tan(beta);//lower right
|
||
218 | return false;
|
||
219 | }
|
||
220 | bool triangleRayIntersection(Eigen::Vector3d triangle1,Eigen::Vector3d triangle2,
|
||
221 | Eigen::Vector3d ray_origin, Eigen::Vector3d ray){
|
||
222 | Eigen::Matrix3d m;
|
||
223 | m.col(2) = -ray;
|
||
224 | m.col(0) = triangle1;
|
||
225 | m.col(1) = triangle2;
|
||
226 | Eigen::Vector3d lengths = m.inverse() * ray_origin;
|
||
227 | return (lengths(0) < 0 && lengths(1) > 0 && lengths(2) > 0 );
|
||
228 | }
|
||
229 | */
|
||
230 | |||
231 | g2o::SE3Quat eigen2G2O(const Eigen::Matrix4d eigen_mat) {
|
||
232 | Eigen::Affine3d eigen_transform(eigen_mat); |
||
233 | Eigen::Quaterniond eigen_quat(eigen_transform.rotation()); |
||
234 | Eigen::Vector3d translation(eigen_mat(0, 3), eigen_mat(1, 3), eigen_mat(2, 3)); |
||
235 | g2o::SE3Quat result(eigen_quat, translation); |
||
236 | |||
237 | return result;
|
||
238 | } |
||
239 | |||
240 | using namespace cv; |
||
241 | ///Analog to opencv example file and modified to use adjusters
|
||
242 | FeatureDetector* createDetector( const string& detectorType ) |
||
243 | { |
||
244 | ParameterServer* params = ParameterServer::instance(); |
||
245 | FeatureDetector* fd = 0;
|
||
246 | if( !detectorType.compare( "FAST" ) ) { |
||
247 | //fd = new FastFeatureDetector( 20/*threshold*/, true/*nonmax_suppression*/ );
|
||
248 | fd = new DynamicAdaptedFeatureDetector (new FastAdjuster(20,true), |
||
249 | params->get<int>("min_keypoints"), |
||
250 | params->get<int>("max_keypoints"), |
||
251 | params->get<int>("fast_max_iterations")); |
||
252 | } |
||
253 | else if( !detectorType.compare( "STAR" ) ) { |
||
254 | fd = new StarFeatureDetector( 16/*max_size*/, 5/*response_threshold*/, 10/*line_threshold_projected*/, |
||
255 | 8/*line_threshold_binarized*/, 5/*suppress_nonmax_size*/ ); |
||
256 | } |
||
257 | else if( !detectorType.compare( "SIFT" ) ) { |
||
258 | fd = new SiftFeatureDetector(SIFT::DetectorParams::GET_DEFAULT_THRESHOLD(),
|
||
259 | SIFT::DetectorParams::GET_DEFAULT_EDGE_THRESHOLD()); |
||
260 | ROS_INFO("Default SIFT threshold: %f, Default SIFT Edge Threshold: %f",
|
||
261 | SIFT::DetectorParams::GET_DEFAULT_THRESHOLD(), |
||
262 | SIFT::DetectorParams::GET_DEFAULT_EDGE_THRESHOLD()); |
||
263 | } |
||
264 | else if( !detectorType.compare( "SURF" ) ) { |
||
265 | fd = new DynamicAdaptedFeatureDetector(new SurfAdjuster(), |
||
266 | params->get<int>("min_keypoints"), |
||
267 | params->get<int>("max_keypoints"), |
||
268 | params->get<int>("surf_max_iterations")); |
||
269 | } |
||
270 | else if( !detectorType.compare( "MSER" ) ) { |
||
271 | fd = new MserFeatureDetector( 1/*delta*/, 60/*min_area*/, 14400/*_max_area*/, 0.35f/*max_variation*/, |
||
272 | 0.2/*min_diversity*/, 200/*max_evolution*/, 1.01/*area_threshold*/, 0.003/*min_margin*/, |
||
273 | 5/*edge_blur_size*/ ); |
||
274 | } |
||
275 | else if( !detectorType.compare( "GFTT" ) ) { |
||
276 | fd = new GoodFeaturesToTrackDetector( 200/*maxCorners*/, 0.001/*qualityLevel*/, 1./*minDistance*/, |
||
277 | 5/*int _blockSize*/, true/*useHarrisDetector*/, 0.04/*k*/ ); |
||
278 | } |
||
279 | #if CV_MAJOR_VERSION > 2 || CV_MINOR_VERSION >= 3 |
||
280 | else if( !detectorType.compare( "ORB" ) ) { |
||
281 | fd = new OrbFeatureDetector(params->get<int>("max_keypoints"), |
||
282 | ORB::CommonParams(1.2, ORB::CommonParams::DEFAULT_N_LEVELS, 31, ORB::CommonParams::DEFAULT_FIRST_LEVEL)); |
||
283 | } |
||
284 | #endif
|
||
285 | else {
|
||
286 | ROS_ERROR("No valid detector-type given: %s. Using SURF.", detectorType.c_str());
|
||
287 | fd = createDetector("SURF"); //recursive call with correct parameter |
||
288 | } |
||
289 | ROS_ERROR_COND(fd == 0, "No detector could be created"); |
||
290 | return fd;
|
||
291 | } |
||
292 | |||
293 | DescriptorExtractor* createDescriptorExtractor( const string& descriptorType ) |
||
294 | { |
||
295 | DescriptorExtractor* extractor = 0;
|
||
296 | if( !descriptorType.compare( "SIFT" ) ) { |
||
297 | extractor = new SiftDescriptorExtractor();/*( double magnification=SIFT::DescriptorParams::GET_DEFAULT_MAGNIFICATION(), bool isNormalize=true, bool recalculateAngles=true, int nOctaves=SIFT::CommonParams::DEFAULT_NOCTAVES, int nOctaveLayers=SIFT::CommonParams::DEFAULT_NOCTAVE_LAYERS, int firstOctave=SIFT::CommonParams::DEFAULT_FIRST_OCTAVE, int angleMode=SIFT::CommonParams::FIRST_ANGLE )*/ |
||
298 | } |
||
299 | else if( !descriptorType.compare( "SURF" ) ) { |
||
300 | extractor = new SurfDescriptorExtractor();/*( int nOctaves=4, int nOctaveLayers=2, bool extended=false )*/ |
||
301 | } |
||
302 | #if CV_MAJOR_VERSION > 2 || CV_MINOR_VERSION >= 3 |
||
303 | else if( !descriptorType.compare( "ORB" ) ) { |
||
304 | extractor = new OrbDescriptorExtractor();
|
||
305 | } |
||
306 | #endif
|
||
307 | else {
|
||
308 | ROS_ERROR("No valid descriptor-matcher-type given: %s. Using SURF", descriptorType.c_str());
|
||
309 | extractor = createDescriptorExtractor("SURF");
|
||
310 | } |
||
311 | return extractor;
|
||
312 | } |
||
313 | |||
314 | void depthToCV8UC1(const cv::Mat& float_img, cv::Mat& mono8_img){ |
||
315 | //Process images
|
||
316 | if(mono8_img.rows != float_img.rows || mono8_img.cols != float_img.cols){
|
||
317 | mono8_img = cv::Mat(float_img.size(), CV_8UC1);} |
||
318 | //The following doesn't work due to NaNs
|
||
319 | //double minVal, maxVal;
|
||
320 | //minMaxLoc(float_img, &minVal, &maxVal);
|
||
321 | //ROS_DEBUG("Minimum/Maximum Depth in current image: %f/%f", minVal, maxVal);
|
||
322 | //mono8_img = cv::Scalar(0);
|
||
323 | //cv::line( mono8_img, cv::Point2i(10,10),cv::Point2i(200,100), cv::Scalar(255), 3, 8);
|
||
324 | cv::convertScaleAbs(float_img, mono8_img, 100, 0.0); |
||
325 | } |
||
326 | |||
327 | //Little debugging helper functions
|
||
328 | std::string openCVCode2String(unsigned int code){ |
||
329 | switch(code){
|
||
330 | case 0 : return std::string("CV_8UC1" ); |
||
331 | case 8 : return std::string("CV_8UC2" ); |
||
332 | case 16: return std::string("CV_8UC3" ); |
||
333 | case 24: return std::string("CV_8UC4" ); |
||
334 | case 2 : return std::string("CV_16UC1"); |
||
335 | case 10: return std::string("CV_16UC2"); |
||
336 | case 18: return std::string("CV_16UC3"); |
||
337 | case 26: return std::string("CV_16UC4"); |
||
338 | case 5 : return std::string("CV_32FC1"); |
||
339 | case 13: return std::string("CV_32FC2"); |
||
340 | case 21: return std::string("CV_32FC3"); |
||
341 | case 29: return std::string("CV_32FC4"); |
||
342 | } |
||
343 | return std::string("Unknown"); |
||
344 | } |
||
345 | |||
346 | void printMatrixInfo(cv::Mat& image){
|
||
347 | ROS_DEBUG_STREAM("Matrix Type:" << openCVCode2String(image.type()) << " rows: " << image.rows << " cols: " << image.cols); |
||
348 | } |