├── .gitignore ├── example ├── bbx.json ├── metas.json ├── results.txt └── tracks.txt ├── multi_rpc_triangulate ├── CMakeLists.txt └── multi_rpc_triangulate.cpp ├── readme.md ├── rpc_model.py └── triangulate.py /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | -------------------------------------------------------------------------------- /example/bbx.json: -------------------------------------------------------------------------------- 1 | { 2 | "lat_min": -34.492935318092016, 3 | "lat_max": -34.486961658161164, 4 | "lon_min": -58.58957804544399, 5 | "lon_max": -58.58170443530272, 6 | "alt_min": -30.0, 7 | "alt_max": 120.0 8 | } -------------------------------------------------------------------------------- /example/metas.json: -------------------------------------------------------------------------------- 1 | { 2 | "0000.png": { 3 | "rpc": { 4 | "rowNum": [ 5 | -0.01155039, 6 | -0.02219071, 7 | 1.037603, 8 | -0.0143093, 9 | 0.001430638, 10 | -4.002035e-05, 11 | 0.0006988066, 12 | -0.0002803232, 13 | 0.008815574, 14 | -9.492739e-06, 15 | 1.691069e-06, 16 | 7.474433e-07, 17 | 1.716209e-05, 18 | 1.368658e-06, 19 | -5.536113e-05, 20 | -0.0001669293, 21 | -6.567348e-05, 22 | 4.828972e-07, 23 | 2.179128e-05, 24 | 9.029418e-07 25 | ], 26 | "rowDen": [ 27 | 1.0, 28 | -0.001748998, 29 | 0.002867378, 30 | -0.000891891, 31 | -1.1009e-05, 32 | -3.130936e-06, 33 | -1.505779e-05, 34 | -4.256269e-05, 35 | 0.0001791118, 36 | -6.411448e-05, 37 | -1.480182e-07, 38 | 1.884002e-07, 39 | 2.299439e-06, 40 | 2.065444e-07, 41 | 1.937789e-07, 42 | 6.022982e-05, 43 | 3.128753e-07, 44 | 9.631539e-08, 45 | -1.065953e-06, 46 | 1.06707e-07 47 | ], 48 | "colNum": [ 49 | 0.003072148, 50 | -1.015328, 51 | 0.0001564052, 52 | 0.007921286, 53 | -0.003215768, 54 | -0.0003729291, 55 | -0.0002210835, 56 | -0.002129823, 57 | 1.602836e-05, 58 | 5.707117e-06, 59 | -9.895511e-06, 60 | 3.067685e-05, 61 | 0.0001623712, 62 | 2.474425e-05, 63 | -1.358906e-05, 64 | -0.0002529502, 65 | -5.387624e-07, 66 | -1.077882e-06, 67 | 7.690791e-06, 68 | -1.855927e-07 69 | ], 70 | "colDen": [ 71 | 1.0, 72 | 0.0009596563, 73 | -0.003263896, 74 | -0.0004033469, 75 | 9.097765e-06, 76 | -1.20064e-06, 77 | -9.51889e-06, 78 | -2.531124e-05, 79 | 0.0001419847, 80 | -2.445755e-05, 81 | -9.716162e-08, 82 | 4.391155e-08, 83 | 2.657253e-06, 84 | 2.810917e-08, 85 | 1.262118e-07, 86 | 3.400646e-06, 87 | 1.128371e-07, 88 | 2.805515e-08, 89 | 8.26855e-07, 90 | 1.966835e-08 91 | ], 92 | "rowOff": 4124.0, 93 | "rowScale": 14360.0, 94 | "colOff": 5291.0, 95 | "colScale": 13617.0, 96 | "latOff": -34.4754, 97 | "latScale": 0.0688, 98 | "lonOff": -58.611, 99 | "lonScale": 0.0758, 100 | "altOff": 31.0, 101 | "altScale": 500.0 102 | }, 103 | "width": 1430, 104 | "height": 1390 105 | }, 106 | "0001.png": { 107 | "rpc": { 108 | "rowNum": [ 109 | -0.0005215006, 110 | -0.00246106, 111 | -1.012953, 112 | 0.00957596, 113 | -3.681847e-05, 114 | 3.964446e-07, 115 | 2.197551e-05, 116 | 0.0002731655, 117 | 0.0003501338, 118 | -9.337211e-07, 119 | 0.0, 120 | 0.0, 121 | 2.233399e-06, 122 | 0.0, 123 | -2.992915e-06, 124 | -1.098616e-05, 125 | -3.292832e-06, 126 | 2.094706e-08, 127 | 6.692392e-08, 128 | 3.184292e-08 129 | ], 130 | "rowDen": [ 131 | 1.0, 132 | -3.754527e-05, 133 | -0.0001619289, 134 | 2.719147e-05, 135 | 1.134559e-06, 136 | -4.515624e-08, 137 | 1.875883e-07, 138 | 2.059601e-06, 139 | -4.100731e-06, 140 | 3.245604e-06, 141 | 2.351355e-08, 142 | 0.0, 143 | -1.188876e-06, 144 | 0.0, 145 | 3.296711e-08, 146 | -2.891807e-05, 147 | 0.0, 148 | 0.0, 149 | 8.338245e-07, 150 | 0.0 151 | ], 152 | "colNum": [ 153 | 0.001157397, 154 | 1.003802, 155 | 7.515602e-06, 156 | 0.00169916, 157 | 0.0004304021, 158 | 0.0004354668, 159 | -0.0002122753, 160 | -0.0007096324, 161 | -0.0007261066, 162 | 2.682025e-06, 163 | 1.869307e-07, 164 | -5.06571e-06, 165 | -5.122405e-06, 166 | -2.260461e-06, 167 | 3.702445e-05, 168 | 0.0002526328, 169 | -1.293693e-08, 170 | -5.333612e-07, 171 | -6.681183e-06, 172 | 0.0 173 | ], 174 | "colDen": [ 175 | 1.0, 176 | -0.0004503515, 177 | -0.0004313161, 178 | -0.0004390822, 179 | 3.77587e-06, 180 | 4.554034e-07, 181 | -2.82664e-07, 182 | 6.703623e-07, 183 | 2.370933e-06, 184 | -2.56702e-06, 185 | -4.920662e-08, 186 | 0.0, 187 | 2.35854e-07, 188 | 0.0, 189 | 3.93288e-08, 190 | 6.575355e-08, 191 | 0.0, 192 | 0.0, 193 | 1.376104e-08, 194 | 0.0 195 | ], 196 | "rowOff": -4759.0, 197 | "rowScale": 22430.0, 198 | "colOff": -5634.0, 199 | "colScale": 21250.0, 200 | "latOff": -34.4731, 201 | "latScale": 0.0654, 202 | "lonOff": -58.6088, 203 | "lonScale": 0.0734, 204 | "altOff": 31.0, 205 | "altScale": 500.0 206 | }, 207 | "width": 2238, 208 | "height": 2146 209 | }, 210 | "0002.png": { 211 | "rpc": { 212 | "rowNum": [ 213 | 0.004881554, 214 | 0.004118263, 215 | 1.008025, 216 | 0.003332543, 217 | 0.001004237, 218 | 7.043565e-06, 219 | 0.0008877913, 220 | -0.0003650401, 221 | -0.002762487, 222 | 2.471877e-06, 223 | 1.36154e-06, 224 | -5.583681e-07, 225 | -2.941552e-05, 226 | -2.169115e-07, 227 | -4.567445e-05, 228 | -0.0002027093, 229 | -5.507408e-05, 230 | -4.721923e-07, 231 | -6.037733e-06, 232 | -1.815503e-07 233 | ], 234 | "rowDen": [ 235 | 1.0, 236 | -0.001029776, 237 | -0.002214435, 238 | -0.0009107737, 239 | 1.76976e-05, 240 | -2.020627e-06, 241 | 3.104974e-06, 242 | -3.579727e-05, 243 | 0.0001254937, 244 | -5.609755e-05, 245 | 1.998184e-07, 246 | 8.472848e-08, 247 | 3.731752e-06, 248 | 1.087777e-07, 249 | -1.638358e-07, 250 | 0.0002653348, 251 | -1.617888e-08, 252 | 9.004217e-08, 253 | 3.886523e-06, 254 | 9.828119e-08 255 | ], 256 | "colNum": [ 257 | 0.003033693, 258 | -1.008621, 259 | 0.0002754154, 260 | 0.006653833, 261 | -6.467854e-05, 262 | -0.0004177845, 263 | -0.0002410013, 264 | -0.002031984, 265 | -0.0001757272, 266 | 1.499041e-06, 267 | 4.029234e-07, 268 | 4.232789e-05, 269 | 0.000174208, 270 | 3.260106e-05, 271 | 1.302525e-05, 272 | -0.0002129977, 273 | -5.311378e-07, 274 | -1.408533e-06, 275 | -3.221355e-06, 276 | -2.140649e-07 277 | ], 278 | "colDen": [ 279 | 1.0, 280 | 0.001012835, 281 | -6.437918e-05, 282 | -0.0004416834, 283 | 2.492265e-05, 284 | -8.044819e-07, 285 | 1.334667e-06, 286 | -3.806435e-05, 287 | 0.0001416828, 288 | -3.262402e-05, 289 | 1.247399e-07, 290 | 3.874027e-08, 291 | 1.905747e-06, 292 | 3.121852e-08, 293 | -6.198464e-08, 294 | -2.434802e-06, 295 | 0.0, 296 | 3.715774e-08, 297 | 1.126273e-06, 298 | 2.806686e-08 299 | ], 300 | "rowOff": 6950.0, 301 | "rowScale": 22682.0, 302 | "colOff": 7793.0, 303 | "colScale": 21250.0, 304 | "latOff": -34.4728, 305 | "latScale": 0.0651, 306 | "lonOff": -58.6088, 307 | "lonScale": 0.0738, 308 | "altOff": 31.0, 309 | "altScale": 501.0 310 | }, 311 | "width": 2265, 312 | "height": 2129 313 | }, 314 | "0003.png": { 315 | "rpc": { 316 | "rowNum": [ 317 | -1.733015e-05, 318 | -0.0294956, 319 | 1.034711, 320 | -0.004316195, 321 | -0.0001791221, 322 | 2.606759e-07, 323 | 1.42591e-05, 324 | -0.0004309942, 325 | -6.98447e-05, 326 | 2.447565e-07, 327 | 4.459035e-07, 328 | -3.077291e-07, 329 | -3.431228e-06, 330 | -5.173902e-07, 331 | 1.327066e-05, 332 | 6.277082e-05, 333 | 1.813037e-05, 334 | -7.354437e-08, 335 | -2.609415e-07, 336 | -7.519989e-08 337 | ], 338 | "rowDen": [ 339 | 1.0, 340 | 0.0001735415, 341 | 2.964654e-06, 342 | -1.468645e-05, 343 | 4.742401e-06, 344 | 4.174773e-07, 345 | 6.290509e-07, 346 | 1.213321e-05, 347 | -5.528016e-05, 348 | 1.752562e-05, 349 | 2.402698e-07, 350 | 0.0, 351 | -2.964147e-05, 352 | 0.0, 353 | 1.007585e-06, 354 | 0.0003457741, 355 | 2.425708e-08, 356 | 0.0, 357 | -4.192093e-06, 358 | 0.0 359 | ], 360 | "colNum": [ 361 | 0.005592772, 362 | -1.030261, 363 | 0.001303112, 364 | 0.02798617, 365 | 0.0007866897, 366 | -0.0002365756, 367 | 0.000177299, 368 | -0.005481605, 369 | -0.0002242166, 370 | 7.190978e-06, 371 | 1.591482e-06, 372 | -2.309644e-05, 373 | -6.050273e-06, 374 | 1.111121e-06, 375 | -6.38013e-06, 376 | 0.0002390846, 377 | 6.781821e-08, 378 | -3.021705e-06, 379 | -3.602249e-06, 380 | -3.185207e-08 381 | ], 382 | "colDen": [ 383 | 1.0, 384 | 0.0001200143, 385 | 0.000798583, 386 | -0.0004008609, 387 | -2.318286e-05, 388 | -2.146953e-06, 389 | 1.224164e-06, 390 | 3.798324e-06, 391 | -6.887296e-06, 392 | -1.232298e-06, 393 | 0.0, 394 | -5.565139e-08, 395 | -1.911932e-07, 396 | 0.0, 397 | -1.715211e-07, 398 | 1.673782e-07, 399 | 0.0, 400 | -3.039548e-08, 401 | 0.0, 402 | 0.0 403 | ], 404 | "rowOff": 6553.0, 405 | "rowScale": 21798.0, 406 | "colOff": 6628.0, 407 | "colScale": 21250.0, 408 | "latOff": -34.4734, 409 | "latScale": 0.0695, 410 | "lonOff": -58.6088, 411 | "lonScale": 0.0888, 412 | "altOff": 31.0, 413 | "altScale": 501.0 414 | }, 415 | "width": 2074, 416 | "height": 2024 417 | }, 418 | "0004.png": { 419 | "rpc": { 420 | "rowNum": [ 421 | -0.0006788097, 422 | -0.01420803, 423 | 1.016359, 424 | 0.001390559, 425 | 0.0001965326, 426 | 4.824609e-08, 427 | 1.813672e-05, 428 | -0.0005558081, 429 | 0.0005805524, 430 | -1.57612e-07, 431 | -2.884825e-06, 432 | -1.078353e-06, 433 | -8.741689e-06, 434 | -1.345701e-06, 435 | 6.839306e-05, 436 | 0.0003416782, 437 | 9.596204e-05, 438 | 1.258132e-07, 439 | 4.892664e-07, 440 | 1.312171e-07 441 | ], 442 | "rowDen": [ 443 | 1.0, 444 | -0.0002159421, 445 | 7.800686e-05, 446 | -1.76644e-05, 447 | 9.867735e-06, 448 | -2.830848e-06, 449 | -1.049131e-06, 450 | 6.745473e-05, 451 | -0.0001956175, 452 | 9.4383e-05, 453 | -2.442918e-08, 454 | -3.709715e-08, 455 | -7.463133e-06, 456 | -4.08381e-08, 457 | 9.175942e-07, 458 | 8.523518e-05, 459 | -3.928508e-08, 460 | 0.0, 461 | 6.674268e-07, 462 | 0.0 463 | ], 464 | "colNum": [ 465 | -0.008393292, 466 | -1.033898, 467 | -0.002486696, 468 | -0.03059007, 469 | -0.001386879, 470 | -0.0001349388, 471 | 0.0001402342, 472 | 0.007298067, 473 | 0.0005063114, 474 | -7.653245e-06, 475 | -1.153843e-06, 476 | -4.292629e-05, 477 | -3.760359e-06, 478 | 2.865841e-06, 479 | 1.319114e-05, 480 | 0.000243392, 481 | 5.065825e-08, 482 | 2.898404e-06, 483 | 1.203222e-06, 484 | 8.514166e-08 485 | ], 486 | "colDen": [ 487 | 1.0, 488 | -0.001139391, 489 | -0.001450011, 490 | -0.0003958873, 491 | -1.735844e-05, 492 | 2.714537e-06, 493 | -8.086768e-07, 494 | 1.107537e-05, 495 | -3.809142e-06, 496 | -2.864114e-06, 497 | 1.311925e-08, 498 | 1.887902e-07, 499 | -3.79846e-07, 500 | -1.311038e-08, 501 | 1.747421e-08, 502 | -9.814794e-07, 503 | 0.0, 504 | -4.880182e-08, 505 | 1.442472e-08, 506 | 0.0 507 | ], 508 | "rowOff": 5837.0, 509 | "rowScale": 21082.0, 510 | "colOff": 5833.0, 511 | "colScale": 21250.0, 512 | "latOff": -34.4749, 513 | "latScale": 0.0673, 514 | "lonOff": -58.6054, 515 | "lonScale": 0.0932, 516 | "altOff": 31.0, 517 | "altScale": 501.0 518 | }, 519 | "width": 2001, 520 | "height": 1937 521 | } 522 | } -------------------------------------------------------------------------------- /multi_rpc_triangulate/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(multi_rpc_triangulate) 4 | 5 | find_package(Ceres REQUIRED) 6 | find_package (Eigen3 REQUIRED) 7 | 8 | 9 | include_directories(${CERES_INCLUDE_DIRS}) 10 | 11 | add_executable(multi_rpc_triangulate multi_rpc_triangulate.cpp) 12 | target_link_libraries(multi_rpc_triangulate ${CERES_LIBRARIES} Eigen3::Eigen) 13 | -------------------------------------------------------------------------------- /multi_rpc_triangulate/multi_rpc_triangulate.cpp: -------------------------------------------------------------------------------- 1 | // =============================================================================================================== 2 | // Copyright (c) 2019, Cornell University. All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without modification, are permitted provided that 5 | // the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright otice, this list of conditions and 8 | // the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and 11 | // the following disclaimer in the documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of Cornell University nor the names of its contributors may be used to endorse or 14 | // promote products derived from this software without specific prior written permission. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED 17 | // WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 18 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE 19 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 20 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 22 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 23 | // OF SUCH DAMAGE. 24 | // 25 | // Author: Kai Zhang (kz298@cornell.edu) 26 | // 27 | // The research is based upon work supported by the Office of the Director of National Intelligence (ODNI), 28 | // Intelligence Advanced Research Projects Activity (IARPA), via DOI/IBC Contract Number D17PC00287. 29 | // The U.S. Government is authorized to reproduce and distribute copies of this work for Governmental purposes. 30 | // =============================================================================================================== 31 | 32 | 33 | #include "ceres/ceres.h" 34 | #include "glog/logging.h" 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | using Eigen::MatrixXd; 45 | 46 | using ceres::AutoDiffCostFunction; 47 | using ceres::Problem; 48 | using ceres::Solver; 49 | using ceres::Solve; 50 | 51 | using std::cout; 52 | using std::vector; 53 | using std::string; 54 | using std::istringstream; 55 | using std::ifstream; 56 | using std::ofstream; 57 | using std::stod; 58 | using std::runtime_error; 59 | using std::setprecision; 60 | 61 | typedef std::numeric_limits dbl; 62 | #define INIT_VAL -1e10 63 | 64 | 65 | struct RPCCamera { 66 | RPCCamera() {} 67 | 68 | // RPC camera parameters 69 | double col_numera[20] = {INIT_VAL}; 70 | double col_denomi[20] = {INIT_VAL}; 71 | double row_numera[20] = {INIT_VAL}; 72 | double row_denomi[20] = {INIT_VAL}; 73 | double lat_off = INIT_VAL, lat_scale = INIT_VAL; 74 | double lon_off = INIT_VAL, lon_scale = INIT_VAL; 75 | double alt_off = INIT_VAL, alt_scale = INIT_VAL; 76 | double row_off = INIT_VAL, row_scale = INIT_VAL; 77 | double col_off = INIT_VAL, col_scale = INIT_VAL; 78 | 79 | // affine approximation of the RPC camera 80 | // [col, row]^T = M * [lat, lon, alt]^T 81 | double M11; 82 | double M12; 83 | double M13; 84 | double M14; 85 | 86 | double M21; 87 | double M22; 88 | double M23; 89 | double M24; 90 | }; 91 | 92 | struct Observation { 93 | Observation(RPCCamera* cam, double col, double row): cam(cam), col(col), row(row) {} 94 | 95 | RPCCamera* cam = NULL; 96 | double col = INIT_VAL; 97 | double row = INIT_VAL; 98 | }; 99 | 100 | struct ReprojResidual { 101 | ReprojResidual(Observation* pixel): pixel(pixel) {} 102 | 103 | template 104 | bool operator() (const T* const lat, const T* const lon, const T* const alt, 105 | T* residuals) const { 106 | RPCCamera& cam = *(this->pixel->cam); 107 | 108 | T lat_normed = (lat[0] - T(cam.lat_off)) / T(cam.lat_scale); 109 | T lon_normed = (lon[0] - T(cam.lon_off)) / T(cam.lon_scale); 110 | T alt_normed = (alt[0] - T(cam.alt_off)) / T(cam.alt_scale); 111 | 112 | T row_numera = this->apply_poly(cam.row_numera, lat_normed, lon_normed, alt_normed); 113 | T row_denomi = this->apply_poly(cam.row_denomi, lat_normed, lon_normed, alt_normed); 114 | 115 | T predict_row = row_numera / row_denomi * T(cam.row_scale) + T(cam.row_off); 116 | 117 | T col_numera = this->apply_poly(cam.col_numera, lat_normed, lon_normed, alt_normed); 118 | T col_denomi = this->apply_poly(cam.col_denomi, lat_normed, lon_normed, alt_normed); 119 | 120 | T predict_col = col_numera / col_denomi * T(cam.col_scale) + T(cam.col_off); 121 | 122 | residuals[0] = predict_row - T(this->pixel->row); 123 | residuals[1] = predict_col - T(this->pixel->col); 124 | return true; 125 | } 126 | 127 | private: 128 | template 129 | T apply_poly(const double* const poly, T x, T y, T z) const { 130 | T out = T(poly[0]); 131 | out += poly[1]*y + poly[2]*x + poly[3]*z; 132 | out += poly[4]*y*x + poly[5]*y*z +poly[6]*x*z; 133 | out += poly[7]*y*y + poly[8]*x*x + poly[9]*z*z; 134 | out += poly[10]*x*y*z; 135 | out += poly[11]*y*y*y; 136 | out += poly[12]*y*x*x + poly[13]*y*z*z + poly[14]*y*y*x; 137 | out += poly[15]*x*x*x; 138 | out += poly[16]*x*z*z + poly[17]*y*y*z + poly[18]*x*x*z; 139 | out += poly[19]*z*z*z; 140 | 141 | return out; 142 | } 143 | 144 | private: 145 | Observation* pixel = NULL; 146 | }; 147 | 148 | void solve_initial(const vector& pixels, vector& initial) { 149 | assert (pixels.size() >= 2 && initial.size() == 3); 150 | 151 | MatrixXd A(2 * pixels.size(), 3); // one observation contributes to two equations 152 | MatrixXd b(2 * pixels.size(), 1); 153 | for (int i=0; i < pixels.size(); ++i) { 154 | A(i * 2, 0) = pixels[i]->cam->M11; 155 | A(i * 2, 1) = pixels[i]->cam->M12; 156 | A(i * 2, 2) = pixels[i]->cam->M13; 157 | b(i * 2, 0) = pixels[i]->col - pixels[i]->cam->M14; 158 | 159 | A(i * 2 + 1, 0) = pixels[i]->cam->M21; 160 | A(i * 2 + 1, 1) = pixels[i]->cam->M22; 161 | A(i * 2 + 1, 2) = pixels[i]->cam->M23; 162 | b(i * 2 + 1, 0) = pixels[i]->row - pixels[i]->cam->M24; 163 | } 164 | 165 | MatrixXd x = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); 166 | for (int i=0; i < 3; ++i) { 167 | initial[i] = x(i, 0); 168 | } 169 | } 170 | 171 | void refine_initial(const vector& pixels, const vector& initial, vector & final, vector & reproj_error) { 172 | assert (initial.size() == 3 && final.size() == 3 && reproj_error.size() == 2); 173 | 174 | double lat = initial[0]; 175 | double lon = initial[1]; 176 | double alt = initial[2]; 177 | 178 | Problem problem; 179 | for (int i=0; i < pixels.size(); ++i) { 180 | problem.AddResidualBlock( 181 | new AutoDiffCostFunction(new ReprojResidual(pixels[i])), 182 | NULL, &lat, &lon, &alt); 183 | } 184 | 185 | Solver::Options options; 186 | options.max_num_iterations = 100; 187 | // options.function_tolerance = 1e-10; 188 | options.linear_solver_type = ceres::DENSE_QR; 189 | // set the following options to true for debugging 190 | options.minimizer_progress_to_stdout = false; 191 | 192 | Solver::Summary summary; 193 | Solve(options, &problem, &summary); 194 | 195 | double init_error = sqrt(summary.initial_cost * 2 / pixels.size()); 196 | double final_error = sqrt(summary.final_cost * 2 / pixels.size()); 197 | 198 | // for debugging 199 | // cout << summary.BriefReport() << "\n"; 200 | // cout << "\ninitial Point: (" << initial[0] << "," << initial[1] << "," << initial[2] << "), reproj_error: " << init_error << " pixels\n"; 201 | // cout << "final Point: (" << lat << "," << lon << "," << alt << "), reproj_error: " << final_error << " pixels\n"; 202 | 203 | // output 204 | final[0] = lat; 205 | final[1] = lon; 206 | final[2] = alt; 207 | reproj_error[0] = init_error; 208 | reproj_error[1] = final_error; 209 | } 210 | 211 | void read_rpc_cameras(const string& fname, vector& rpc_cameras) { 212 | // number of rpc cameras 213 | // camera_id 214 | // 20 column numerator coefficients 215 | // 20 column denominator coefficients 216 | // 20 row numerator coefficients 217 | // 20 row denominator coefficients 218 | // 10 normalization constants: lat off, lat scale, lon off, lon scale, alt off, alt scale, col off, col scale 219 | // 8 affine approximation coefficients: M11, M12, M13, M14, M21, M22, M23, M24 220 | // ... 221 | 222 | ifstream infile; 223 | infile.open(fname); 224 | if (!infile) { 225 | throw runtime_error("unable to open " + fname); 226 | } 227 | 228 | int cnt; 229 | infile >> cnt; 230 | for (int i=0; i> cam_id; 234 | assert(cam_id == i); 235 | 236 | rpc_cameras.push_back(new RPCCamera()); 237 | RPCCamera *cam = rpc_cameras.back(); 238 | 239 | // read rpc camera parameters 240 | for (int i = 0; i < 20; ++i) { 241 | infile >> cam->col_numera[i]; 242 | } 243 | for (int i = 0; i < 20; ++i) { 244 | infile >> cam->col_denomi[i]; 245 | } 246 | for (int i = 0; i < 20; ++i) { 247 | infile >> cam->row_numera[i]; 248 | } 249 | for (int i = 0; i < 20; ++i) { 250 | infile >> cam->row_denomi[i]; 251 | } 252 | infile >> cam->lat_off >> cam->lat_scale; 253 | infile >> cam->lon_off >> cam->lon_scale; 254 | infile >> cam->alt_off >> cam->alt_scale; 255 | infile >> cam->col_off >> cam->col_scale; 256 | infile >> cam->row_off >> cam->row_scale; 257 | 258 | // read affine approximation parameters 259 | infile >> cam->M11 >> cam->M12 >> cam->M13 >> cam->M14; 260 | infile >> cam->M21 >> cam->M22 >> cam->M23 >> cam->M24; 261 | } 262 | 263 | infile.close(); 264 | } 265 | 266 | void triangulate_tracks(const string& cameras_fname, const string& tracks_fname, const string& results_fname) { 267 | vector rpc_cameras; 268 | read_rpc_cameras(cameras_fname, rpc_cameras); 269 | 270 | ifstream infile; 271 | infile.open(tracks_fname); 272 | if (!infile) { 273 | throw runtime_error("unable to open " + tracks_fname); 274 | } 275 | 276 | ofstream outfile; 277 | outfile.open(results_fname); 278 | if (!outfile) { 279 | throw runtime_error("unable to open " + results_fname); 280 | } 281 | outfile << setprecision(dbl::max_digits10); 282 | 283 | int cnt; 284 | infile >> cnt; 285 | outfile << cnt << '\n'; 286 | for (int i=0; i> len; 290 | // read all the observations for this track 291 | vector pixels; 292 | for (int j=0; j < len; ++j) { 293 | int cam_id; 294 | double col, row; 295 | infile >> cam_id >> col >> row; 296 | 297 | assert(cam_id < rpc_cameras.size()); 298 | pixels.push_back(new Observation(rpc_cameras[cam_id], col, row)); 299 | } 300 | 301 | // solve for (lat, lon, alt) 302 | vector initial(3, INIT_VAL); 303 | vector final(3, INIT_VAL); 304 | vector reproj_error(2, INIT_VAL); 305 | solve_initial(pixels, initial); 306 | refine_initial(pixels, initial, final, reproj_error); 307 | 308 | // write results to file 309 | // each line is "intial lat, initial lon, initial alt, initial reproj err, final lat, final lon, final alt, final reproj err" 310 | outfile << initial[0] << " " << initial[1] << " " << initial[2] << " " << reproj_error[0] << " "; 311 | outfile << final[0] << " " << final[1] << " " << final[2] << " " << reproj_error[1] << "\n"; 312 | 313 | // free memory 314 | for (int i = 0; i < pixels.size(); ++i) { 315 | delete pixels[i]; 316 | } 317 | } 318 | 319 | // close file 320 | infile.close(); 321 | outfile.close(); 322 | 323 | // free memory 324 | for (int i = 0; i < rpc_cameras.size(); ++i) { 325 | delete rpc_cameras[i]; 326 | } 327 | } 328 | 329 | 330 | int main(int argc, char** argv) { 331 | // program name, cameras_fname, tracks_fname, results_fname 332 | assert(argc == 4); 333 | google::InitGoogleLogging(argv[0]); 334 | string cameras_fname = string(argv[1]); 335 | string tracks_fname = string(argv[2]); 336 | string results_fname = string(argv[3]); 337 | 338 | triangulate_tracks(cameras_fname, tracks_fname, results_fname); 339 | return 0; 340 | } 341 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # Triangulation Solver for the RPC Model in Satellite Images 2 | 3 | **Project page**: [https://kai-46.github.io/VisSat/](https://kai-46.github.io/VisSat/) 4 | 5 | ## Introduction 6 | An RPC model maps a 3D point (lat, lon, alt) to a pixel (col, row) in a satellite image. It is a non-linear mapping. 7 | 8 | For a specific 3D point, if you have multiple observations in different images (called a feature track), then you can triangulate the 3D point's coordinates with RPC models. This repo implements this functionality. 9 | 10 | The triangulation solver basically operates in three steps: 11 | 12 | 1. linearize each RPC model in a local area, or you can say 'locally approxmiate each RPC model with an affine model'; 13 | 2. solve a linear system to get an initial guess of the 3D point's coordinates; 14 | 3. then, use [Ceres](http://ceres-solver.org/) to iteratively refine the initial guess by minimizing the average reprojection error. 15 | 16 | ## Installation 17 | 18 | * install [Ceres](http://ceres-solver.org/) and [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page) on your machine 19 | * compile the C++ backbone "multi_rpc_triangulate/" via: 20 | ```{r, engine='bash'} 21 | cd multi_rpc_triangulate && mkdir build && cd build && cmake .. && make 22 | ``` 23 | * the python interface "triangulate.py" uses python3 instead of python2; it requires the minimal dependency: numpy 24 | 25 | ## Quick Start 26 | The folder "example/" contains three example input configuration files: "metas.json" for specifying RPC camera parameters and image sizes, "tracks.txt" for specifying feature tracks, "bbx.json" for specifying the local area in which the RPC models will be linearized. You can triangulate this example by typing: 27 | ```{r, engine='bash'} 28 | python3 triangulate.py 29 | ``` 30 | The solver will write the triangulation results to "example/results.txt". 31 | 32 | The format of the input file "metas.json" is, 33 | ```{r, engine='bash'} 34 | { 35 | "img1": {"rpc": {"colNum": [], "colDen": [], "rowNum": [], "rowDen": [], 36 | "latOff": , "latScale": , "lonOff": , "lonScale": , "altOff": , "altScale": , 37 | "colOff": , "colScale": , "rowOff": , "rowScale": }, 38 | "width": , "height": }, 39 | "img2": {"rpc": {"colNum": [], "colDen": [], "rowNum": [], "rowDen": [], 40 | "latOff": , "latScale": , "lonOff": , "lonScale": , "altOff": , "altScale": , 41 | "colOff": , "colScale": , "rowOff": , "rowScale": }, 42 | "width": , "height": } 43 | ... 44 | } 45 | ``` 46 | The format of the input file "bbx.json" is, 47 | ```{r, engine='bash'} 48 | {"lat_min": , "lat_max": , "lon_min": , "lon_max": , "alt_min": , "alt_max": } 49 | ``` 50 | The format of the input file "tracks.txt" is, 51 | ```{r, engine='bash'} 52 | number_of_tracks 53 | track_length img1 col row img2 col row img3 col row ... 54 | track_length img1 col row img2 col row ... 55 | ... 56 | ``` 57 | The format of the output file "results.txt" is, 58 | ```{r, engine='bash'} 59 | number_of_3D_points 60 | initial_lat initial_lon initial_alt inital_reproj_err final_lat final_lon final_alt final_reproj_err 61 | initial_lat initial_lon initial_alt inital_reproj_err final_lat final_lon final_alt final_reproj_err 62 | ... 63 | ``` 64 | 65 | ## License 66 | This software uses the [3-clause BSD license](https://opensource.org/licenses/BSD-3-Clause). 67 | -------------------------------------------------------------------------------- /rpc_model.py: -------------------------------------------------------------------------------- 1 | # =============================================================================================================== 2 | # Copyright (c) 2019, Cornell University. All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 5 | # the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright otice, this list of conditions and 8 | # the following disclaimer. 9 | # 10 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and 11 | # the following disclaimer in the documentation and/or other materials provided with the distribution. 12 | # 13 | # * Neither the name of Cornell University nor the names of its contributors may be used to endorse or 14 | # promote products derived from this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED 17 | # WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 18 | # A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE 19 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 20 | # TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21 | # HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 22 | # NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 23 | # OF SUCH DAMAGE. 24 | # 25 | # Author: Kai Zhang (kz298@cornell.edu) 26 | # 27 | # The research is based upon work supported by the Office of the Director of National Intelligence (ODNI), 28 | # Intelligence Advanced Research Projects Activity (IARPA), via DOI/IBC Contract Number D17PC00287. 29 | # The U.S. Government is authorized to reproduce and distribute copies of this work for Governmental purposes. 30 | # =============================================================================================================== 31 | 32 | 33 | # Copyright (C) 2015, Carlo de Franchis 34 | # Copyright (C) 2015, Gabriele Facciolo 35 | # Copyright (C) 2015, Enric Meinhardt 36 | 37 | 38 | import numpy as np 39 | 40 | def apply_poly(poly, x, y, z): 41 | """ 42 | Evaluates a 3-variables polynom of degree 3 on a triplet of numbers. 43 | Args: 44 | poly: list of the 20 coefficients of the 3-variate degree 3 polynom, 45 | ordered following the RPC convention. 46 | x, y, z: triplet of floats. They may be numpy arrays of same length. 47 | Returns: 48 | the value(s) of the polynom on the input point(s). 49 | """ 50 | out = 0 51 | out += poly[0] 52 | out += poly[1]*y + poly[2]*x + poly[3]*z 53 | out += poly[4]*y*x + poly[5]*y*z +poly[6]*x*z 54 | out += poly[7]*y*y + poly[8]*x*x + poly[9]*z*z 55 | out += poly[10]*x*y*z 56 | out += poly[11]*y*y*y 57 | out += poly[12]*y*x*x + poly[13]*y*z*z + poly[14]*y*y*x 58 | out += poly[15]*x*x*x 59 | out += poly[16]*x*z*z + poly[17]*y*y*z + poly[18]*x*x*z 60 | out += poly[19]*z*z*z 61 | return out 62 | 63 | def apply_rfm(num, den, x, y, z): 64 | """ 65 | Evaluates a Rational Function Model (rfm), on a triplet of numbers. 66 | Args: 67 | num: list of the 20 coefficients of the numerator 68 | den: list of the 20 coefficients of the denominator 69 | All these coefficients are ordered following the RPC convention. 70 | x, y, z: triplet of floats. They may be numpy arrays of same length. 71 | Returns: 72 | the value(s) of the rfm on the input point(s). 73 | """ 74 | return apply_poly(num, x, y, z) / apply_poly(den, x, y, z) 75 | 76 | class RPCModel(object): 77 | def __init__(self, meta_dict): 78 | rpc_dict = meta_dict['rpc'] 79 | # normalization constant 80 | self.colOff = rpc_dict['colOff'] 81 | self.colScale = rpc_dict['colScale'] 82 | 83 | self.rowOff = rpc_dict['rowOff'] 84 | self.rowScale = rpc_dict['rowScale'] 85 | 86 | self.latOff = rpc_dict['latOff'] 87 | self.latScale = rpc_dict['latScale'] 88 | 89 | self.lonOff = rpc_dict['lonOff'] 90 | self.lonScale = rpc_dict['lonScale'] 91 | 92 | self.altOff = rpc_dict['altOff'] 93 | self.altScale = rpc_dict['altScale'] 94 | 95 | # polynomial coefficients 96 | self.colNum = rpc_dict['colNum'] 97 | self.colDen = rpc_dict['colDen'] 98 | self.rowNum = rpc_dict['rowNum'] 99 | self.rowDen = rpc_dict['rowDen'] 100 | 101 | # img_width, img_height 102 | self.width = meta_dict['width'] 103 | self.height = meta_dict['height'] 104 | 105 | def projection(self, lat, lon, alt): 106 | cLon = (lon - self.lonOff) / self.lonScale 107 | cLat = (lat - self.latOff) / self.latScale 108 | cAlt = (alt - self.altOff) / self.altScale 109 | cCol = apply_rfm(self.colNum, self.colDen, cLat, cLon, cAlt) 110 | cRow = apply_rfm(self.rowNum, self.rowDen, cLat, cLon, cAlt) 111 | col = cCol*self.colScale + self.colOff 112 | row = cRow*self.rowScale + self.rowOff 113 | return col, row 114 | 115 | def to_string(self): 116 | string = '' 117 | 118 | tmp = [str(x) for x in self.colNum] 119 | string += ' '.join(tmp) + '\n' 120 | 121 | tmp = [str(x) for x in self.colDen] 122 | string += ' '.join(tmp) + '\n' 123 | 124 | tmp = [str(x) for x in self.rowNum] 125 | string += ' '.join(tmp) + '\n' 126 | 127 | tmp = [str(x) for x in self.rowDen] 128 | string += ' '.join(tmp) + '\n' 129 | 130 | string += '{} {} {} {} {} {} {} {} {} {}\n'.format(self.latOff, self.latScale, self.lonOff, self.lonScale, self.altOff, self.altScale, 131 | self.colOff, self.colScale, self.rowOff, self.rowScale) 132 | return string 133 | 134 | 135 | ################################################################################################## 136 | # local approximation of the rpc model with an affine model 137 | ################################################################################################## 138 | def generate_grid(x_points, y_points, z_points): 139 | x_point_cnt = x_points.size 140 | y_point_cnt = y_points.size 141 | z_point_cnt = z_points.size 142 | point_cnt = x_point_cnt * y_point_cnt * z_point_cnt 143 | 144 | xx, yy = np.meshgrid(x_points, y_points, indexing='ij') 145 | xx = np.reshape(xx, (-1, 1)) 146 | yy = np.reshape(yy, (-1, 1)) 147 | xx = np.tile(xx, (z_point_cnt, 1)) 148 | yy = np.tile(yy, (z_point_cnt, 1)) 149 | 150 | zz = np.zeros((point_cnt, 1)) 151 | for j in range(z_point_cnt): 152 | idx1 = j * x_point_cnt * y_point_cnt 153 | idx2 = (j + 1) * x_point_cnt * y_point_cnt 154 | zz[idx1:idx2, 0] = z_points[j] 155 | 156 | return xx, yy, zz 157 | 158 | def solve_affine(xx, yy, zz, col, row, valid_mask=None): 159 | diff_size = np.array([yy.size - xx.size, zz.size - xx.size, col.size - xx.size, row.size - xx.size]) 160 | assert (np.all(diff_size == 0)) 161 | 162 | if valid_mask is not None: 163 | xx = xx[valid_mask].reshape((-1, 1)) 164 | yy = yy[valid_mask].reshape((-1, 1)) 165 | zz = zz[valid_mask].reshape((-1, 1)) 166 | row = row[valid_mask].reshape((-1, 1)) 167 | col = col[valid_mask].reshape((-1, 1)) 168 | 169 | # construct a least square problem 170 | point_cnt = xx.size 171 | all_ones = np.ones((point_cnt, 1)) 172 | all_zeros = np.zeros((point_cnt, 4)) 173 | # construct the least square problem 174 | A1 = np.hstack((xx, yy, zz, all_ones, all_zeros)) 175 | A2 = np.hstack((all_zeros, xx, yy, zz, all_ones)) 176 | 177 | A = np.vstack((A1, A2)) 178 | b = np.vstack((col, row)) 179 | res = np.linalg.lstsq(A, b, rcond=-1) 180 | P = res[0].reshape((2, 4)) 181 | 182 | return P 183 | 184 | def affine_approx(rpc_model, bbx): 185 | lat_min, lat_max, lon_min, lon_max, alt_min, alt_max = bbx 186 | 187 | xy_axis_grid_points = 100 188 | z_axis_grid_points = 20 189 | 190 | lat_points = np.linspace(lat_min, lat_max, xy_axis_grid_points) 191 | lon_points = np.linspace(lon_min, lon_max, xy_axis_grid_points) 192 | alt_points = np.linspace(alt_min, alt_max, z_axis_grid_points) 193 | lat_points, lon_points, alt_points = generate_grid(lat_points, lon_points, alt_points) 194 | 195 | col, row = rpc_model.projection(lat_points, lon_points, alt_points) 196 | 197 | valid_mask = np.logical_and.reduce((col>=0, row>=0, col