GainCompensator.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "rtabmap/core/GainCompensator.h"
00029 
00030 #include <rtabmap/utilite/ULogger.h>
00031 #include <rtabmap/utilite/UStl.h>
00032 #include <rtabmap/utilite/UConversion.h>
00033 #include <rtabmap/core/util3d_transforms.h>
00034 #include <pcl/search/kdtree.h>
00035 #include <pcl/common/common.h>
00036 #include <pcl/common/transforms.h>
00037 #include <pcl/correspondence.h>
00038 
00039 namespace rtabmap {
00040 
00041 double sqr(uchar v)
00042 {
00043         return double(v)*double(v);
00044 }
00045 
00046 GainCompensator::GainCompensator(double maxCorrespondenceDistance, double minOverlap, double alpha, double beta) :
00047                 maxCorrespondenceDistance_(maxCorrespondenceDistance),
00048                 minOverlap_(minOverlap),
00049                 alpha_(alpha),
00050                 beta_(beta)
00051 {
00052 }
00053 
00054 GainCompensator::~GainCompensator() {
00055 }
00056 
00057 void GainCompensator::feed(
00058                 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudA,
00059                 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudB,
00060                 const Transform & transformB)
00061 {
00062         std::multimap<int, Link> links;
00063         links.insert(std::make_pair(0, Link(0,1,Link::kUserClosure, transformB)));
00064         std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
00065         clouds.insert(std::make_pair(0, cloudA));
00066         clouds.insert(std::make_pair(1, cloudB));
00067         feed(clouds, links);
00068 }
00069 
00070 void GainCompensator::feed(
00071                 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudA,
00072                 const pcl::IndicesPtr & indicesA,
00073                 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudB,
00074                 const pcl::IndicesPtr & indicesB,
00075                 const Transform & transformB)
00076 {
00077         std::multimap<int, Link> links;
00078         links.insert(std::make_pair(0, Link(0,1,Link::kUserClosure, transformB)));
00079         std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
00080         clouds.insert(std::make_pair(0, cloudA));
00081         clouds.insert(std::make_pair(1, cloudB));
00082         std::map<int, pcl::IndicesPtr> indices;
00083         indices.insert(std::make_pair(0, indicesA));
00084         indices.insert(std::make_pair(1, indicesB));
00085         feed(clouds, indices, links);
00086 }
00087 
00088 void GainCompensator::feed(
00089                 const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds,
00090                 const std::multimap<int, Link> & links)
00091 {
00092         std::map<int, pcl::IndicesPtr> indices;
00093         feed(clouds, indices, links);
00094 }
00095 
00096 // @see https://studiofreya.com/3d-math-and-physics/simple-aabb-vs-aabb-collision-detection/
00097 struct AABB
00098 {
00099     AABB() : c(), r() {}
00100 
00101     AABB(const Eigen::Vector3f & center, const Eigen::Vector3f & halfwidths)
00102         : c(center)
00103         , r(halfwidths)
00104     {}
00105 
00106     Eigen::Vector3f c;        // center point
00107     Eigen::Vector3f r;        // halfwidths
00108 };
00109 
00110 bool testAABBAABB(const AABB &a, const AABB &b)
00111 {
00112     if ( fabs(a.c[0] - b.c[0]) > (a.r[0] + b.r[0]) ) return false;
00113     if ( fabs(a.c[1] - b.c[1]) > (a.r[1] + b.r[1]) ) return false;
00114     if ( fabs(a.c[2] - b.c[2]) > (a.r[2] + b.r[2]) ) return false;
00115 
00116     // We have an overlap
00117     return true;
00118 };
00119 
00123 template<typename PointT>
00124 void feedImpl(
00125                 const std::map<int, typename pcl::PointCloud<PointT>::Ptr> & clouds,
00126                 const std::map<int, pcl::IndicesPtr> & indices,
00127                 const std::multimap<int, Link> & links,
00128                 float maxCorrespondenceDistance,
00129                 double minOverlap,
00130                 double alpha,
00131                 double beta,
00132                 cv::Mat_<double> & gains,
00133                 std::map<int, int> & idToIndex)
00134 {
00135         UDEBUG("Exposure compensation...");
00136 
00137         UASSERT(maxCorrespondenceDistance > 0.0f);
00138         UASSERT(indices.size() == 0 || clouds.size() == indices.size());
00139 
00140         const int num_images = static_cast<int>(clouds.size());
00141         cv::Mat_<int> N(num_images, num_images); N.setTo(0);
00142         cv::Mat_<double> I(num_images, num_images); I.setTo(0);
00143 
00144         cv::Mat_<double> IR(num_images, num_images); IR.setTo(0);
00145         cv::Mat_<double> IG(num_images, num_images); IG.setTo(0);
00146         cv::Mat_<double> IB(num_images, num_images); IB.setTo(0);
00147 
00148         // make id to index map
00149         idToIndex.clear();
00150         std::vector<int> indexToId(clouds.size());
00151         int oi=0;
00152         std::map<int, std::pair<pcl::PointXYZ, pcl::PointXYZ> > boundingBoxes;
00153         for(typename std::map<int, typename pcl::PointCloud<PointT>::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
00154         {
00155                 idToIndex.insert(std::make_pair(iter->first, oi));
00156                 indexToId[oi] = iter->first;
00157                 UASSERT(indices.empty() || uContains(indices, iter->first));
00158                 Eigen::Vector4f minPt(0,0,0,0);
00159                 Eigen::Vector4f maxPt(0,0,0,0);
00160                 if(indices.empty() || indices.at(iter->first)->empty())
00161                 {
00162                         N(oi,oi) = iter->second->size();
00163                         pcl::getMinMax3D(*iter->second, minPt, maxPt);
00164                 }
00165                 else
00166                 {
00167                         N(oi,oi) = indices.at(iter->first)->size();
00168                         pcl::getMinMax3D(*iter->second, *indices.at(iter->first), minPt, maxPt);
00169                 }
00170                 minPt[0] -= maxCorrespondenceDistance;
00171                 minPt[1] -= maxCorrespondenceDistance;
00172                 minPt[2] -= maxCorrespondenceDistance;
00173                 maxPt[0] += maxCorrespondenceDistance;
00174                 maxPt[1] += maxCorrespondenceDistance;
00175                 maxPt[2] += maxCorrespondenceDistance;
00176                 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]))));
00177                 ++oi;
00178         }
00179 
00180         typename pcl::search::KdTree<PointT> kdtree;
00181         int lastKdTreeId = 0;
00182         for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00183         {
00184                 if(uContains(idToIndex, iter->second.from()) && uContains(idToIndex, iter->second.to()))
00185                 {
00186                         const typename pcl::PointCloud<PointT>::Ptr & cloudFrom = clouds.at(iter->second.from());
00187                         const typename pcl::PointCloud<PointT>::Ptr & cloudTo = clouds.at(iter->second.to());
00188                         if(cloudFrom->size() && cloudTo->size())
00189                         {
00190                                 //Are bounding boxes intersect?
00191                                 std::pair<pcl::PointXYZ, pcl::PointXYZ> bbMinMaxFrom = boundingBoxes.at(iter->second.from());
00192                                 std::pair<pcl::PointXYZ, pcl::PointXYZ> bbMinMaxTo = boundingBoxes.at(iter->second.to());
00193                                 Eigen::Affine3f t = Transform::getIdentity().toEigen3f();
00194                                 if(!iter->second.transform().isIdentity() && !iter->second.transform().isNull())
00195                                 {
00196                                         t = iter->second.transform().toEigen3f();
00197                                         bbMinMaxTo.first = pcl::transformPoint(bbMinMaxTo.first, t);
00198                                         bbMinMaxTo.second = pcl::transformPoint(bbMinMaxTo.second, t);
00199                                 }
00200                                 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),
00201                                                  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));
00202                                 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),
00203                                                  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));
00204                                 //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]);
00205                                 //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]);
00206                                 if(testAABBAABB(bbFrom, bbTo))
00207                                 {
00208                                         if(lastKdTreeId <= 0 || lastKdTreeId!=iter->second.from())
00209                                         {
00210                                                 //reconstruct kdtree
00211                                                 if(indices.size() && indices.at(iter->second.from())->size())
00212                                                 {
00213                                                         kdtree.setInputCloud(cloudFrom, indices.at(iter->second.from()));
00214                                                 }
00215                                                 else
00216                                                 {
00217                                                         kdtree.setInputCloud(cloudFrom);
00218                                                 }
00219                                         }
00220 
00221                                         pcl::Correspondences correspondences;
00222                                         pcl::IndicesPtr indicesTo(new std::vector<int>);
00223                                         std::set<int> addedFrom;
00224                                         if(indices.size() && indices.at(iter->second.to())->size())
00225                                         {
00226                                                 const pcl::IndicesPtr & indicesTo = indices.at(iter->second.to());
00227                                                 correspondences.resize(indicesTo->size());
00228                                                 int oi=0;
00229                                                 for(unsigned int i=0; i<indicesTo->size(); ++i)
00230                                                 {
00231                                                         std::vector<int> k_indices;
00232                                                         std::vector<float> k_sqr_distances;
00233                                                         if(kdtree.radiusSearch(pcl::transformPoint(cloudTo->at(indicesTo->at(i)), t), maxCorrespondenceDistance, k_indices, k_sqr_distances, 1))
00234                                                         {
00235                                                                 if(addedFrom.find(k_indices[0]) == addedFrom.end())
00236                                                                 {
00237                                                                         correspondences[oi].index_match = k_indices[0];
00238                                                                         correspondences[oi].index_query = indicesTo->at(i);
00239                                                                         correspondences[oi].distance = k_sqr_distances[0];
00240                                                                         addedFrom.insert(k_indices[0]);
00241                                                                         ++oi;
00242                                                                 }
00243                                                         }
00244                                                 }
00245                                                 correspondences.resize(oi);
00246                                         }
00247                                         else
00248                                         {
00249                                                 correspondences.resize(cloudTo->size());
00250                                                 int oi=0;
00251                                                 for(unsigned int i=0; i<cloudTo->size(); ++i)
00252                                                 {
00253                                                         std::vector<int> k_indices;
00254                                                         std::vector<float> k_sqr_distances;
00255                                                         if(kdtree.radiusSearch(pcl::transformPoint(cloudTo->at(i), t), maxCorrespondenceDistance, k_indices, k_sqr_distances, 1))
00256                                                         {
00257                                                                 if(addedFrom.find(k_indices[0]) == addedFrom.end())
00258                                                                 {
00259                                                                         correspondences[oi].index_match = k_indices[0];
00260                                                                         correspondences[oi].index_query = i;
00261                                                                         correspondences[oi].distance = k_sqr_distances[0];
00262                                                                         addedFrom.insert(k_indices[0]);
00263                                                                         ++oi;
00264                                                                 }
00265                                                         }
00266                                                 }
00267                                                 correspondences.resize(oi);
00268                                         }
00269 
00270                                         UDEBUG("%d->%d: correspondences = %d", iter->second.from(), iter->second.to(), (int)correspondences.size());
00271                                         if(correspondences.size() && (minOverlap <= 0.0 ||
00272                                                         (double(correspondences.size()) / double(clouds.at(iter->second.from())->size()) >= minOverlap &&
00273                                                          double(correspondences.size()) / double(clouds.at(iter->second.to())->size()) >= minOverlap)))
00274                                         {
00275                                                 int i = idToIndex.at(iter->second.from());
00276                                                 int j = idToIndex.at(iter->second.to());
00277 
00278                                                 double Isum1 = 0, Isum2 = 0;
00279                                                 double IRsum1 = 0, IRsum2 = 0;
00280                                                 double IGsum1 = 0, IGsum2 = 0;
00281                                                 double IBsum1 = 0, IBsum2 = 0;
00282                                                 for (unsigned int c = 0; c < correspondences.size(); ++c)
00283                                                 {
00284                                                         const PointT & pt1 = cloudFrom->at(correspondences.at(c).index_match);
00285                                                         const PointT & pt2 = cloudTo->at(correspondences.at(c).index_query);
00286 
00287                                                         Isum1 += std::sqrt(static_cast<double>(sqr(pt1.r) + sqr(pt1.g) + sqr(pt1.b)));
00288                                                         Isum2 += std::sqrt(static_cast<double>(sqr(pt2.r) + sqr(pt2.g) + sqr(pt2.b)));
00289 
00290                                                         IRsum1 += static_cast<double>(pt1.r);
00291                                                         IRsum2 += static_cast<double>(pt2.r);
00292                                                         IGsum1 += static_cast<double>(pt1.g);
00293                                                         IGsum2 += static_cast<double>(pt2.g);
00294                                                         IBsum1 += static_cast<double>(pt1.b);
00295                                                         IBsum2 += static_cast<double>(pt2.b);
00296                                                 }
00297                                                 N(i, j) = N(j, i) = correspondences.size();
00298                                                 I(i, j) = Isum1 / N(i, j);
00299                                                 I(j, i) = Isum2 / N(i, j);
00300 
00301                                                 IR(i, j) = IRsum1 / N(i, j);
00302                                                 IR(j, i) = IRsum2 / N(i, j);
00303                                                 IG(i, j) = IGsum1 / N(i, j);
00304                                                 IG(j, i) = IGsum2 / N(i, j);
00305                                                 IB(i, j) = IBsum1 / N(i, j);
00306                                                 IB(j, i) = IBsum2 / N(i, j);
00307                                         }
00308                                 }
00309                         }
00310                 }
00311         }
00312 
00313         cv::Mat_<double> A(num_images, num_images); A.setTo(0);
00314         cv::Mat_<double> b(num_images, 1); b.setTo(0);
00315         cv::Mat_<double> AR(num_images, num_images); AR.setTo(0);
00316         cv::Mat_<double> AG(num_images, num_images); AG.setTo(0);
00317         cv::Mat_<double> AB(num_images, num_images); AB.setTo(0);
00318         for (int i = 0; i < num_images; ++i)
00319         {
00320                 for (int j = 0; j < num_images; ++j)
00321                 {
00322                         b(i, 0) += beta * N(i, j);
00323                         A(i, i) += beta * N(i, j);
00324                         AR(i, i) += beta * N(i, j);
00325                         AG(i, i) += beta * N(i, j);
00326                         AB(i, i) += beta * N(i, j);
00327                         if (j == i) continue;
00328                         A(i, i) += 2 * alpha * I(i, j) * I(i, j) * N(i, j);
00329                         A(i, j) -= 2 * alpha * I(i, j) * I(j, i) * N(i, j);
00330 
00331                         AR(i, i) += 2 * alpha * IR(i, j) * IR(i, j) * N(i, j);
00332                         AR(i, j) -= 2 * alpha * IR(i, j) * IR(j, i) * N(i, j);
00333 
00334                         AG(i, i) += 2 * alpha * IG(i, j) * IG(i, j) * N(i, j);
00335                         AG(i, j) -= 2 * alpha * IG(i, j) * IG(j, i) * N(i, j);
00336 
00337                         AB(i, i) += 2 * alpha * IB(i, j) * IB(i, j) * N(i, j);
00338                         AB(i, j) -= 2 * alpha * IB(i, j) * IB(j, i) * N(i, j);
00339                 }
00340         }
00341 
00342         cv::Mat_<double> gainsGray, gainsR, gainsG, gainsB;
00343         cv::solve(A, b, gainsGray);
00344 
00345         cv::solve(AR, b, gainsR);
00346         cv::solve(AG, b, gainsG);
00347         cv::solve(AB, b, gainsB);
00348 
00349         gains = cv::Mat_<double>(gainsGray.rows, 4);
00350         gainsGray.copyTo(gains.col(0));
00351         gainsR.copyTo(gains.col(1));
00352         gainsG.copyTo(gains.col(2));
00353         gainsB.copyTo(gains.col(3));
00354 
00355         if(ULogger::kInfo)
00356         {
00357                 for(int i=0; i<gains.rows; ++i)
00358                 {
00359                         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));
00360                 }
00361         }
00362 }
00363 
00364 void GainCompensator::feed(
00365                 const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds,
00366                 const std::map<int, pcl::IndicesPtr> & indices,
00367                 const std::multimap<int, Link> & links)
00368 {
00369         feedImpl<pcl::PointXYZRGB>(clouds, indices, links, maxCorrespondenceDistance_, minOverlap_, alpha_, beta_, gains_, idToIndex_);
00370 }
00371 void GainCompensator::feed(
00372                 const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
00373                 const std::map<int, pcl::IndicesPtr> & indices,
00374                 const std::multimap<int, Link> & links)
00375 {
00376         feedImpl<pcl::PointXYZRGBNormal>(clouds, indices, links, maxCorrespondenceDistance_, minOverlap_, alpha_, beta_, gains_, idToIndex_);
00377 }
00378 void GainCompensator::feed(
00379                 const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & cloudsIndices,
00380                 const std::multimap<int, Link> & links)
00381 {
00382         std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
00383         std::map<int, pcl::IndicesPtr> indices;
00384         for(std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> >::const_iterator iter=cloudsIndices.begin(); iter!=cloudsIndices.end(); ++iter)
00385         {
00386                 clouds.insert(std::make_pair(iter->first, iter->second.first));
00387                 indices.insert(std::make_pair(iter->first, iter->second.second));
00388         }
00389         feedImpl<pcl::PointXYZRGBNormal>(clouds, indices, links, maxCorrespondenceDistance_, minOverlap_, alpha_, beta_, gains_, idToIndex_);
00390 }
00391 
00392 template<typename PointT>
00393 void applyImpl(
00394                 int index,
00395                 typename pcl::PointCloud<PointT>::Ptr & cloud,
00396                 const pcl::IndicesPtr & indices,
00397                 const cv::Mat_<double> & gains,
00398                 bool rgb)
00399 {
00400         double gainR = gains(index, rgb?1:0);
00401         double gainG = gains(index, rgb?2:0);
00402         double gainB = gains(index, rgb?3:0);
00403         UDEBUG("index=%d gain=%f (%f,%f,%f)", index, gains(index, 0), gainR, gainG, gainB);
00404         if(indices->size())
00405         {
00406                 for(unsigned int i=0; i<indices->size(); ++i)
00407                 {
00408                         PointT & pt = cloud->at(indices->at(i));
00409                         pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gainR)));
00410                         pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gainG)));
00411                         pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gainB)));
00412                 }
00413         }
00414         else
00415         {
00416                 for(unsigned int i=0; i<cloud->size(); ++i)
00417                 {
00418                         PointT & pt = cloud->at(i);
00419                         pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gainR)));
00420                         pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gainG)));
00421                         pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gainB)));
00422                 }
00423         }
00424 }
00425 
00426 void GainCompensator::apply(
00427                 int id,
00428                 pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
00429                 bool rgb) const
00430 {
00431         pcl::IndicesPtr indices(new std::vector<int>);
00432         apply(id, cloud, indices, rgb);
00433 }
00434 void GainCompensator::apply(
00435                 int id,
00436                 pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
00437                 const pcl::IndicesPtr & indices,
00438                 bool rgb) const
00439 {
00440         UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
00441         applyImpl<pcl::PointXYZRGB>(idToIndex_.at(id), cloud, indices, gains_, rgb);
00442 }
00443 void GainCompensator::apply(
00444                 int id,
00445                 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
00446                 const pcl::IndicesPtr & indices,
00447                 bool rgb) const
00448 {
00449         UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
00450         applyImpl<pcl::PointXYZRGBNormal>(idToIndex_.at(id), cloud, indices, gains_, rgb);
00451 }
00452 
00453 void GainCompensator::apply(
00454                 int id,
00455                 cv::Mat & image,
00456                 bool rgb) const
00457 {
00458         UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
00459         if(image.channels() == 1 || !rgb)
00460         {
00461                 cv::multiply(image, gains_(idToIndex_.at(id), 0), image);
00462         }
00463         else if(image.channels()>=3)
00464         {
00465                 std::vector<cv::Mat> channels;
00466                 cv::split(image, channels);
00467                 // assuming BGR
00468                 cv::multiply(channels[0], gains_(idToIndex_.at(id), 3), channels[0]);
00469                 cv::multiply(channels[1], gains_(idToIndex_.at(id), 2), channels[1]);
00470                 cv::multiply(channels[2], gains_(idToIndex_.at(id), 1), channels[2]);
00471                 cv::merge(channels, image);
00472         }
00473 }
00474 
00475 double GainCompensator::getGain(int id, double * r, double * g, double * b) const
00476 {
00477         UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str());
00478 
00479         if(r)
00480         {
00481                 *r = gains_(idToIndex_.at(id), 1);
00482         }
00483         if(g)
00484         {
00485                 *g = gains_(idToIndex_.at(id), 2);
00486         }
00487         if(b)
00488         {
00489                 *b = gains_(idToIndex_.at(id), 3);
00490         }
00491 
00492         return gains_(idToIndex_.at(id), 0);
00493 }
00494 
00495 int GainCompensator::getIndex(int id) const
00496 {
00497         if(uContains(idToIndex_, id))
00498         {
00499                 return idToIndex_.at(id);
00500         }
00501         return -1;
00502 }
00503 
00504 } /* namespace rtabmap */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:20