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 |
} |