CFeatureExtraction_spinImg.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "vision-precomp.h"  // Precompiled headers

#include <mrpt/math/ops_matrices.h>
#include <mrpt/vision/CFeatureExtraction.h>
#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::img;
using namespace mrpt::system;
using namespace mrpt::math;
using namespace std;

void CFeatureExtraction::internal_computeSpinImageDescriptors(
    const CImage& in_img, CFeatureList& in_features)
{
    MRPT_START
    mrpt::system::CTimeLoggerEntry tle(
        profiler, "internal_computeSpinImageDescriptors");

    ASSERT_(options.SpinImagesOptions.radius > 1);

    // This is a C++ implementation of the descriptor "Intensity-domain spin
    // images" as described
    //  in "A sparse texture representation using affine-invariant regions", S
    //  Lazebnik, C Schmid, J Ponce,
    //  2003 IEEE Computer Society Conference on Computer Vision.

    const unsigned int HIST_N_INT =
        options.SpinImagesOptions.hist_size_intensity;
    const unsigned int HIST_N_DIS =
        options.SpinImagesOptions.hist_size_distance;
    const unsigned int R = options.SpinImagesOptions.radius;
    const int img_w = static_cast<int>(in_img.getWidth());
    const int img_h = static_cast<int>(in_img.getHeight());
    const bool img_color = in_img.isColor();

    // constant for passing intensity [0,255] to histogram index
    // [0,HIST_N_INT-1]:
    const float k_int2idx = (HIST_N_INT - 1) / 255.0f;
    const float k_idx2int = 1.0f / k_int2idx;

    // constant for passing distances in pixels [0,R] to histogram index
    // [0,HIST_N_DIS-1]:
    const float k_dis2idx = (HIST_N_DIS - 1) / static_cast<float>(R);
    const float k_idx2dis = 1.0f / k_dis2idx;

    // The Gaussian kernel used below is approximated up to a given distance
    //  in pixels, given by 2 times the appropriate std. deviations:
    const int STD_TIMES = 2;
    const int kernel_size_dist = static_cast<int>(
        ceil(k_dis2idx * STD_TIMES * options.SpinImagesOptions.std_dist));

    const float _2var_int =
        -1.0f / (2 * square(options.SpinImagesOptions.std_intensity));
    const float _2var_dist =
        -1.0f / (2 * square(options.SpinImagesOptions.std_dist));

    // Create the 2D histogram:
    CMatrixDouble hist2d(HIST_N_INT, HIST_N_DIS);

    // Compute intensity-domain spin images
    for (auto& in_feature : in_features)
    {
        // Overwrite scale with the descriptor scale:
        in_feature.keypoint.octave = options.SpinImagesOptions.radius;

        // Reset histogram to zeros:
        hist2d.setZero();

        // Define the ROI around the interest point which counts for the
        // histogram:
        int px0 = round(in_feature.keypoint.pt.x - R);
        int px1 = round(in_feature.keypoint.pt.x + R);
        int py0 = round(in_feature.keypoint.pt.y - R);
        int py1 = round(in_feature.keypoint.pt.y + R);

        // Clip at img borders:
        px0 = max(0, px0);
        px1 = min(img_w - 1, px1);
        py0 = max(0, py0);
        py1 = min(img_h - 1, py1);

        for (int px = px0; px <= px1; px++)
        {
            for (int py = py0; py <= py1; py++)
            {
                uint8_t pix_val;
                // get the pixel color [0,255]
                if (!img_color)
                    pix_val = in_img.at<uint8_t>(px, py);
                else
                {
                    auto aux_pix_ptr = in_img.ptr<uint8_t>(px, py);
                    pix_val =
                        (aux_pix_ptr[0] + aux_pix_ptr[1] + aux_pix_ptr[2]) / 3;
                }

                const float pix_dist = hypot(
                    in_feature.keypoint.pt.x - px,
                    in_feature.keypoint.pt.y - py);
                const int center_bin_dist = k_dis2idx * pix_dist;

                // A factor to correct the histogram due to the existence of
                // more pixels at larger radius:
                //  Obtained as Area = PI ( R1^2 - R2^2 ) for R1,R2 being
                //  pix_dist +- Delta/2
                // const double density_circular_area = 1.0/ (M_2PI *
                // (center_bin_dist+0.5) * square(k_idx2dis) );

                // Apply a "soft-histogram", so each pixel counts into several
                // bins,
                //  weighted by a exponential function to model a Gaussian
                //  kernel
                const int bin_int_low =
                    max(0, static_cast<int>(ceil(
                               k_int2idx *
                               (pix_val -
                                STD_TIMES *
                                    options.SpinImagesOptions.std_intensity))));
                const int bin_int_hi =
                    min(static_cast<int>(HIST_N_INT - 1),
                        static_cast<int>(ceil(
                            k_int2idx *
                            (pix_val +
                             STD_TIMES *
                                 options.SpinImagesOptions.std_intensity))));

                // cout << "d: " << pix_dist << "v: " << (int)pix_val << "\t";

                if (center_bin_dist <
                    static_cast<int>(HIST_N_DIS))  // this accounts for the
                // "square" or "circle"
                // shapes of the area to
                // account for
                {
                    const int bin_dist_low =
                        max(0, center_bin_dist - kernel_size_dist);
                    const int bin_dist_hi =
                        min(static_cast<int>(HIST_N_DIS - 1),
                            center_bin_dist + kernel_size_dist);

                    int bin_dist, bin_int;
                    float pix_dist_cur_dist =
                        pix_dist - bin_dist_low * k_idx2dis;

                    for (bin_dist = bin_dist_low; bin_dist <= bin_dist_hi;
                         bin_dist++, pix_dist_cur_dist -= k_idx2dis)
                    {
                        float pix_val_cur_val =
                            pix_val - (bin_int_low * k_idx2int);

                        for (bin_int = bin_int_low; bin_int <= bin_int_hi;
                             bin_int++, pix_val_cur_val -= k_idx2int)
                        {
                            // Gaussian kernel:
                            double v = _2var_dist * square(pix_dist_cur_dist) +
                                       _2var_int * square(pix_val_cur_val);

                            hist2d(bin_int, bin_dist) += exp(v);
                        }
                    }
                    // hist2d(bin_int,bin_dist) *= ;
                }  // in range

            }  // end py
        }  // end px

        // Normalize [0,1]
        mrpt::math::normalize(hist2d, 0, 1);

        // Save the histogram as a vector:
        unsigned idx = 0;
        std::vector<float>& ptr_trg = *in_feature.descriptors.SpinImg;
        ptr_trg.resize(HIST_N_INT * HIST_N_DIS);

        for (unsigned i = 0; i < HIST_N_DIS; i++)
            for (unsigned j = 0; j < HIST_N_INT; j++)
                ptr_trg[idx++] = hist2d(j, i);

        in_feature.descriptors.SpinImg_range_rows = HIST_N_DIS;

    }  // end for each feature

    MRPT_END
}