GainCompensator.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/core/GainCompensator.h"
29 
30 #include <rtabmap/utilite/ULogger.h>
31 #include <rtabmap/utilite/UStl.h>
32 #include <rtabmap/utilite/UConversion.h>
33 #include <rtabmap/core/util3d_transforms.h>
34 #include <pcl/search/kdtree.h>
35 #include <pcl/common/common.h>
36 #include <pcl/common/transforms.h>
37 #include <pcl/correspondence.h>
38 
39 namespace rtabmap {
40 
41 double sqr(uchar v)
42 {
43  return double(v)*double(v);
44 }
45 
46 GainCompensator::GainCompensator(double maxCorrespondenceDistance, double minOverlap, double alpha, double beta) :
47  maxCorrespondenceDistance_(maxCorrespondenceDistance),
48  minOverlap_(minOverlap),
49  alpha_(alpha),
50  beta_(beta)
51 {
52 }
53 
54 GainCompensator::~GainCompensator() {
55 }
56 
57 void GainCompensator::feed(
58  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudA,
59  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudB,
60  const Transform & transformB)
61 {
62  std::multimap<int, Link> links;
63  links.insert(std::make_pair(0, Link(0,1,Link::kUserClosure, transformB)));
64  std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
65  clouds.insert(std::make_pair(0, cloudA));
66  clouds.insert(std::make_pair(1, cloudB));
67  feed(clouds, links);
68 }
69 
70 void GainCompensator::feed(
71  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudA,
72  const pcl::IndicesPtr & indicesA,
73  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudB,
74  const pcl::IndicesPtr & indicesB,
75  const Transform & transformB)
76 {
77  std::multimap<int, Link> links;
78  links.insert(std::make_pair(0, Link(0,1,Link::kUserClosure, transformB)));
79  std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
80  clouds.insert(std::make_pair(0, cloudA));
81  clouds.insert(std::make_pair(1, cloudB));
82  std::map<int, pcl::IndicesPtr> indices;
83  indices.insert(std::make_pair(0, indicesA));
84  indices.insert(std::make_pair(1, indicesB));
85  feed(clouds, indices, links);
86 }
87 
88 void GainCompensator::feed(
89  const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds,
90  const std::multimap<int, Link> & links)
91 {
92  std::map<int, pcl::IndicesPtr> indices;
93  feed(clouds, indices, links);
94 }
95 
96 // @see https://studiofreya.com/3d-math-and-physics/simple-aabb-vs-aabb-collision-detection/
97 struct AABB
98 {
99  AABB() : c(), r() {}
100 
101  AABB(const Eigen::Vector3f & center, const Eigen::Vector3f & halfwidths)
102  : c(center)
103  , r(halfwidths)
104  {}
105 
106  Eigen::Vector3f c; // center point
107  Eigen::Vector3f r; // halfwidths
108 };
109 
110 bool testAABBAABB(const AABB &a, const AABB &b)
111 {
112  if ( fabs(a.c[0] - b.c[0]) > (a.r[0] + b.r[0]) ) return false;
113  if ( fabs(a.c[1] - b.c[1]) > (a.r[1] + b.r[1]) ) return false;
114  if ( fabs(a.c[2] - b.c[2]) > (a.r[2] + b.r[2]) ) return false;
115 
116  // We have an overlap
117  return true;
118 };
119 
123 template<typename PointT>
124 void feedImpl(
125  const std::map<int, typename pcl::PointCloud<PointT>::Ptr> & clouds,
126  const std::map<int, pcl::IndicesPtr> & indices,
127  const std::multimap<int, Link> & links,
128  float maxCorrespondenceDistance,
129  double minOverlap,
130  double alpha,
131  double beta,
132  cv::Mat_<double> & gains,
133  std::map<int, int> & idToIndex)
134 {
135  UDEBUG("Exposure compensation...");
136 
137  UASSERT(maxCorrespondenceDistance > 0.0f);
138  UASSERT(indices.size() == 0 || clouds.size() == indices.size());
139 
140  const int num_images = static_cast<int>(clouds.size());
141  cv::Mat_<int> N(num_images, num_images); N.setTo(0);
142  cv::Mat_<double> I(num_images, num_images); I.setTo(0);
143 
144  cv::Mat_<double> IR(num_images, num_images); IR.setTo(0);
145  cv::Mat_<double> IG(num_images, num_images); IG.setTo(0);
146  cv::Mat_<double> IB(num_images, num_images); IB.setTo(0);
147 
148  // make id to index map
149  idToIndex.clear();
150  std::vector<int> indexToId(clouds.size());
151  int oi=0;
152  std::map<int, std::pair<pcl::PointXYZ, pcl::PointXYZ> > boundingBoxes;
153  for(typename std::map<int, typename pcl::PointCloud<PointT>::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
154  {
155  idToIndex.insert(std::make_pair(iter->first, oi));
156  indexToId[oi] = iter->first;
157  UASSERT(indices.empty() || uContains(indices, iter->first));
158  Eigen::Vector4f minPt(0,0,0,0);
159  Eigen::Vector4f maxPt(0,0,0,0);
160  if(indices.empty() || indices.at(iter->first)->empty())
161  {
162  N(oi,oi) = iter->second->size();
163  pcl::getMinMax3D(*iter->second, minPt, maxPt);
164  }
165  else
166  {
167  N(oi,oi) = indices.at(iter->first)->size();
168  pcl::getMinMax3D(*iter->second, *indices.at(iter->first), minPt, maxPt);
169  }
170  minPt[0] -= maxCorrespondenceDistance;
171  minPt[1] -= maxCorrespondenceDistance;
172  minPt[2] -= maxCorrespondenceDistance;
173  maxPt[0] += maxCorrespondenceDistance;
174  maxPt[1] += maxCorrespondenceDistance;
175  maxPt[2] += maxCorrespondenceDistance;
176  boundingBoxes.insert(std::make_pair(iter->first, std::make_pair(pcl::PointXYZ(minPt[0], minPt[1], minPt[2]), pcl::PointXYZ(maxPt[0], maxPt[1], maxPt[2]))));
177  ++oi;
178  }
179 
180  typename pcl::search::KdTree<PointT> kdtree;
181  int lastKdTreeId = 0;
182  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
183  {
184  if(uContains(idToIndex, iter->second.from()) && uContains(idToIndex, iter->second.to()))
185  {
186  const typename pcl::PointCloud<PointT>::Ptr & cloudFrom = clouds.at(iter->second.from());
187  const typename pcl::PointCloud<PointT>::Ptr & cloudTo = clouds.at(iter->second.to());
188  if(cloudFrom->size() && cloudTo->size())
189  {
190  //Are bounding boxes intersect?
191  std::pair<pcl::PointXYZ, pcl::PointXYZ> bbMinMaxFrom = boundingBoxes.at(iter->second.from());
192  std::pair<pcl::PointXYZ, pcl::PointXYZ> bbMinMaxTo = boundingBoxes.at(iter->second.to());
193  Eigen::Affine3f t = Transform::getIdentity().toEigen3f();
194  if(!iter->second.transform().isIdentity() && !iter->second.transform().isNull())
195  {
196  t = iter->second.transform().toEigen3f();
197  bbMinMaxTo.first = pcl::transformPoint(bbMinMaxTo.first, t);
198  bbMinMaxTo.second = pcl::transformPoint(bbMinMaxTo.second, t);
199  }
200  AABB bbFrom(Eigen::Vector3f((bbMinMaxFrom.second.x + bbMinMaxFrom.first.x)/2.0f, (bbMinMaxFrom.second.y + bbMinMaxFrom.first.y)/2.0f, (bbMinMaxFrom.second.z + bbMinMaxFrom.first.z)/2.0f),
201  Eigen::Vector3f((bbMinMaxFrom.second.x - bbMinMaxFrom.first.x)/2.0f, (bbMinMaxFrom.second.y - bbMinMaxFrom.first.y)/2.0f, (bbMinMaxFrom.second.z - bbMinMaxFrom.first.z)/2.0f));
202  AABB bbTo(Eigen::Vector3f((bbMinMaxTo.second.x + bbMinMaxTo.first.x)/2.0f, (bbMinMaxTo.second.y + bbMinMaxTo.first.y)/2.0f, (bbMinMaxTo.second.z + bbMinMaxTo.first.z)/2.0f),
203  Eigen::Vector3f((bbMinMaxTo.second.x - bbMinMaxTo.first.x)/2.0f, (bbMinMaxTo.second.y - bbMinMaxTo.first.y)/2.0f, (bbMinMaxTo.second.z - bbMinMaxTo.first.z)/2.0f));
204  //UDEBUG("%d = %f,%f,%f %f,%f,%f", iter->second.from(), bbMinMaxFrom.first[0], bbMinMaxFrom.first[1], bbMinMaxFrom.first[2], bbMinMaxFrom.second[0], bbMinMaxFrom.second[1], bbMinMaxFrom.second[2]);
205  //UDEBUG("%d = %f,%f,%f %f,%f,%f", iter->second.to(), bbMinMaxTo.first[0], bbMinMaxTo.first[1], bbMinMaxTo.first[2], bbMinMaxTo.second[0], bbMinMaxTo.second[1], bbMinMaxTo.second[2]);
206  if(testAABBAABB(bbFrom, bbTo))
207  {
208  if(lastKdTreeId <= 0 || lastKdTreeId!=iter->second.from())
209  {
210  //reconstruct kdtree
211  if(indices.size() && indices.at(iter->second.from())->size())
212  {
213  kdtree.setInputCloud(cloudFrom, indices.at(iter->second.from()));
214  }
215  else
216  {
217  kdtree.setInputCloud(cloudFrom);
218  }
219  }
220 
221  pcl::Correspondences correspondences;
222  pcl::IndicesPtr indicesTo(new std::vector<int>);
223  std::set<int> addedFrom;
224  if(indices.size() && indices.at(iter->second.to())->size())
225  {
226  const pcl::IndicesPtr & indicesTo = indices.at(iter->second.to());
227  correspondences.resize(indicesTo->size());
228  int oi=0;
229  for(unsigned int i=0; i<indicesTo->size(); ++i)
230  {
231  std::vector<int> k_indices;
232  std::vector<float> k_sqr_distances;
233  if(kdtree.radiusSearch(pcl::transformPoint(cloudTo->at(indicesTo->at(i)), t), maxCorrespondenceDistance, k_indices, k_sqr_distances, 1))
234  {
235  if(addedFrom.find(k_indices[0]) == addedFrom.end())
236  {
237  correspondences[oi].index_match = k_indices[0];
238  correspondences[oi].index_query = indicesTo->at(i);
239  correspondences[oi].distance = k_sqr_distances[0];
240  addedFrom.insert(k_indices[0]);
241  ++oi;
242  }
243  }
244  }
245  correspondences.resize(oi);
246  }
247  else
248  {
249  correspondences.resize(cloudTo->size());
250  int oi=0;
251  for(unsigned int i=0; i<cloudTo->size(); ++i)
252  {
253  std::vector<int> k_indices;
254  std::vector<float> k_sqr_distances;
255  if(kdtree.radiusSearch(pcl::transformPoint(cloudTo->at(i), t), maxCorrespondenceDistance, k_indices, k_sqr_distances, 1))
256  {
257  if(addedFrom.find(k_indices[0]) == addedFrom.end())
258  {
259  correspondences[oi].index_match = k_indices[0];
260  correspondences[oi].index_query = i;
261  correspondences[oi].distance = k_sqr_distances[0];
262  addedFrom.insert(k_indices[0]);
263  ++oi;
264  }
265  }
266  }
267  correspondences.resize(oi);
268  }
269 
270  UDEBUG("%d->%d: correspondences = %d", iter->second.from(), iter->second.to(), (int)correspondences.size());
271  if(correspondences.size() && (minOverlap <= 0.0 ||
272  (double(correspondences.size()) / double(clouds.at(iter->second.from())->size()) >= minOverlap &&
273  double(correspondences.size()) / double(clouds.at(iter->second.to())->size()) >= minOverlap)))
274  {
275  int i = idToIndex.at(iter->second.from());
276  int j = idToIndex.at(iter->second.to());
277 
278  double Isum1 = 0, Isum2 = 0;
279  double IRsum1 = 0, IRsum2 = 0;
280  double IGsum1 = 0, IGsum2 = 0;
281  double IBsum1 = 0, IBsum2 = 0;
282  for (unsigned int c = 0; c < correspondences.size(); ++c)
283  {
284  const PointT & pt1 = cloudFrom->at(correspondences.at(c).index_match);
285  const PointT & pt2 = cloudTo->at(correspondences.at(c).index_query);
286 
287  Isum1 += std::sqrt(static_cast<double>(sqr(pt1.r) + sqr(pt1.g) + sqr(pt1.b)));
288  Isum2 += std::sqrt(static_cast<double>(sqr(pt2.r) + sqr(pt2.g) + sqr(pt2.b)));
289 
290  IRsum1 += static_cast<double>(pt1.r);
291  IRsum2 += static_cast<double>(pt2.r);
292  IGsum1 += static_cast<double>(pt1.g);
293  IGsum2 += static_cast<double>(pt2.g);
294  IBsum1 += static_cast<double>(pt1.b);
295  IBsum2 += static_cast<double>(pt2.b);
296  }
297  N(i, j) = N(j, i) = correspondences.size();
298  I(i, j) = Isum1 / N(i, j);
299  I(j, i) = Isum2 / N(i, j);
300 
301  IR(i, j) = IRsum1 / N(i, j);
302  IR(j, i) = IRsum2 / N(i, j);
303  IG(i, j) = IGsum1 / N(i, j);
304  IG(j, i) = IGsum2 / N(i, j);
305  IB(i, j) = IBsum1 / N(i, j);
306  IB(j, i) = IBsum2 / N(i, j);
307  }
308  }
309  }
310  }
311  }
312 
313  cv::Mat_<double> A(num_images, num_images); A.setTo(0);
314  cv::Mat_<double> b(num_images, 1); b.setTo(0);
315  cv::Mat_<double> AR(num_images, num_images); AR.setTo(0);
316  cv::Mat_<double> AG(num_images, num_images); AG.setTo(0);
317  cv::Mat_<double> AB(num_images, num_images); AB.setTo(0);
318  for (int i = 0; i < num_images; ++i)
319  {
320  for (int j = 0; j < num_images; ++j)
321  {
322  b(i, 0) += beta * N(i, j);
323  A(i, i) += beta * N(i, j);
324  AR(i, i) += beta * N(i, j);
325  AG(i, i) += beta * N(i, j);
326  AB(i, i) += beta * N(i, j);
327  if (j == i) continue;
328  A(i, i) += 2 * alpha * I(i, j) * I(i, j) * N(i, j);
329  A(i, j) -= 2 * alpha * I(i, j) * I(j, i) * N(i, j);
330 
331  AR(i, i) += 2 * alpha * IR(i, j) * IR(i, j) * N(i, j);
332  AR(i, j) -= 2 * alpha * IR(i, j) * IR(j, i) * N(i, j);
333 
334  AG(i, i) += 2 * alpha * IG(i, j) * IG(i, j) * N(i, j);
335  AG(i, j) -= 2 * alpha * IG(i, j) * IG(j, i) * N(i, j);
336 
337  AB(i, i) += 2 * alpha * IB(i, j) * IB(i, j) * N(i, j);
338  AB(i, j) -= 2 * alpha * IB(i, j) * IB(j, i) * N(i, j);
339  }
340  }
341 
342  cv::Mat_<double> gainsGray, gainsR, gainsG, gainsB;
343  cv::solve(A, b, gainsGray);
344 
345  cv::solve(AR, b, gainsR);
346  cv::solve(AG, b, gainsG);
347  cv::solve(AB, b, gainsB);
348 
349  gains = cv::Mat_<double>(gainsGray.rows, 4);
350  gainsGray.copyTo(gains.col(0));
351  gainsR.copyTo(gains.col(1));
352  gainsG.copyTo(gains.col(2));
353  gainsB.copyTo(gains.col(3));
354 
355  if(ULogger::kInfo)
356  {
357  for(int i=0; i<gains.rows; ++i)
358  {
359  UINFO("Gain index=%d (id=%d) = %f (%f,%f,%f)", i, indexToId[i], gains(i, 0), gains(i, 1), gains(i, 2), gains(i, 3));
360  }
361  }
362 }
363 
364 void GainCompensator::feed(
365  const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds,
366  const std::map<int, pcl::IndicesPtr> & indices,
367  const std::multimap<int, Link> & links)
368 {
369  feedImpl<pcl::PointXYZRGB>(clouds, indices, links, maxCorrespondenceDistance_, minOverlap_, alpha_, beta_, gains_, idToIndex_);
370 }
371 void GainCompensator::feed(
372  const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
373  const std::map<int, pcl::IndicesPtr> & indices,
374  const std::multimap<int, Link> & links)
375 {
376  feedImpl<pcl::PointXYZRGBNormal>(clouds, indices, links, maxCorrespondenceDistance_, minOverlap_, alpha_, beta_, gains_, idToIndex_);
377 }
378 void GainCompensator::feed(
379  const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & cloudsIndices,
380  const std::multimap<int, Link> & links)
381 {
382  std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
383  std::map<int, pcl::IndicesPtr> indices;
384  for(std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> >::const_iterator iter=cloudsIndices.begin(); iter!=cloudsIndices.end(); ++iter)
385  {
386  clouds.insert(std::make_pair(iter->first, iter->second.first));
387  indices.insert(std::make_pair(iter->first, iter->second.second));
388  }
389  feedImpl<pcl::PointXYZRGBNormal>(clouds, indices, links, maxCorrespondenceDistance_, minOverlap_, alpha_, beta_, gains_, idToIndex_);
390 }
391 
392 template<typename PointT>
393 void applyImpl(
394  int index,
395  typename pcl::PointCloud<PointT>::Ptr & cloud,
396  const pcl::IndicesPtr & indices,
397  const cv::Mat_<double> & gains,
398  bool rgb)
399 {
400  double gainR = gains(index, rgb?1:0);
401  double gainG = gains(index, rgb?2:0);
402  double gainB = gains(index, rgb?3:0);
403  UDEBUG("index=%d gain=%f (%f,%f,%f)", index, gains(index, 0), gainR, gainG, gainB);
404  if(indices->size())
405  {
406  for(unsigned int i=0; i<indices->size(); ++i)
407  {
408  PointT & pt = cloud->at(indices->at(i));
409  pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gainR)));
410  pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gainG)));
411  pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gainB)));
412  }
413  }
414  else
415  {
416  for(unsigned int i=0; i<cloud->size(); ++i)
417  {
418  PointT & pt = cloud->at(i);
419  pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gainR)));
420  pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gainG)));
421  pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gainB)));
422  }
423  }
424 }
425 
426 void GainCompensator::apply(
427  int id,
428  pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
429  bool rgb) const
430 {
431  pcl::IndicesPtr indices(new std::vector<int>);
432  apply(id, cloud, indices, rgb);
433 }
434 void GainCompensator::apply(
435  int id,
436  pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
437  const pcl::IndicesPtr & indices,
438  bool rgb) const
439 {
440  UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
441  applyImpl<pcl::PointXYZRGB>(idToIndex_.at(id), cloud, indices, gains_, rgb);
442 }
443 void GainCompensator::apply(
444  int id,
445  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
446  const pcl::IndicesPtr & indices,
447  bool rgb) const
448 {
449  UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
450  applyImpl<pcl::PointXYZRGBNormal>(idToIndex_.at(id), cloud, indices, gains_, rgb);
451 }
452 
453 void GainCompensator::apply(
454  int id,
455  cv::Mat & image,
456  bool rgb) const
457 {
458  UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
459  if(image.channels() == 1 || !rgb)
460  {
461  cv::multiply(image, gains_(idToIndex_.at(id), 0), image);
462  }
463  else if(image.channels()>=3)
464  {
465  std::vector<cv::Mat> channels;
466  cv::split(image, channels);
467  // assuming BGR
468  cv::multiply(channels[0], gains_(idToIndex_.at(id), 3), channels[0]);
469  cv::multiply(channels[1], gains_(idToIndex_.at(id), 2), channels[1]);
470  cv::multiply(channels[2], gains_(idToIndex_.at(id), 1), channels[2]);
471  cv::merge(channels, image);
472  }
473 }
474 
475 double GainCompensator::getGain(int id, double * r, double * g, double * b) const
476 {
477  UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
478 
479  if(r)
480  {
481  *r = gains_(idToIndex_.at(id), 1);
482  }
483  if(g)
484  {
485  *g = gains_(idToIndex_.at(id), 2);
486  }
487  if(b)
488  {
489  *b = gains_(idToIndex_.at(id), 3);
490  }
491 
492  return gains_(idToIndex_.at(id), 0);
493 }
494 
495 int GainCompensator::getIndex(int id) const
496 {
497  if(uContains(idToIndex_, id))
498  {
499  return idToIndex_.at(id);
500  }
501  return -1;
502 }
503 
504 } /* namespace rtabmap */
rtabmap::GainCompensator::gains_
cv::Mat_< double > gains_
Definition: GainCompensator.h:97
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::util3d::getMinMax3D
void RTABMAP_EXP getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2666
f
f
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtflann::uchar
unsigned char uchar
Definition: matrix.h:41
rtabmap::AABB::AABB
AABB(const Eigen::Vector3f &center, const Eigen::Vector3f &halfwidths)
Definition: GainCompensator.cpp:101
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:380
UConversion.h
Some conversion functions.
rtabmap::AABB::r
Eigen::Vector3f r
Definition: GainCompensator.cpp:107
rtabmap::GainCompensator::getGain
double getGain(int id, double *r=0, double *g=0, double *b=0) const
Definition: GainCompensator.cpp:475
rtabmap::GainCompensator::GainCompensator
GainCompensator(double maxCorrespondenceDistance=0.02, double minOverlap=0.0, double alpha=0.01, double beta=10)
Definition: GainCompensator.cpp:46
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::GainCompensator::idToIndex_
std::map< int, int > idToIndex_
Definition: GainCompensator.h:98
rtabmap::feedImpl
void feedImpl(const std::map< int, typename pcl::PointCloud< PointT >::Ptr > &clouds, const std::map< int, pcl::IndicesPtr > &indices, const std::multimap< int, Link > &links, float maxCorrespondenceDistance, double minOverlap, double alpha, double beta, cv::Mat_< double > &gains, std::map< int, int > &idToIndex)
Definition: GainCompensator.cpp:124
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
util3d_transforms.h
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::GainCompensator::apply
void apply(int id, pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, bool rgb=true) const
Definition: GainCompensator.cpp:426
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::sqr
double sqr(uchar v)
Definition: GainCompensator.cpp:41
rtabmap::applyImpl
void applyImpl(int index, typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const cv::Mat_< double > &gains, bool rgb)
Definition: GainCompensator.cpp:393
ULogger.h
ULogger class and convenient macros.
rtabmap::GainCompensator::feed
void feed(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloudA, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloudB, const Transform &transformB)
Definition: GainCompensator.cpp:57
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:360
rtabmap::GainCompensator::getIndex
int getIndex(int id) const
Definition: GainCompensator.cpp:495
rtabmap::testAABBAABB
bool testAABBAABB(const AABB &a, const AABB &b)
Definition: GainCompensator.cpp:110
rtabmap::AABB::c
Eigen::Vector3f c
Definition: GainCompensator.cpp:106
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58