thermalvis: cloudproc.cpp Source File

Go to the documentation of this file.
00001 
00005 #include "cloudproc.hpp"
00006 
00007 void findThermalCloudPercentiles(pcl::PointCloud<pcl::RGB>::Ptr& thermalCloud, double *vals, double *percentiles, unsigned int num) {
00008 
00009         // img -> pcl::PointCloud<pcl::RGB>::Ptr& thermalCloud
00010 
00011         cv::Mat mx;
00012         
00013         unsigned int validPoints = 0;
00014         
00015         for (unsigned int i = 0; i < thermalCloud->size(); i++) {
00016                 if (thermalCloud->points[i].b != 0) {
00017                         validPoints++;
00018                 }
00019         }
00020         
00021         //printf("%s << validPoints = (%d)\n", __FUNCTION__, validPoints);
00022         
00023         
00024         cv::Mat img = cv::Mat::zeros(1, validPoints, CV_16UC1);
00025         
00026         unsigned int validPointsIndex = 0;
00027         
00028         for (unsigned int i = 0; i < thermalCloud->size(); i++) {
00029                 if (thermalCloud->points[i].b != 0) {
00030                         img.at<unsigned short>(0,validPointsIndex) = 256*thermalCloud->points[i].r + thermalCloud->points[i].g;
00031                         //printf("%s << Assigning value to (%d) of = (%d)\n", __FUNCTION__, validPointsIndex, img.at<unsigned short>(0,validPointsIndex));
00032                         validPointsIndex++;
00033                         
00034                 }
00035         }
00036         
00037         //double currMin, currMax;
00038         //minMaxLoc(img, &currMin, &currMax);
00039         //printf("%s << min/max = (%f, %f)\n", __FUNCTION__, currMin, currMax);
00040         
00041         //printf("%s << Looking for (%f, %f) : (%d, %d)\n", __FUNCTION__, percentiles[0], percentiles[1], img.rows, img.cols);
00042         findPercentiles(img, vals, percentiles, num);
00043         
00044         for (unsigned int iii = 0; iii < num; iii++) {
00045                 //printf("%s << Correcting from [%d](%f)\n", __FUNCTION__, iii, vals[iii]);
00046                 vals[iii] = (vals[iii] - 1000.0) / 10.0;
00047                 
00048         }
00049         
00050 
00051         
00052 }
00053 
00054 void cCloudScheme::confidence_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& ioCloud, pcl::PointCloud<pcl::RGB>::Ptr& thermalCloud) {
00055         
00056         for (size_t i = 0; i < ioCloud->size (); ++i) {
00057                 ioCloud->points[i].b = thermalCloud->points[i].b;
00058                 ioCloud->points[i].g = thermalCloud->points[i].b;
00059                 ioCloud->points[i].r = thermalCloud->points[i].b;
00060         }
00061 }
00062 
00063 void cCloudScheme::falsify_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& ioCloud, pcl::PointCloud<pcl::RGB>::Ptr& thermalCloud, double minTemp, double maxTemp, int deadPointMode, double volumeSize, double minConfidence, bool clampOutliers) {
00064 
00065         //printf("%s << %f -> %f; (%d)\n", __FUNCTION__, minConfidence, (minConfidence * 255.0), clampOutliers);
00066 
00067         double currentTemp;
00068         int lookupIndex = 0;
00069         
00070         //printf("%s << %f -> %f\n", __FUNCTION__, minConfidence, (minConfidence * 255.0));
00071 
00072         for (size_t i = 0; i < ioCloud->size (); ++i) {
00073                 
00074                 //if (thermalCloud->points[i].b != 0) { printf("%s << (%d) (%f)\n", __FUNCTION__, thermalCloud->points[i].b, double(thermalCloud->points[i].b)); }
00075                 
00076                 
00077                 
00078                 currentTemp = ((double(thermalCloud->points[i].r)*256.0 + double(thermalCloud->points[i].g)) - 1000.0) / 10.0;
00079                 
00080                 bool thermalValid = (double(thermalCloud->points[i].b) >= (minConfidence * 255.0)) && !(!clampOutliers && ( (currentTemp < minTemp) || (currentTemp > maxTemp) ) );
00081                 
00082                 
00083                 if (!thermalValid) {
00084                         
00085                         // printf("%s << Dead point.\n", __FUNCTION__);
00086                                         
00087                         if (deadPointMode == DPM_GRAY) {
00088                                 ioCloud->points[i].b = 128;
00089                                 ioCloud->points[i].g = 128;
00090                                 ioCloud->points[i].r = 128;
00091                         } else if (deadPointMode == DPM_AXES) {
00092                                 ioCloud->points[i].b = (unsigned char) (255.0 * (ioCloud->points[i].x / volumeSize));
00093                                 ioCloud->points[i].g = (unsigned char) (255.0 * (ioCloud->points[i].y / volumeSize));
00094                                 ioCloud->points[i].r = (unsigned char) (255.0 * (ioCloud->points[i].z / volumeSize));
00095                         }
00096                 } else {
00097                         
00098                         // use ( clampOutliers ) to decide what to do with temps outside the range
00099                         // currentTemp = max(minTemp, min(maxTemp, ((double(thermalCloud->points[i].r)*256.0 + double(thermalCloud->points[i].g)) - 1000.0) / 10.0));
00100                         
00101                         currentTemp = max(minTemp, min(maxTemp, currentTemp));
00102                         
00103                         // must convert temp to look up index using min/max
00104                         
00105                         lookupIndex = 255.0 * ((currentTemp - minTemp) / (maxTemp - minTemp));
00106                         
00107                         if ((lookupIndex < 0) || (lookupIndex >= 65536)) {
00108                                 
00109                                 if (deadPointMode == DPM_GRAY) {
00110                                 ioCloud->points[i].b = 128;
00111                                 ioCloud->points[i].g = 128;
00112                                 ioCloud->points[i].r = 128;
00113                         } else if (deadPointMode == DPM_AXES) {
00114                                 ioCloud->points[i].b = (unsigned char) (255.0 * (ioCloud->points[i].x / volumeSize));
00115                                 ioCloud->points[i].g = (unsigned char) (255.0 * (ioCloud->points[i].y / volumeSize));
00116                                 ioCloud->points[i].r = (unsigned char) (255.0 * (ioCloud->points[i].z / volumeSize));
00117                         }
00118                         
00119                         continue;
00120                                 //printf("%s << currentTemp = (%f); lookupIndex = (%d)\n", __FUNCTION__, currentTemp, lookupIndex);
00121                         } 
00122                         
00123                         //if (lookupIndex == 0) {
00124                         //      printf("%s << currentTemp = (%f); lookupIndex = (%d)\n", __FUNCTION__, currentTemp, lookupIndex);
00125                         //}
00126                         
00127                         ioCloud->points[i].b = lookupTable_1[lookupIndex][0];
00128                         ioCloud->points[i].g = lookupTable_1[lookupIndex][1];
00129                         ioCloud->points[i].r = lookupTable_1[lookupIndex][2];
00130                         
00131                         
00132                         /*
00133                         ioCloud->points[i].b = lookupIndex;
00134                         ioCloud->points[i].g = 0;
00135                         ioCloud->points[i].r = 0;
00136                         */
00137                 }
00138                 
00139         }
00140 
00141 }
00142 
00143 void cCloudScheme::falsify_cloud(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& ioCloud, pcl::PointCloud<pcl::RGB>::Ptr& thermalCloud, double minTemp, double maxTemp, int deadPointMode, double volumeSize, double minConfidence, bool clampOutliers) {
00144 
00145         double currentTemp;
00146         int lookupIndex = 0;
00147         
00148         
00149 
00150         for (size_t i = 0; i < ioCloud->size (); ++i) {
00151                 
00152                 //if (thermalCloud->points[i].b != 0) { printf("%s << (%d) (%f)\n", __FUNCTION__, thermalCloud->points[i].b, double(thermalCloud->points[i].b)); }
00153                 
00154                 
00155                 
00156                 currentTemp = ((double(thermalCloud->points[i].r)*256.0 + double(thermalCloud->points[i].g)) - 1000.0) / 10.0;
00157                 
00158                 bool thermalValid = (double(thermalCloud->points[i].b) >= (minConfidence * 255.0)) && !(!clampOutliers && ( (currentTemp < minTemp) || (currentTemp > maxTemp) ) );
00159                 
00160                 
00161                 if (!thermalValid) {
00162                         
00163                         // printf("%s << Dead point.\n", __FUNCTION__);
00164                                         
00165                         if (deadPointMode == DPM_GRAY) {
00166                                 ioCloud->points[i].b = 128;
00167                                 ioCloud->points[i].g = 128;
00168                                 ioCloud->points[i].r = 128;
00169                         } else if (deadPointMode == DPM_AXES) {
00170                                 ioCloud->points[i].b = (unsigned char) (255.0 * (ioCloud->points[i].x / volumeSize));
00171                                 ioCloud->points[i].g = (unsigned char) (255.0 * (ioCloud->points[i].y / volumeSize));
00172                                 ioCloud->points[i].r = (unsigned char) (255.0 * (ioCloud->points[i].z / volumeSize));
00173                         }
00174                 } else {
00175                         
00176                         // use ( clampOutliers ) to decide what to do with temps outside the range
00177                         // currentTemp = max(minTemp, min(maxTemp, ((double(thermalCloud->points[i].r)*256.0 + double(thermalCloud->points[i].g)) - 1000.0) / 10.0));
00178                         
00179                         currentTemp = max(minTemp, min(maxTemp, currentTemp));
00180                         
00181                         // must convert temp to look up index using min/max
00182                         
00183                         lookupIndex = 255.0 * ((currentTemp - minTemp) / (maxTemp - minTemp));
00184                         
00185                         
00186                         //if (lookupIndex == 0) {
00187                         //      printf("%s << currentTemp = (%f); lookupIndex = (%d)\n", __FUNCTION__, currentTemp, lookupIndex);
00188                         //}
00189                         
00190                         ioCloud->points[i].b = lookupTable_1[lookupIndex][0];
00191                         ioCloud->points[i].g = lookupTable_1[lookupIndex][1];
00192                         ioCloud->points[i].r = lookupTable_1[lookupIndex][2];
00193                         
00194                         
00195                         /*
00196                         ioCloud->points[i].b = lookupIndex;
00197                         ioCloud->points[i].g = 0;
00198                         ioCloud->points[i].r = 0;
00199                         */
00200                 }
00201                 
00202         }
00203 
00204 }
00205 
00206 
00207 void cCloudScheme::fuse_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& ioCloud, pcl::PointCloud<pcl::RGB>::Ptr& colorCloud, pcl::PointCloud<pcl::RGB>::Ptr& thermalCloud, double minTemp, double maxTemp, int deadPointMode, double volumeSize, double minConfidence, bool clampOutliers, double *params) {
00208         
00209         double currentTemp, visibleVal, lumChange;
00210         int lookupIndex = 0;
00211         
00212         double working_params[2];
00213         
00214         if (params == NULL) {
00215                 working_params[0] = DEFAULT_LOWER_VISIBLE_FUSION_LIMIT;
00216                 working_params[1] = DEFAULT_UPPER_VISIBLE_FUSION_LIMIT;
00217         } else {
00218                 working_params[0] = params[0];
00219                 working_params[1] = params[1];
00220         }
00221 
00222         for (size_t i = 0; i < ioCloud->size (); ++i) {
00223                 
00224                 visibleVal = 0.0;
00225                                 
00226                 /*
00227                 visibleVal += 0.299 * (((double) colorCloud->points[i].r) * working_params[0] + 255.0 * ((1.0 - working_params[1])/2.0));
00228                 visibleVal += 0.587 * (((double) colorCloud->points[i].g) * working_params[0] + 255.0 * ((1.0 - working_params[1])/2.0));
00229                 visibleVal += 0.114 * (((double) colorCloud->points[i].b) * working_params[0] + 255.0 * ((1.0 - working_params[1])/2.0));
00230                 lumChange = 2.0 * (visibleVal/255.0 - 0.5);
00231                 */
00232                 
00233                 visibleVal += 0.299 * ((double) colorCloud->points[i].r);
00234                 visibleVal += 0.587 * ((double) colorCloud->points[i].g);
00235                 visibleVal += 0.114 * ((double) colorCloud->points[i].b);
00236                 lumChange = 2.0 * ((visibleVal / 255.0) - 0.5) * (working_params[1] - working_params[0]);
00237                 
00238                 
00239                 
00240                 bool visibleValid = (colorCloud->points[i].r != 0) || (colorCloud->points[i].g != 0) || (colorCloud->points[i].b != 0);
00241                 
00242                 currentTemp = ((double(thermalCloud->points[i].r)*256.0 + double(thermalCloud->points[i].g)) - 1000.0) / 10.0;
00243                 
00244                 bool thermalValid = (double(thermalCloud->points[i].b) >= (minConfidence * 255.0)) && !(!clampOutliers && ( (currentTemp < minTemp) || (currentTemp > maxTemp) ) );
00245                 
00246                 currentTemp = max(minTemp, min(maxTemp, currentTemp));
00247                 lookupIndex = 255.0 * ((currentTemp - minTemp) / (maxTemp - minTemp));
00248                 
00249                 // If thermal (whether or not RGB is valid)
00250                 if (thermalValid) {
00251                         ioCloud->points[i].b = lookupTable_1[lookupIndex][0];
00252                         ioCloud->points[i].g = lookupTable_1[lookupIndex][1];
00253                         ioCloud->points[i].r = lookupTable_1[lookupIndex][2];
00254                 }
00255                 
00256                 // Both thermal and RGB
00257                 if (visibleValid && thermalValid) {                     
00258                         
00259                         if (lumChange > 0) {
00260                                 ioCloud->points[i].b = ioCloud->points[i].b + (255.0 - ((double)ioCloud->points[i].b)) * lumChange;
00261                                 ioCloud->points[i].g = ioCloud->points[i].g + (255.0 - ((double)ioCloud->points[i].g)) * lumChange;
00262                                 ioCloud->points[i].r = ioCloud->points[i].r + (255.0 - ((double)ioCloud->points[i].r)) * lumChange;
00263                         }
00264                         else {
00265                                 ioCloud->points[i].b = ioCloud->points[i].b + ((double)ioCloud->points[i].b) * lumChange;
00266                                 ioCloud->points[i].g = ioCloud->points[i].g + ((double)ioCloud->points[i].g) * lumChange;
00267                                 ioCloud->points[i].r = ioCloud->points[i].r + ((double)ioCloud->points[i].r) * lumChange;
00268                         }
00269                 }
00270                 // RGB only (grayscale RGB)
00271                 else if (visibleValid) {
00272                         ioCloud->points[i].b = colorCloud->points[i].r;
00273                         ioCloud->points[i].g = colorCloud->points[i].g;
00274                         ioCloud->points[i].r = colorCloud->points[i].b;
00275                         
00276                 }
00277                 // Neither (Saturation 0%, Lightness 50%)
00278                 else if (!visibleValid & !thermalValid) {
00279                         
00280                         if (deadPointMode == DPM_GRAY) {
00281                                 ioCloud->points[i].b = 128;
00282                                 ioCloud->points[i].g = 128;
00283                                 ioCloud->points[i].r = 128;
00284                         } else if (deadPointMode == DPM_AXES) {
00285                                 ioCloud->points[i].b = (unsigned char) (255.0 * (ioCloud->points[i].x / volumeSize));
00286                                 ioCloud->points[i].g = (unsigned char) (255.0 * (ioCloud->points[i].y / volumeSize));
00287                                 ioCloud->points[i].r = (unsigned char) (255.0 * (ioCloud->points[i].z / volumeSize));
00288                         }
00289                         
00290                         
00291                 }
00292 
00293         }
00294         
00295 }
00296 
00297 void cCloudScheme::fuse_cloud(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& ioCloud, pcl::PointCloud<pcl::RGB>::Ptr& colorCloud, pcl::PointCloud<pcl::RGB>::Ptr& thermalCloud, double minTemp, double maxTemp, int deadPointMode, double volumeSize, double minConfidence, bool clampOutliers, double *params) {
00298         
00299         double currentTemp, visibleVal, lumChange;
00300         int lookupIndex = 0;
00301         
00302         double working_params[2];
00303         
00304         if (params == NULL) {
00305                 working_params[0] = DEFAULT_LOWER_VISIBLE_FUSION_LIMIT;
00306                 working_params[1] = DEFAULT_UPPER_VISIBLE_FUSION_LIMIT;
00307         } else {
00308                 working_params[0] = params[0];
00309                 working_params[1] = params[1];
00310         }
00311 
00312         for (size_t i = 0; i < ioCloud->size (); ++i) {
00313                 
00314                 visibleVal = 0.0;
00315                                 
00316                 /*
00317                 visibleVal += 0.299 * (((double) colorCloud->points[i].r) * working_params[0] + 255.0 * ((1.0 - working_params[1])/2.0));
00318                 visibleVal += 0.587 * (((double) colorCloud->points[i].g) * working_params[0] + 255.0 * ((1.0 - working_params[1])/2.0));
00319                 visibleVal += 0.114 * (((double) colorCloud->points[i].b) * working_params[0] + 255.0 * ((1.0 - working_params[1])/2.0));
00320                 lumChange = 2.0 * (visibleVal/255.0 - 0.5);
00321                 */
00322                 
00323                 visibleVal += 0.299 * ((double) colorCloud->points[i].r);
00324                 visibleVal += 0.587 * ((double) colorCloud->points[i].g);
00325                 visibleVal += 0.114 * ((double) colorCloud->points[i].b);
00326                 lumChange = 2.0 * ((visibleVal / 255.0) - 0.5) * (working_params[1] - working_params[0]);
00327                 
00328                 
00329                 
00330                 bool visibleValid = (colorCloud->points[i].r != 0) || (colorCloud->points[i].g != 0) || (colorCloud->points[i].b != 0);
00331                 
00332                 currentTemp = ((double(thermalCloud->points[i].r)*256.0 + double(thermalCloud->points[i].g)) - 1000.0) / 10.0;
00333                 
00334                 bool thermalValid = (double(thermalCloud->points[i].b) >= (minConfidence * 255.0)) && !(!clampOutliers && ( (currentTemp < minTemp) || (currentTemp > maxTemp) ) );
00335                 
00336                 currentTemp = max(minTemp, min(maxTemp, currentTemp));
00337                 lookupIndex = 255.0 * ((currentTemp - minTemp) / (maxTemp - minTemp));
00338                 
00339                 // If thermal (whether or not RGB is valid)
00340                 if (thermalValid) {
00341                         ioCloud->points[i].b = lookupTable_1[lookupIndex][0];
00342                         ioCloud->points[i].g = lookupTable_1[lookupIndex][1];
00343                         ioCloud->points[i].r = lookupTable_1[lookupIndex][2];
00344                 }
00345                 
00346                 // Both thermal and RGB
00347                 if (visibleValid && thermalValid) {                     
00348                         
00349                         if (lumChange > 0) {
00350                                 ioCloud->points[i].b = ioCloud->points[i].b + (255.0 - ((double)ioCloud->points[i].b)) * lumChange;
00351                                 ioCloud->points[i].g = ioCloud->points[i].g + (255.0 - ((double)ioCloud->points[i].g)) * lumChange;
00352                                 ioCloud->points[i].r = ioCloud->points[i].r + (255.0 - ((double)ioCloud->points[i].r)) * lumChange;
00353                         }
00354                         else {
00355                                 ioCloud->points[i].b = ioCloud->points[i].b + ((double)ioCloud->points[i].b) * lumChange;
00356                                 ioCloud->points[i].g = ioCloud->points[i].g + ((double)ioCloud->points[i].g) * lumChange;
00357                                 ioCloud->points[i].r = ioCloud->points[i].r + ((double)ioCloud->points[i].r) * lumChange;
00358                         }
00359                 }
00360                 // RGB only (grayscale RGB)
00361                 else if (visibleValid) {
00362                         ioCloud->points[i].b = colorCloud->points[i].r;
00363                         ioCloud->points[i].g = colorCloud->points[i].g;
00364                         ioCloud->points[i].r = colorCloud->points[i].b;
00365                         
00366                 }
00367                 // Neither (Saturation 0%, Lightness 50%)
00368                 else if (!visibleValid & !thermalValid) {
00369                         
00370                         if (deadPointMode == DPM_GRAY) {
00371                                 ioCloud->points[i].b = 128;
00372                                 ioCloud->points[i].g = 128;
00373                                 ioCloud->points[i].r = 128;
00374                         } else if (deadPointMode == DPM_AXES) {
00375                                 ioCloud->points[i].b = (unsigned char) (255.0 * (ioCloud->points[i].x / volumeSize));
00376                                 ioCloud->points[i].g = (unsigned char) (255.0 * (ioCloud->points[i].y / volumeSize));
00377                                 ioCloud->points[i].r = (unsigned char) (255.0 * (ioCloud->points[i].z / volumeSize));
00378                         }
00379                         
00380                         
00381                 }
00382 
00383         }
00384         
00385 }