← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/samples/cpp/tutorial_code/HighGUI/GDAL_IO/gdal-image.cpp

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/samples/cpp/tutorial_code/HighGUI/GDAL_IO/gdal-image.cpp
 
1
/*
 
2
 * gdal_image.cpp -- Load GIS data into OpenCV Containers using the Geospatial Data Abstraction Library
 
3
*/
 
4
 
 
5
// OpenCV Headers
 
6
#include "opencv2/core/core.hpp"
 
7
#include "opencv2/imgproc/imgproc.hpp"
 
8
#include "opencv2/highgui/highgui.hpp"
 
9
 
 
10
// C++ Standard Libraries
 
11
#include <cmath>
 
12
#include <iostream>
 
13
#include <stdexcept>
 
14
#include <vector>
 
15
 
 
16
using namespace std;
 
17
 
 
18
// define the corner points
 
19
//    Note that GDAL library can natively determine this
 
20
cv::Point2d tl( -122.441017, 37.815664 );
 
21
cv::Point2d tr( -122.370919, 37.815311 );
 
22
cv::Point2d bl( -122.441533, 37.747167 );
 
23
cv::Point2d br( -122.3715,   37.746814 );
 
24
 
 
25
// determine dem corners
 
26
cv::Point2d dem_bl( -122.0, 38);
 
27
cv::Point2d dem_tr( -123.0, 37);
 
28
 
 
29
// range of the heat map colors
 
30
std::vector<std::pair<cv::Vec3b,double> > color_range;
 
31
 
 
32
 
 
33
// List of all function prototypes
 
34
cv::Point2d lerp( const cv::Point2d&, const cv::Point2d&, const double& );
 
35
 
 
36
cv::Vec3b get_dem_color( const double& );
 
37
 
 
38
cv::Point2d world2dem( const cv::Point2d&, const cv::Size&);
 
39
 
 
40
cv::Point2d pixel2world( const int&, const int&, const cv::Size& );
 
41
 
 
42
void add_color( cv::Vec3b& pix, const uchar& b, const uchar& g, const uchar& r );
 
43
 
 
44
 
 
45
 
 
46
/*
 
47
 * Linear Interpolation
 
48
 * p1 - Point 1
 
49
 * p2 - Point 2
 
50
 * t  - Ratio from Point 1 to Point 2
 
51
*/
 
52
cv::Point2d lerp( cv::Point2d const& p1, cv::Point2d const& p2, const double& t ){
 
53
    return cv::Point2d( ((1-t)*p1.x) + (t*p2.x),
 
54
                        ((1-t)*p1.y) + (t*p2.y));
 
55
}
 
56
 
 
57
/*
 
58
 * Interpolate Colors
 
59
*/
 
60
template <typename DATATYPE, int N>
 
61
cv::Vec<DATATYPE,N> lerp( cv::Vec<DATATYPE,N> const& minColor,
 
62
                          cv::Vec<DATATYPE,N> const& maxColor,
 
63
                          double const& t ){
 
64
 
 
65
    cv::Vec<DATATYPE,N> output;
 
66
    for( int i=0; i<N; i++ ){
 
67
        output[i] = (uchar)(((1-t)*minColor[i]) + (t * maxColor[i]));
 
68
    }
 
69
    return output;
 
70
}
 
71
 
 
72
/*
 
73
 * Compute the dem color
 
74
*/
 
75
cv::Vec3b get_dem_color( const double& elevation ){
 
76
 
 
77
    // if the elevation is below the minimum, return the minimum
 
78
    if( elevation < color_range[0].second ){
 
79
        return color_range[0].first;
 
80
    }
 
81
    // if the elevation is above the maximum, return the maximum
 
82
    if( elevation > color_range.back().second ){
 
83
        return color_range.back().first;
 
84
    }
 
85
 
 
86
    // otherwise, find the proper starting index
 
87
    int idx=0;
 
88
    double t = 0;
 
89
    for( int x=0; x<(int)(color_range.size()-1); x++ ){
 
90
 
 
91
        // if the current elevation is below the next item, then use the current
 
92
        // two colors as our range
 
93
        if( elevation < color_range[x+1].second ){
 
94
            idx=x;
 
95
            t = (color_range[x+1].second - elevation)/
 
96
                (color_range[x+1].second - color_range[x].second);
 
97
 
 
98
            break;
 
99
        }
 
100
    }
 
101
 
 
102
    // interpolate the color
 
103
    return lerp( color_range[idx].first, color_range[idx+1].first, t);
 
104
}
 
105
 
 
106
/*
 
107
 * Given a pixel coordinate and the size of the input image, compute the pixel location
 
108
 * on the DEM image.
 
109
*/
 
110
cv::Point2d world2dem( cv::Point2d const& coordinate, const cv::Size& dem_size   ){
 
111
 
 
112
 
 
113
    // relate this to the dem points
 
114
    // ASSUMING THAT DEM DATA IS ORTHORECTIFIED
 
115
    double demRatioX = ((dem_tr.x - coordinate.x)/(dem_tr.x - dem_bl.x));
 
116
    double demRatioY = 1-((dem_tr.y - coordinate.y)/(dem_tr.y - dem_bl.y));
 
117
 
 
118
    cv::Point2d output;
 
119
    output.x = demRatioX * dem_size.width;
 
120
    output.y = demRatioY * dem_size.height;
 
121
 
 
122
    return output;
 
123
}
 
124
 
 
125
/*
 
126
 * Convert a pixel coordinate to world coordinates
 
127
*/
 
128
cv::Point2d pixel2world( const int& x, const int& y, const cv::Size& size ){
 
129
 
 
130
    // compute the ratio of the pixel location to its dimension
 
131
    double rx = (double)x / size.width;
 
132
    double ry = (double)y / size.height;
 
133
 
 
134
    // compute LERP of each coordinate
 
135
    cv::Point2d rightSide = lerp(tr, br, ry);
 
136
    cv::Point2d leftSide  = lerp(tl, bl, ry);
 
137
 
 
138
    // compute the actual Lat/Lon coordinate of the interpolated coordinate
 
139
    return lerp( leftSide, rightSide, rx );
 
140
}
 
141
 
 
142
/*
 
143
 * Add color to a specific pixel color value
 
144
*/
 
145
void add_color( cv::Vec3b& pix, const uchar& b, const uchar& g, const uchar& r ){
 
146
 
 
147
    if( pix[0] + b < 255 && pix[0] + b >= 0 ){ pix[0] += b; }
 
148
    if( pix[1] + g < 255 && pix[1] + g >= 0 ){ pix[1] += g; }
 
149
    if( pix[2] + r < 255 && pix[2] + r >= 0 ){ pix[2] += r; }
 
150
}
 
151
 
 
152
 
 
153
/*
 
154
 * Main Function
 
155
*/
 
156
int main( int argc, char* argv[] ){
 
157
 
 
158
    /*
 
159
     * Check input arguments
 
160
    */
 
161
    if( argc < 3 ){
 
162
        cout << "usage: " << argv[0] << " <image> <dem>" << endl;
 
163
        return 1;
 
164
    }
 
165
 
 
166
    // load the image (note that we don't have the projection information.  You will
 
167
    // need to load that yourself or use the full GDAL driver.  The values are pre-defined
 
168
    // at the top of this file
 
169
    cv::Mat image = cv::imread(argv[1], cv::IMREAD_LOAD_GDAL | cv::IMREAD_COLOR );
 
170
 
 
171
    // load the dem model
 
172
    cv::Mat dem = cv::imread(argv[2], cv::IMREAD_LOAD_GDAL | cv::IMREAD_ANYDEPTH );
 
173
 
 
174
    // create our output products
 
175
    cv::Mat output_dem(   image.size(), CV_8UC3 );
 
176
    cv::Mat output_dem_flood(   image.size(), CV_8UC3 );
 
177
 
 
178
    // for sanity sake, make sure GDAL Loads it as a signed short
 
179
    if( dem.type() != CV_16SC1 ){ throw std::runtime_error("DEM image type must be CV_16SC1"); }
 
180
 
 
181
    // define the color range to create our output DEM heat map
 
182
    //  Pair format ( Color, elevation );  Push from low to high
 
183
    //  Note:  This would be perfect for a configuration file, but is here for a working demo.
 
184
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 188, 154,  46),   -1));
 
185
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 110, 220, 110), 0.25));
 
186
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 150, 250, 230),   20));
 
187
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 160, 220, 200),   75));
 
188
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 220, 190, 170),  100));
 
189
    color_range.push_back( std::pair<cv::Vec3b,double>(cv::Vec3b( 250, 180, 140),  200));
 
190
 
 
191
    // define a minimum elevation
 
192
    double minElevation = -10;
 
193
 
 
194
    // iterate over each pixel in the image, computing the dem point
 
195
    for( int y=0; y<image.rows; y++ ){
 
196
    for( int x=0; x<image.cols; x++ ){
 
197
 
 
198
        // convert the pixel coordinate to lat/lon coordinates
 
199
        cv::Point2d coordinate = pixel2world( x, y, image.size() );
 
200
 
 
201
        // compute the dem image pixel coordinate from lat/lon
 
202
        cv::Point2d dem_coordinate = world2dem( coordinate, dem.size() );
 
203
 
 
204
        // extract the elevation
 
205
        double dz;
 
206
        if( dem_coordinate.x >=    0    && dem_coordinate.y >=    0     &&
 
207
            dem_coordinate.x < dem.cols && dem_coordinate.y < dem.rows ){
 
208
            dz = dem.at<short>(dem_coordinate);
 
209
        }else{
 
210
            dz = minElevation;
 
211
        }
 
212
 
 
213
        // write the pixel value to the file
 
214
        output_dem_flood.at<cv::Vec3b>(y,x) = image.at<cv::Vec3b>(y,x);
 
215
 
 
216
        // compute the color for the heat map output
 
217
        cv::Vec3b actualColor = get_dem_color(dz);
 
218
        output_dem.at<cv::Vec3b>(y,x) = actualColor;
 
219
 
 
220
        // show effect of a 10 meter increase in ocean levels
 
221
        if( dz < 10 ){
 
222
            add_color( output_dem_flood.at<cv::Vec3b>(y,x), 90, 0, 0 );
 
223
        }
 
224
        // show effect of a 50 meter increase in ocean levels
 
225
        else if( dz < 50 ){
 
226
            add_color( output_dem_flood.at<cv::Vec3b>(y,x), 0, 90, 0 );
 
227
        }
 
228
        // show effect of a 100 meter increase in ocean levels
 
229
        else if( dz < 100 ){
 
230
            add_color( output_dem_flood.at<cv::Vec3b>(y,x), 0, 0, 90 );
 
231
        }
 
232
 
 
233
    }}
 
234
 
 
235
    // print our heat map
 
236
    cv::imwrite( "heat-map.jpg"   ,  output_dem );
 
237
 
 
238
    // print the flooding effect image
 
239
    cv::imwrite( "flooded.jpg",  output_dem_flood);
 
240
 
 
241
    return 0;
 
242
}