root / rgbdslam / src / node.cpp @ 9240aaa3
History | View | Annotate | Download (39.4 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 |
|
17 |
|
18 |
#include "node.h" |
19 |
#include <cmath> |
20 |
#include <ctime> |
21 |
#include <Eigen/Geometry> |
22 |
#include "pcl/ros/conversions.h" |
23 |
#include <pcl/common/transformation_from_correspondences.h> |
24 |
//#include <opencv2/highgui/highgui.hpp>
|
25 |
#include <qtconcurrentrun.h> |
26 |
#include <QtConcurrentMap> |
27 |
|
28 |
#ifdef USE_SIFT_GPU
|
29 |
#include "sift_gpu_feature_detector.h" |
30 |
#endif
|
31 |
|
32 |
//#include <math.h>
|
33 |
#include <fstream> |
34 |
#ifdef USE_ICP_BIN
|
35 |
#include "gicp-fallback.h" |
36 |
#endif
|
37 |
|
38 |
#ifdef USE_ICP_CODE
|
39 |
#include "../gicp/transform.h" |
40 |
#endif
|
41 |
|
42 |
//#include <iostream>
|
43 |
#include <Eigen/StdVector> |
44 |
#include "misc.h" |
45 |
#include <pcl/filters/voxel_grid.h> |
46 |
|
47 |
Node::Node(const cv::Mat visual,
|
48 |
cv::Ptr<cv::FeatureDetector> detector, |
49 |
cv::Ptr<cv::DescriptorExtractor> extractor, |
50 |
pointcloud_type::Ptr point_cloud, |
51 |
const cv::Mat detection_mask)
|
52 |
: id_(0),
|
53 |
flannIndex(NULL),
|
54 |
base2points_(tf::Transform::getIdentity(), point_cloud->header.stamp,ParameterServer::instance()->get<std::string>("base_frame_name"), point_cloud->header.frame_id), |
55 |
ground_truth_transform_(tf::Transform::getIdentity(), point_cloud->header.stamp, ParameterServer::instance()->get<std::string>("ground_truth_frame_name"), ParameterServer::instance()->get<std::string>("base_frame_name")), |
56 |
initial_node_matches_(0)
|
57 |
{ |
58 |
#ifdef USE_ICP_CODE
|
59 |
gicp_initialized = false;
|
60 |
#endif
|
61 |
struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
62 |
|
63 |
#ifdef USE_SIFT_GPU
|
64 |
float* descriptors = NULL; |
65 |
if(ParameterServer::instance()->get<std::string>("feature_detector_type") == "SIFTGPU"){ |
66 |
SiftGPUFeatureDetector* siftgpu = SiftGPUFeatureDetector::GetInstance(); |
67 |
descriptors = siftgpu->detect(visual, feature_locations_2d_); |
68 |
if (descriptors == NULL) { |
69 |
ROS_FATAL("Can't run SiftGPU");
|
70 |
} |
71 |
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", "Feature Detection and Descriptor Extraction runtime: "<< elapsed <<" s"); |
72 |
} else {
|
73 |
#endif
|
74 |
ROS_FATAL_COND(detector.empty(), "No valid detector!");
|
75 |
detector->detect( visual, feature_locations_2d_, detection_mask);// fill 2d locations
|
76 |
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", "Feature Detection runtime: "<< elapsed <<" s"); |
77 |
#ifdef USE_SIFT_GPU
|
78 |
} |
79 |
#endif
|
80 |
|
81 |
|
82 |
// get pcl::Pointcloud to extract depthValues a pixel positions
|
83 |
struct timespec starttime5; clock_gettime(CLOCK_MONOTONIC, &starttime5);
|
84 |
// TODO: If batch sending/saving of clouds would be removed, the pointcloud wouldn't have to be saved
|
85 |
// which would slim down the Memory requirements
|
86 |
//pcl::fromROSMsg(*point_cloud,(*pc_col));
|
87 |
pc_col = point_cloud; |
88 |
clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime5.tv_sec); elapsed += (finish.tv_nsec - starttime5.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
89 |
|
90 |
// project pixels to 3dPositions and create search structures for the gicp
|
91 |
#ifdef USE_SIFT_GPU
|
92 |
if(ParameterServer::instance()->get<std::string>("feature_detector_type") == "SIFTGPU"){ |
93 |
// removes also unused descriptors from the descriptors matrix
|
94 |
// build descriptor matrix
|
95 |
projectTo3DSiftGPU(feature_locations_2d_, feature_locations_3d_, pc_col, descriptors, feature_descriptors_); //takes less than 0.01 sec
|
96 |
|
97 |
if (descriptors != NULL) delete descriptors; |
98 |
}else{
|
99 |
#endif
|
100 |
projectTo3D(feature_locations_2d_, feature_locations_3d_, pc_col); //takes less than 0.01 sec
|
101 |
// projectTo3d need a dense cloud to use the points.at(px.x,px.y)-Call
|
102 |
struct timespec starttime2; clock_gettime(CLOCK_MONOTONIC, &starttime2);
|
103 |
extractor->compute(visual, feature_locations_2d_, feature_descriptors_); //fill feature_descriptors_ with information
|
104 |
clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime2.tv_sec); elapsed += (finish.tv_nsec - starttime2.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", "Feature Extraction runtime: "<< elapsed <<" s"); |
105 |
#ifdef USE_SIFT_GPU
|
106 |
} |
107 |
#endif
|
108 |
assert(feature_locations_2d_.size() == feature_locations_3d_.size()); |
109 |
ROS_INFO_NAMED("statistics", "Feature Count of Node:\t%d", (int)feature_locations_2d_.size()); |
110 |
size_t max_keyp = ParameterServer::instance()->get<int>("max_keypoints"); |
111 |
if(feature_locations_2d_.size() > max_keyp) {
|
112 |
feature_locations_2d_.resize(max_keyp); |
113 |
feature_locations_3d_.resize(max_keyp); |
114 |
feature_descriptors_ = feature_descriptors_.rowRange(0,max_keyp-1); |
115 |
} |
116 |
|
117 |
#ifdef USE_ICP_CODE
|
118 |
struct timespec starttime4; clock_gettime(CLOCK_MONOTONIC, &starttime4);
|
119 |
createGICPStructures(); |
120 |
clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime4.tv_sec); elapsed += (finish.tv_nsec - starttime4.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
121 |
#endif
|
122 |
|
123 |
if((!ParameterServer::instance()->get<bool>("use_glwidget") || |
124 |
!ParameterServer::instance()->get<bool>("use_gui")) && |
125 |
!ParameterServer::instance()->get<bool>("store_pointclouds")) |
126 |
{ |
127 |
ROS_WARN("Clearing out points");
|
128 |
this->clearPointCloud();
|
129 |
} else if(ParameterServer::instance()->get<double>("voxelfilter_size") > 0.0) { |
130 |
double vfs = ParameterServer::instance()->get<double>("voxelfilter_size"); |
131 |
pcl::VoxelGrid<point_type> sor; |
132 |
sor.setLeafSize(vfs,vfs,vfs); |
133 |
pointcloud_type::ConstPtr const_cloud_ptr = boost::make_shared<pointcloud_type> (*pc_col); |
134 |
sor.setInputCloud (const_cloud_ptr); |
135 |
sor.filter (*pc_col); |
136 |
} |
137 |
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"); |
138 |
|
139 |
} |
140 |
|
141 |
Node::~Node() { |
142 |
if (ParameterServer::instance()->get<std::string> ("feature_detector_type").compare("ORB") != 0) { |
143 |
if (flannIndex)
|
144 |
delete flannIndex;
|
145 |
} |
146 |
} |
147 |
|
148 |
void Node::setGroundTruthTransform(tf::StampedTransform gt){
|
149 |
ground_truth_transform_ = gt; |
150 |
} |
151 |
void Node::setBase2PointsTransform(tf::StampedTransform b2p){
|
152 |
base2points_ = b2p; |
153 |
} |
154 |
tf::StampedTransform Node::getGroundTruthTransform(){ |
155 |
return ground_truth_transform_;
|
156 |
} |
157 |
tf::StampedTransform Node::getBase2PointsTransform(){ |
158 |
return base2points_;
|
159 |
} |
160 |
|
161 |
void Node::publish(const char* frame, ros::Time timestamp, ros::Publisher pub){ |
162 |
sensor_msgs::PointCloud2 cloudMessage; |
163 |
pcl::toROSMsg((*pc_col),cloudMessage); |
164 |
cloudMessage.header.frame_id = frame; |
165 |
cloudMessage.header.stamp = timestamp; |
166 |
pub.publish(cloudMessage); |
167 |
ROS_INFO("Pointcloud with id %i sent with frame %s", id_, frame);
|
168 |
} |
169 |
|
170 |
#ifdef USE_ICP_CODE
|
171 |
bool Node::getRelativeTransformationTo_ICP_code(const Node* target_node,Eigen::Matrix4f& transformation, |
172 |
const Eigen::Matrix4f* initial_transformation){
|
173 |
//std::clock_t starttime_icp = std::clock();
|
174 |
dgc_transform_t initial; |
175 |
|
176 |
// use optional matrix as first guess in icp
|
177 |
if (initial_transformation == NULL){ |
178 |
gicpSetIdentity(initial); |
179 |
}else {
|
180 |
Eigen2GICP(*initial_transformation,initial); |
181 |
} |
182 |
|
183 |
|
184 |
dgc_transform_t final_trafo; |
185 |
dgc_transform_identity(final_trafo); |
186 |
|
187 |
|
188 |
assert(gicp_initialized && target_node->gicp_initialized); |
189 |
|
190 |
unsigned int iterations = target_node->gicp_point_set->AlignScan(this->gicp_point_set, initial, final_trafo, gicp_d_max); |
191 |
|
192 |
|
193 |
GICP2Eigen(final_trafo,transformation); |
194 |
|
195 |
//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");
|
196 |
|
197 |
return iterations < gicp_max_iterations;
|
198 |
|
199 |
} |
200 |
|
201 |
# endif
|
202 |
|
203 |
#ifdef USE_ICP_BIN
|
204 |
bool Node::getRelativeTransformationTo_ICP_bin(const Node* target_node, |
205 |
Eigen::Matrix4f& transformation, |
206 |
const Eigen::Matrix4f* initial_transformation){
|
207 |
std::clock_t starttime_icp = std::clock(); |
208 |
|
209 |
bool converged;
|
210 |
|
211 |
if (initial_transformation != NULL) |
212 |
{ |
213 |
pointcloud_type pc2; |
214 |
pcl::transformPointCloud((*pc_col),pc2,*initial_transformation); |
215 |
converged = gicpfallback(pc2,target_node->(*pc_col), transformation); |
216 |
} |
217 |
else {
|
218 |
converged = gicpfallback((*pc_col),target_node->(*pc_col), transformation); } |
219 |
|
220 |
// Paper
|
221 |
// 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");
|
222 |
|
223 |
return converged;
|
224 |
} |
225 |
#endif
|
226 |
|
227 |
// build search structure for descriptor matching
|
228 |
void Node::buildFlannIndex() {
|
229 |
if (ParameterServer::instance()->get<bool> ("use_flann") && |
230 |
ParameterServer::instance()->get<std::string> ("feature_detector_type").compare("ORB") != 0) |
231 |
{ |
232 |
struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
233 |
//KDTreeIndexParams When passing an object of this type the index constructed will
|
234 |
//consist of a set of randomized kd-trees which will be searched in parallel.
|
235 |
flannIndex = new cv::flann::Index(feature_descriptors_, cv::flann::KDTreeIndexParams(16)); |
236 |
ROS_DEBUG("Built flannIndex (address %p) for Node %i", flannIndex, this->id_); |
237 |
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"); |
238 |
} |
239 |
} |
240 |
|
241 |
|
242 |
#ifdef USE_ICP_CODE
|
243 |
void Node::createGICPStructures(unsigned int max_count){ |
244 |
|
245 |
gicp_point_set = new dgc::gicp::GICPPointSet();
|
246 |
|
247 |
dgc::gicp::GICPPoint g_p; |
248 |
g_p.range = -1;
|
249 |
for(int k = 0; k < 3; k++) { |
250 |
for(int l = 0; l < 3; l++) { |
251 |
g_p.C[k][l] = (k == l)?1:0; |
252 |
} |
253 |
} |
254 |
|
255 |
int step = 1; |
256 |
if ((*pc_col).points.size()>max_count)
|
257 |
step = ceil((*pc_col).points.size()*1.0/max_count); |
258 |
|
259 |
int cnt = 0; |
260 |
for (unsigned int i=0; i<(*pc_col).points.size(); i++ ){ |
261 |
point_type p = (*pc_col).points.at(i); |
262 |
if (!(isnan(p.x) || isnan(p.y) || isnan(p.z))) {
|
263 |
// add points to pointset for icp
|
264 |
if (cnt++%step == 0){ |
265 |
g_p.x=p.x; |
266 |
g_p.y=p.y; |
267 |
g_p.z=p.z; |
268 |
gicp_point_set->AppendPoint(g_p); |
269 |
} |
270 |
} |
271 |
} |
272 |
ROS_WARN("gicp_point_set.Size() %i", gicp_point_set->Size() );
|
273 |
|
274 |
|
275 |
std::clock_t starttime_gicp = std::clock(); |
276 |
// build search structure for gicp:
|
277 |
gicp_point_set->SetDebug(false);
|
278 |
gicp_point_set->SetGICPEpsilon(gicp_epsilon); |
279 |
gicp_point_set->BuildKDTree(); |
280 |
gicp_point_set->ComputeMatrices(); |
281 |
gicp_point_set->SetMaxIterationInner(8); // as in test_gicp->cpp |
282 |
gicp_point_set->SetMaxIteration(gicp_max_iterations); |
283 |
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"); |
284 |
|
285 |
ROS_INFO_STREAM("time for creating the structure: " << ((std::clock()-starttime_gicp*1.0) / (double)CLOCKS_PER_SEC)); |
286 |
ROS_INFO_STREAM("current: " << std::clock() << " " << "start_time: " << starttime_gicp); |
287 |
|
288 |
gicp_initialized = true;
|
289 |
|
290 |
} |
291 |
#endif
|
292 |
|
293 |
//TODO: This function seems to be resistant to paralellization probably due to knnSearch
|
294 |
int Node::findPairsFlann(const Node* other, std::vector<cv::DMatch>* matches) const { |
295 |
struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
296 |
assert(matches->size()==0);
|
297 |
// number of neighbours found (two, to compare the best matches for distinctness
|
298 |
const int k = 2; |
299 |
|
300 |
// number of neighbors found (has to be two, see l. 57)
|
301 |
double sum_distances = 0.0; |
302 |
ParameterServer* ps = ParameterServer::instance(); |
303 |
const int min_kp = ps->get<int> ("min_keypoints"); |
304 |
//using BruteForceMatcher for ORB features
|
305 |
if (!ps->get<bool> ("use_flann") || |
306 |
ps->get<std::string> ("feature_detector_type") == "ORB") |
307 |
{ |
308 |
|
309 |
cv::Ptr<cv::DescriptorMatcher> matcher; |
310 |
std::string brute_force_type("BruteForce"); //L2 per default |
311 |
if(ps->get<std::string> ("feature_detector_type") == "ORB"){ |
312 |
brute_force_type.append("-HammingLUT");
|
313 |
} |
314 |
matcher = cv::DescriptorMatcher::create(brute_force_type); |
315 |
std::vector< std::vector<cv::DMatch> > bruteForceMatches; |
316 |
matcher->knnMatch(feature_descriptors_, other->feature_descriptors_, bruteForceMatches, k); |
317 |
int dist_ratio_fac = 0.6; |
318 |
if ((int)bruteForceMatches.size() < min_kp) dist_ratio_fac = 1.0; //if necessary use possibly bad descriptors |
319 |
for (unsigned int i = 0; i < bruteForceMatches.size(); i++) { |
320 |
cv::DMatch m1 = bruteForceMatches[i][0];
|
321 |
cv::DMatch m2 = bruteForceMatches[i][1];
|
322 |
if (m1.distance < 0.6 * m2.distance) {//this check seems crucial to matching quality |
323 |
matches->push_back(m1); |
324 |
sum_distances += m1.distance; |
325 |
} |
326 |
} |
327 |
//matcher->match(feature_descriptors_, other->feature_descriptors_, *matches);
|
328 |
} |
329 |
else {
|
330 |
if (other->flannIndex == NULL) { |
331 |
ROS_FATAL("Node %i in findPairsFlann: flann Index of Node %i was not initialized", this->id_, other->id_); |
332 |
return -1; |
333 |
} |
334 |
// compare
|
335 |
// http://opencv-cocoa.googlecode.com/svn/trunk/samples/c/find_obj.cpp
|
336 |
cv::Mat indices(feature_descriptors_.rows, k, CV_32S); |
337 |
cv::Mat dists(feature_descriptors_.rows, k, CV_32F); |
338 |
|
339 |
// get the best two neighbors
|
340 |
other->flannIndex->knnSearch(feature_descriptors_, indices, dists, k, cv::flann::SearchParams(64));
|
341 |
//64: The number of times the tree(s) in the index should be recursively traversed. A higher value for this parameter would give better search precision, but also take more time. If automatic configuration was used when the index was created, the number of checks required to achieve the specified precision was also computed, in which case this parameter is ignored.
|
342 |
|
343 |
int* indices_ptr = indices.ptr<int> (0); |
344 |
float* dists_ptr = dists.ptr<float> (0); |
345 |
|
346 |
cv::DMatch match; |
347 |
int dist_ratio_fac = 0.6; |
348 |
if (indices.rows < min_kp) dist_ratio_fac = 1.0; //if necessary use possibly bad descriptors |
349 |
for (int i = 0; i < indices.rows; ++i) { |
350 |
if (dists_ptr[2 * i] < 0.6 * dists_ptr[2 * i + 1]) { |
351 |
match.queryIdx = i; |
352 |
match.trainIdx = indices_ptr[2 * i];
|
353 |
match.distance = dists_ptr[2 * i];
|
354 |
sum_distances += match.distance; |
355 |
|
356 |
assert(match.trainIdx < other->feature_descriptors_.rows); |
357 |
assert(match.queryIdx < feature_descriptors_.rows); |
358 |
|
359 |
matches->push_back(match); |
360 |
} |
361 |
} |
362 |
} |
363 |
|
364 |
ROS_INFO_NAMED("statistics", "count_matrix(%3d, %3d) = %4d;", |
365 |
this->id_+1, other->id_+1, (int)matches->size()); |
366 |
ROS_INFO_NAMED("statistics", "dista_matrix(%3d, %3d) = %f;", |
367 |
this->id_+1, other->id_+1, sum_distances/ (float)matches->size()); |
368 |
ROS_DEBUG_NAMED("statistics", "Feature Matches between Nodes %3d (%4d features) and %3d (%4d features):\t%4d", |
369 |
this->id_, (int)this->feature_locations_2d_.size(), |
370 |
other->id_, (int)other->feature_locations_2d_.size(),
|
371 |
(int)matches->size());
|
372 |
|
373 |
//ROS_INFO("matches size: %i, rows: %i", (int) matches->size(), feature_descriptors_.rows);
|
374 |
|
375 |
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"); |
376 |
return matches->size();
|
377 |
} |
378 |
|
379 |
|
380 |
|
381 |
#ifdef USE_SIFT_GPU
|
382 |
void Node::projectTo3DSiftGPU(std::vector<cv::KeyPoint>& feature_locations_2d,
|
383 |
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d, |
384 |
const pointcloud_type::Ptr point_cloud, float* descriptors_in, cv::Mat& descriptors_out){ |
385 |
|
386 |
struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
387 |
|
388 |
cv::Point2f p2d; |
389 |
|
390 |
if(feature_locations_3d.size()){
|
391 |
ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
|
392 |
feature_locations_3d.clear(); |
393 |
} |
394 |
|
395 |
std::list<int> featuresUsed;
|
396 |
|
397 |
int index = -1; |
398 |
for(unsigned int i = 0; i < feature_locations_2d.size(); /*increment at end of loop*/){ |
399 |
++index; |
400 |
|
401 |
p2d = feature_locations_2d[i].pt; |
402 |
if (p2d.x >= point_cloud->width || p2d.x < 0 || |
403 |
p2d.y >= point_cloud->height || p2d.y < 0 ||
|
404 |
std::isnan(p2d.x) || std::isnan(p2d.y)){ //TODO: Unclear why points should be outside the image or be NaN
|
405 |
ROS_WARN_STREAM("Ignoring invalid keypoint: " << p2d); //Does it happen at all? If not, remove this code block |
406 |
feature_locations_2d.erase(feature_locations_2d.begin()+i); |
407 |
continue;
|
408 |
} |
409 |
|
410 |
point_type p3d = point_cloud->at((int) p2d.x,(int) p2d.y); |
411 |
|
412 |
if ( isnan(p3d.x) || isnan(p3d.y) || isnan(p3d.z)){
|
413 |
feature_locations_2d.erase(feature_locations_2d.begin()+i); |
414 |
continue;
|
415 |
} |
416 |
|
417 |
featuresUsed.push_back(index); //save id for constructing the descriptor matrix
|
418 |
feature_locations_3d.push_back(Eigen::Vector4f(p3d.x, p3d.y, p3d.z, 1.0)); |
419 |
i++; //Only increment if no element is removed from vector
|
420 |
} |
421 |
|
422 |
//create descriptor matrix
|
423 |
int size = feature_locations_3d.size();
|
424 |
descriptors_out = cv::Mat(size, 128, CV_32F);
|
425 |
for (int y = 0; y < size && featuresUsed.size() > 0; ++y) { |
426 |
int id = featuresUsed.front();
|
427 |
featuresUsed.pop_front(); |
428 |
|
429 |
for (int x = 0; x < 128; ++x) { |
430 |
descriptors_out.at<float>(y, x) = descriptors_in[id * 128 + x]; |
431 |
} |
432 |
} |
433 |
|
434 |
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"); |
435 |
} |
436 |
#endif
|
437 |
|
438 |
void Node::projectTo3D(std::vector<cv::KeyPoint>& feature_locations_2d,
|
439 |
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d, |
440 |
pointcloud_type::ConstPtr point_cloud){ |
441 |
|
442 |
struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
443 |
|
444 |
cv::Point2f p2d; |
445 |
|
446 |
if(feature_locations_3d.size()){
|
447 |
ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
|
448 |
feature_locations_3d.clear(); |
449 |
} |
450 |
|
451 |
for(unsigned int i = 0; i < feature_locations_2d.size(); /*increment at end of loop*/){ |
452 |
p2d = feature_locations_2d[i].pt; |
453 |
if (p2d.x >= point_cloud->width || p2d.x < 0 || |
454 |
p2d.y >= point_cloud->height || p2d.y < 0 ||
|
455 |
std::isnan(p2d.x) || std::isnan(p2d.y)){ //TODO: Unclear why points should be outside the image or be NaN
|
456 |
ROS_WARN_STREAM("Ignoring invalid keypoint: " << p2d); //Does it happen at all? If not, remove this code block |
457 |
feature_locations_2d.erase(feature_locations_2d.begin()+i); |
458 |
continue;
|
459 |
} |
460 |
|
461 |
point_type p3d = point_cloud->at((int) p2d.x,(int) p2d.y); |
462 |
|
463 |
//ROS_INFO("3d: %f, %f, %f, 2d: %f, %f", p3d.x, p3d.y, p3d.z, p2d.x, p2d.y);
|
464 |
|
465 |
if ( isnan(p3d.x) || isnan(p3d.y) || isnan(p3d.z)){
|
466 |
feature_locations_2d.erase(feature_locations_2d.begin()+i); |
467 |
continue;
|
468 |
} |
469 |
|
470 |
feature_locations_3d.push_back(Eigen::Vector4f(p3d.x, p3d.y, p3d.z, 1.0)); |
471 |
i++; //Only increment if no element is removed from vector
|
472 |
} |
473 |
|
474 |
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"); |
475 |
} |
476 |
|
477 |
|
478 |
|
479 |
void Node::computeInliersAndError(const std::vector<cv::DMatch>& matches, |
480 |
const Eigen::Matrix4f& transformation,
|
481 |
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& origins,
|
482 |
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& earlier,
|
483 |
std::vector<cv::DMatch>& inliers, //output var
|
484 |
double& mean_error,
|
485 |
std::vector<double>& errors,
|
486 |
double squaredMaxInlierDistInM) const{ //output var |
487 |
|
488 |
struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
489 |
|
490 |
inliers.clear(); |
491 |
errors.clear(); |
492 |
|
493 |
std::vector<std::pair<float,int> > dists; |
494 |
std::vector<cv::DMatch> inliers_temp; |
495 |
|
496 |
assert(matches.size() > 0);
|
497 |
mean_error = 0.0; |
498 |
for (unsigned int j = 0; j < matches.size(); j++){ //compute new error and inliers |
499 |
|
500 |
unsigned int this_id = matches[j].queryIdx; |
501 |
unsigned int earlier_id = matches[j].trainIdx; |
502 |
|
503 |
Eigen::Vector4f vec = (transformation * origins[this_id]) - earlier[earlier_id]; |
504 |
|
505 |
double error = vec.dot(vec);
|
506 |
|
507 |
if(error > squaredMaxInlierDistInM)
|
508 |
continue; //ignore outliers |
509 |
if(!(error >= 0.0)){ |
510 |
ROS_DEBUG_STREAM("Transformation for error !> 0: " << transformation);
|
511 |
ROS_DEBUG_STREAM(error << " " << matches.size());
|
512 |
} |
513 |
error = sqrt(error); |
514 |
dists.push_back(std::pair<float,int>(error,j)); |
515 |
inliers_temp.push_back(matches[j]); //include inlier
|
516 |
|
517 |
mean_error += error; |
518 |
errors.push_back(error); |
519 |
} |
520 |
|
521 |
if (inliers_temp.size()<3){ //at least the samples should be inliers |
522 |
ROS_WARN_COND(inliers_temp.size() > 3, "No inliers at all in %d matches!", (int)matches.size()); // only warn if this checks for all initial matches |
523 |
mean_error = 1e9;
|
524 |
} else {
|
525 |
mean_error /= inliers_temp.size(); |
526 |
|
527 |
// sort inlier ascending according to their error
|
528 |
sort(dists.begin(),dists.end()); |
529 |
|
530 |
inliers.resize(inliers_temp.size()); |
531 |
for (unsigned int i=0; i<inliers_temp.size(); i++){ |
532 |
inliers[i] = matches[dists[i].second]; |
533 |
} |
534 |
} |
535 |
if(!(mean_error>0)) ROS_DEBUG_STREAM("Transformation for mean error !> 0: " << transformation); |
536 |
if(!(mean_error>0)) ROS_DEBUG_STREAM(mean_error << " " << inliers_temp.size()); |
537 |
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"); |
538 |
|
539 |
} |
540 |
|
541 |
template<class InputIterator> |
542 |
Eigen::Matrix4f Node::getTransformFromMatches(const Node* earlier_node,
|
543 |
InputIterator iter_begin, |
544 |
InputIterator iter_end, |
545 |
bool& valid,
|
546 |
float max_dist_m) const |
547 |
{ |
548 |
pcl::TransformationFromCorrespondences tfc; |
549 |
valid = true;
|
550 |
std::vector<Eigen::Vector3f> t, f; |
551 |
|
552 |
for ( ;iter_begin!=iter_end; iter_begin++) {
|
553 |
int this_id = iter_begin->queryIdx;
|
554 |
int earlier_id = iter_begin->trainIdx;
|
555 |
|
556 |
Eigen::Vector3f from(this->feature_locations_3d_[this_id][0], |
557 |
this->feature_locations_3d_[this_id][1], |
558 |
this->feature_locations_3d_[this_id][2]); |
559 |
Eigen::Vector3f to (earlier_node->feature_locations_3d_[earlier_id][0],
|
560 |
earlier_node->feature_locations_3d_[earlier_id][1],
|
561 |
earlier_node->feature_locations_3d_[earlier_id][2]);
|
562 |
if (max_dist_m > 0) { //storing is only necessary, if max_dist is given |
563 |
f.push_back(from); |
564 |
t.push_back(to); |
565 |
} |
566 |
tfc.add(from, to, 1.0/to(2));//the further, the less weight b/c of accuracy decay |
567 |
} |
568 |
|
569 |
|
570 |
// find smalles distance between a point and its neighbour in the same cloud
|
571 |
// je groesser das dreieck aufgespannt ist, desto weniger fallen kleine positionsfehler der einzelnen
|
572 |
// Punkte ist Gewicht!
|
573 |
|
574 |
if (max_dist_m > 0) |
575 |
{ |
576 |
//float min_neighbour_dist = 1e6;
|
577 |
Eigen::Matrix4f foo; |
578 |
|
579 |
valid = true;
|
580 |
for (uint i=0; i<f.size(); i++) |
581 |
{ |
582 |
float d_f = (f.at((i+1)%f.size())-f.at(i)).norm(); |
583 |
float d_t = (t.at((i+1)%t.size())-t.at(i)).norm(); |
584 |
|
585 |
if ( abs(d_f-d_t) > max_dist_m ) {
|
586 |
valid = false;
|
587 |
return Eigen::Matrix4f();
|
588 |
} |
589 |
} |
590 |
//here one could signal that some samples are very close, but as the transformation is validated elsewhere we don't
|
591 |
//if (min_neighbour_dist < 0.5) { ROS_INFO...}
|
592 |
} |
593 |
// get relative movement from samples
|
594 |
return tfc.getTransformation().matrix();
|
595 |
} |
596 |
|
597 |
|
598 |
///Find transformation with largest support, RANSAC style.
|
599 |
///Return false if no transformation can be found
|
600 |
bool Node::getRelativeTransformationTo(const Node* earlier_node, |
601 |
std::vector<cv::DMatch>* initial_matches, |
602 |
Eigen::Matrix4f& resulting_transformation, |
603 |
float& rmse,
|
604 |
std::vector<cv::DMatch>& matches) const
|
605 |
{ |
606 |
struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
607 |
|
608 |
assert(initial_matches != NULL);
|
609 |
matches.clear(); |
610 |
|
611 |
if(initial_matches->size() <= (unsigned int) ParameterServer::instance()->get<int>("min_matches")){ |
612 |
ROS_INFO("Only %d feature matches between %d and %d (minimal: %i)",(int)initial_matches->size() , this->id_, earlier_node->id_, ParameterServer::instance()->get<int>("min_matches")); |
613 |
return false; |
614 |
} |
615 |
|
616 |
//unsigned int min_inlier_threshold = int(initial_matches->size()*0.2);
|
617 |
unsigned int min_inlier_threshold = (unsigned int) ParameterServer::instance()->get<int>("min_matches"); |
618 |
std::vector<cv::DMatch> inlier; //holds those feature correspondences that support the transformation
|
619 |
double inlier_error; //all squared errors |
620 |
srand((long)std::clock());
|
621 |
|
622 |
// a point is an inlier if it's no more than max_dist_m m from its partner apart
|
623 |
const float max_dist_m = ParameterServer::instance()->get<double>("max_dist_for_inliers"); |
624 |
const int ransac_iterations = ParameterServer::instance()->get<int>("ransac_iterations"); |
625 |
std::vector<double> dummy;
|
626 |
|
627 |
// best values of all iterations (including invalids)
|
628 |
double best_error = 1e6, best_error_invalid = 1e6; |
629 |
unsigned int best_inlier_invalid = 0, best_inlier_cnt = 0, valid_iterations = 0; |
630 |
|
631 |
Eigen::Matrix4f transformation; |
632 |
|
633 |
const unsigned int sample_size = 3;// chose this many randomly from the correspondences: |
634 |
for (int n_iter = 0; n_iter < ransac_iterations; n_iter++) { |
635 |
//generate a map of samples. Using a map solves the problem of drawing a sample more than once
|
636 |
std::set<cv::DMatch> sample_matches; |
637 |
std::vector<cv::DMatch> sample_matches_vector; |
638 |
while(sample_matches.size() < sample_size){
|
639 |
int id = rand() % initial_matches->size();
|
640 |
sample_matches.insert(initial_matches->at(id)); |
641 |
sample_matches_vector.push_back(initial_matches->at(id)); |
642 |
} |
643 |
|
644 |
bool valid; // valid is false iff the sampled points clearly aren't inliers themself |
645 |
transformation = getTransformFromMatches(earlier_node, sample_matches.begin(), sample_matches.end(),valid,max_dist_m); |
646 |
if (!valid) continue; // valid is false iff the sampled points aren't inliers themself |
647 |
if(transformation!=transformation) continue; //Contains NaN |
648 |
|
649 |
//test whether samples are inliers (more strict than before)
|
650 |
computeInliersAndError(sample_matches_vector, transformation, this->feature_locations_3d_,
|
651 |
earlier_node->feature_locations_3d_, inlier, inlier_error, /*output*/
|
652 |
dummy, max_dist_m*max_dist_m); |
653 |
ROS_DEBUG("Transformation from and for %u samples results in an error of %f and %i inliers.", sample_size, inlier_error, (int)inlier.size()); |
654 |
if(inlier_error > 1000) continue; //most possibly a false match in the samples |
655 |
computeInliersAndError(*initial_matches, transformation, this->feature_locations_3d_,
|
656 |
earlier_node->feature_locations_3d_, inlier, inlier_error, /*output*/
|
657 |
dummy, max_dist_m*max_dist_m); |
658 |
ROS_DEBUG("Transformation from %u samples results in an error of %f and %i inliers for all matches (%i).", sample_size, inlier_error, (int)inlier.size(), (int)initial_matches->size()); |
659 |
|
660 |
// check also invalid iterations
|
661 |
if (inlier.size() > best_inlier_invalid) {
|
662 |
best_inlier_invalid = inlier.size(); |
663 |
best_error_invalid = inlier_error; |
664 |
} |
665 |
// ROS_INFO("iteration %d cnt: %d, best: %d, error: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100);
|
666 |
|
667 |
if(inlier.size() < min_inlier_threshold || inlier_error > max_dist_m){
|
668 |
ROS_DEBUG("Skipped iteration: inliers: %i (min %i), inlier_error: %.2f (max %.2f)", (int)inlier.size(), (int) min_inlier_threshold, inlier_error*100, max_dist_m*100); |
669 |
continue;
|
670 |
} |
671 |
// ROS_INFO("Refining iteration from %i samples: all matches: %i, inliers: %i, inlier_error: %f", (int)sample_size, (int)initial_matches->size(), (int)inlier.size(), inlier_error);
|
672 |
valid_iterations++; |
673 |
//if (inlier_error > 0) ROS_ERROR("size: %i", (int)dummy.size());
|
674 |
assert(inlier_error>=0);
|
675 |
|
676 |
//Performance hacks:
|
677 |
///Iterations with more than half of the initial_matches inlying, count twice
|
678 |
if (inlier.size() > initial_matches->size()*0.5) n_iter+=10; |
679 |
///Iterations with more than 80% of the initial_matches inlying, count threefold
|
680 |
if (inlier.size() > initial_matches->size()*0.8) n_iter+=20; |
681 |
|
682 |
|
683 |
|
684 |
if (inlier_error < best_error) { //copy this to the result |
685 |
resulting_transformation = transformation; |
686 |
matches = inlier; |
687 |
assert(matches.size()>= min_inlier_threshold); |
688 |
best_inlier_cnt = inlier.size(); |
689 |
//assert(matches.size()>= ((float)initial_matches->size())*min_inlier_ratio);
|
690 |
rmse = inlier_error; |
691 |
best_error = inlier_error; |
692 |
// ROS_INFO(" new best iteration %d cnt: %d, best_inlier: %d, error: %.4f, bestError: %.4f",n_iter, inlier.size(), best_inlier_cnt, inlier_error, best_error);
|
693 |
|
694 |
}else
|
695 |
{ |
696 |
// ROS_INFO("NO new best iteration %d cnt: %d, best_inlier: %d, error: %.4f, bestError: %.4f",n_iter, inlier.size(), best_inlier_cnt, inlier_error, best_error);
|
697 |
} |
698 |
|
699 |
//int max_ndx = min((int) min_inlier_threshold,30); //? What is this 30?
|
700 |
double new_inlier_error;
|
701 |
|
702 |
transformation = getTransformFromMatches(earlier_node, matches.begin(), matches.end(), valid); // compute new trafo from all inliers:
|
703 |
if(transformation!=transformation) continue; //Contains NaN |
704 |
computeInliersAndError(*initial_matches, transformation, |
705 |
this->feature_locations_3d_, earlier_node->feature_locations_3d_,
|
706 |
inlier, new_inlier_error, dummy, max_dist_m*max_dist_m); |
707 |
ROS_DEBUG("Refined Transformation from all matches (%i) results in an error of %f and %i inliers for all matches.", (int)initial_matches->size(), inlier_error, (int)inlier.size()); |
708 |
// ROS_INFO("asd recomputed: inliersize: %i, inlier error: %f", (int) inlier.size(),100*new_inlier_error);
|
709 |
|
710 |
|
711 |
// check also invalid iterations
|
712 |
if (inlier.size() > best_inlier_invalid)
|
713 |
{ |
714 |
best_inlier_invalid = inlier.size(); |
715 |
best_error_invalid = inlier_error; |
716 |
} |
717 |
|
718 |
if(inlier.size() < min_inlier_threshold || new_inlier_error > max_dist_m){
|
719 |
//inlier.size() < ((float)initial_matches->size())*min_inlier_ratio ||
|
720 |
// ROS_INFO("Skipped iteration: inliers: %i (min %i), inlier_error: %.2f (max %.2f)", (int)inlier.size(), (int) min_inlier_threshold, inlier_error*100, max_dist_m*100);
|
721 |
continue;
|
722 |
} |
723 |
// ROS_INFO("Refined iteration from %i samples: all matches %i, inliers: %i, new_inlier_error: %f", (int)sample_size, (int)initial_matches->size(), (int)inlier.size(), new_inlier_error);
|
724 |
|
725 |
assert(new_inlier_error>=0);
|
726 |
|
727 |
if (new_inlier_error < best_error)
|
728 |
{ |
729 |
resulting_transformation = transformation; |
730 |
matches = inlier; |
731 |
assert(matches.size()>= min_inlier_threshold); |
732 |
//assert(matches.size()>= ((float)initial_matches->size())*min_inlier_ratio);
|
733 |
rmse = new_inlier_error; |
734 |
best_error = new_inlier_error; |
735 |
// ROS_INFO(" improved: new best iteration %d cnt: %d, best_inlier: %d, error: %.2f, bestError: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100, best_error*100);
|
736 |
}else
|
737 |
{ |
738 |
// ROS_INFO("improved: NO new best iteration %d cnt: %d, best_inlier: %d, error: %.2f, bestError: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100, best_error*100);
|
739 |
} |
740 |
} //iterations
|
741 |
ROS_INFO("%i good iterations (from %i), inlier pct %i, inlier cnt: %i, error: %.2f cm",valid_iterations, ransac_iterations, (int) (matches.size()*1.0/initial_matches->size()*100),(int) matches.size(),rmse*100); |
742 |
// ROS_INFO("best overall: inlier: %i, error: %.2f",best_inlier_invalid, best_error_invalid*100);
|
743 |
|
744 |
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"); |
745 |
return matches.size() >= min_inlier_threshold;
|
746 |
} |
747 |
|
748 |
|
749 |
#ifdef USE_ICP_CODE
|
750 |
void Node::Eigen2GICP(const Eigen::Matrix4f& m, dgc_transform_t g_m){ |
751 |
for(int i=0;i<4; i++) |
752 |
for(int j=0;j<4; j++) |
753 |
g_m[i][j] = m(i,j); |
754 |
|
755 |
} |
756 |
void Node::GICP2Eigen(const dgc_transform_t g_m, Eigen::Matrix4f& m){ |
757 |
for(int i=0;i<4; i++) |
758 |
for(int j=0;j<4; j++) |
759 |
m(i,j) = g_m[i][j]; |
760 |
} |
761 |
|
762 |
void Node::gicpSetIdentity(dgc_transform_t m){
|
763 |
for(int i=0;i<4; i++) |
764 |
for(int j=0;j<4; j++) |
765 |
if (i==j)
|
766 |
m[i][j] = 1;
|
767 |
else
|
768 |
m[i][j] = 0;
|
769 |
} |
770 |
#endif
|
771 |
|
772 |
|
773 |
//TODO: Merge this with processNodePair
|
774 |
MatchingResult Node::matchNodePair(const Node* older_node){
|
775 |
MatchingResult mr; |
776 |
if(initial_node_matches_ > ParameterServer::instance()->get<int>("max_connections")) return mr; //enough is enough |
777 |
const unsigned int min_matches = (unsigned int) ParameterServer::instance()->get<int>("min_matches");// minimal number of feature correspondences to be a valid candidate for a link |
778 |
// struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
|
779 |
|
780 |
this->findPairsFlann(older_node, &mr.all_matches);
|
781 |
ROS_DEBUG("found %i inital matches",(int) mr.all_matches.size()); |
782 |
if (mr.all_matches.size() < min_matches){
|
783 |
ROS_INFO("Too few inliers: Adding no Edge between %i and %i. Only %i correspondences to begin with.",
|
784 |
older_node->id_,this->id_,(int)mr.all_matches.size()); |
785 |
} |
786 |
else if (!getRelativeTransformationTo(older_node,&mr.all_matches, mr.ransac_trafo, mr.rmse, mr.inlier_matches) ){ // mr.all_matches.size()/3 |
787 |
ROS_INFO("Found no valid trafo, but had initially %d feature matches",(int) mr.all_matches.size()); |
788 |
} else {
|
789 |
++initial_node_matches_; //trafo is accepted
|
790 |
mr.final_trafo = mr.ransac_trafo; |
791 |
|
792 |
#ifdef USE_ICP_CODE
|
793 |
getRelativeTransformationTo_ICP_code(older_node,mr.icp_trafo, &mr.ransac_trafo); |
794 |
#endif
|
795 |
|
796 |
#ifdef USE_ICP_BIN
|
797 |
// improve transformation by using the generalized ICP
|
798 |
// std::clock_t starttime_gicp1 = std::clock();
|
799 |
bool converged = getRelativeTransformationTo_ICP_bin(older_node,mr.icp_trafo, &mr.ransac_trafo);
|
800 |
//ROS_INFO_STREAM("Paper: ICP1: " << ((std::clock()-starttime_gicp1*1.0) / (double)CLOCKS_PER_SEC));
|
801 |
|
802 |
|
803 |
ROS_INFO("icp: inliers: %i", mr.inlier_matches.size());
|
804 |
|
805 |
if (!converged) {
|
806 |
ROS_INFO("ICP did not converge. No Edge added");
|
807 |
return mr;
|
808 |
} |
809 |
|
810 |
mr.final_trafo = mr.ransac_trafo * mr.icp_trafo; |
811 |
|
812 |
MatchingResult mr_icp; |
813 |
std::vector<double> errors;
|
814 |
double error;
|
815 |
std::vector<cv::DMatch> inliers; |
816 |
// check if icp improves alignment:
|
817 |
computeInliersAndError(mr.inlier_matches, mr.final_trafo, |
818 |
this->feature_locations_3d_, older_node->feature_locations_3d_,
|
819 |
inliers, error, errors, 0.04*0.04); |
820 |
|
821 |
for (uint i=0; i<errors.size(); i++) |
822 |
{ |
823 |
cout << "error: " << round(errors[i]*10000)/100 << endl; |
824 |
} |
825 |
|
826 |
cout << "error was: " << mr.rmse << " and is now: " << error << endl; |
827 |
|
828 |
double roll, pitch, yaw, dist;
|
829 |
mat2components(mr.ransac_trafo, roll, pitch, yaw, dist); |
830 |
cout << "ransac: " << roll << " "<< pitch << " "<< yaw << " "<< dist << endl; |
831 |
|
832 |
|
833 |
mat2components(mr.icp_trafo, roll, pitch, yaw, dist); |
834 |
cout << "icp: " << roll << " "<< pitch << " "<< yaw << " "<< dist << endl; |
835 |
|
836 |
mat2components(mr.final_trafo, roll, pitch, yaw, dist); |
837 |
cout << "final: " << roll << " "<< pitch << " "<< yaw << " "<< dist << endl; |
838 |
|
839 |
|
840 |
cout << "ransac: " << endl << mr.ransac_trafo << endl;
|
841 |
cout << "icp: " << endl << mr.icp_trafo << endl;
|
842 |
cout << "total: " << endl << mr.final_trafo << endl;
|
843 |
|
844 |
|
845 |
if (error > (mr.rmse+0.02)) |
846 |
{ |
847 |
cout << "#### icp-error is too large, ignoring the connection" << endl;
|
848 |
return mr;
|
849 |
} |
850 |
#endif
|
851 |
|
852 |
|
853 |
#ifndef USE_ICP_BIN
|
854 |
#ifndef USE_ICP_CODE
|
855 |
mr.final_trafo = mr.ransac_trafo; |
856 |
#endif
|
857 |
#endif
|
858 |
|
859 |
mr.edge.id1 = older_node->id_;//and we have a valid transformation
|
860 |
mr.edge.id2 = this->id_; //since there are enough matching features, |
861 |
mr.edge.mean = eigen2G2O(mr.final_trafo.cast<double>());//we insert an edge between the frames |
862 |
|
863 |
/*this is a weight, that discounts the information coming from later nodes w.r.t earlier nodes.
|
864 |
int lowest_id = mr.edge.id1 < mr.edge.id2 ? mr.edge.id1 : mr.edge.id2;
|
865 |
double timeline_weight = lowest_id > 0 ? (double)lowest_id : 1.0;
|
866 |
*/
|
867 |
|
868 |
double w = (double)mr.inlier_matches.size();///(double)mr.all_matches.size(); |
869 |
mr.edge.informationMatrix = Eigen::Matrix<double,6,6>::Identity()*(w*w); //TODO: What do we do about the information matrix? Scale with inlier_count. Should rmse be integrated?) |
870 |
} |
871 |
// Paper
|
872 |
return mr;
|
873 |
} |
874 |
|
875 |
void Node::clearFeatureInformation(){
|
876 |
//clear only points, by swapping data with empty vector (so mem really gets freed)
|
877 |
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > f_l_3d; ///<backprojected 3d descriptor locations relative to cam position in homogeneous coordinates (last dimension is 1.0)
|
878 |
f_l_3d.swap(feature_locations_3d_); |
879 |
std::vector<cv::KeyPoint> f_l_2d; ///<Where in the image are the descriptors
|
880 |
f_l_2d.swap(feature_locations_2d_); |
881 |
feature_descriptors_.release(); |
882 |
} |
883 |
void Node::clearPointCloud(){
|
884 |
//clear only points, by swapping data with empty vector (so mem really gets freed)
|
885 |
pc_col->width = 0;
|
886 |
pc_col->height = 0;
|
887 |
pointcloud_type pc_empty; |
888 |
pc_empty.points.swap(pc_col->points); |
889 |
} |