Statistics
| Branch: | Revision:

root / rgbdslam / src / misc.cpp @ 9240aaa3

History | View | Annotate | Download (15.1 KB)

1
/* 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
}