OTB  6.7.0
Orfeo Toolbox
otbLineOfSightOptimizer.hxx
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2005-2019 Centre National d'Etudes Spatiales (CNES)
3  *
4  * This file is part of Orfeo Toolbox
5  *
6  * https://www.orfeo-toolbox.org/
7  *
8  * Licensed under the Apache License, Version 2.0 (the "License");
9  * you may not use this file except in compliance with the License.
10  * You may obtain a copy of the License at
11  *
12  * http://www.apache.org/licenses/LICENSE-2.0
13  *
14  * Unless required by applicable law or agreed to in writing, software
15  * distributed under the License is distributed on an "AS IS" BASIS,
16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  * See the License for the specific language governing permissions and
18  * limitations under the License.
19  */
20 
21 #ifndef otbLineOfSightOptimizer_hxx
22 #define otbLineOfSightOptimizer_hxx
23 
25 
26 #include "vnl/vnl_inverse.h"
27 
28 namespace otb
29 {
30 
31 template <class TPrecision, class TLabel>
34 {
35  m_Residues.clear();
36 
37  m_GlobalResidue = 0;
38 
39  m_InvCumul = vnl_matrix<PrecisionType>(3,3);
40 
41  m_Identity = vnl_matrix<PrecisionType>(3,3);
42  m_Identity.fill(0);
43  m_Identity.fill_diagonal(1.);
44 
45  m_SecCumul = vnl_vector<PrecisionType>(3);
46 }
47 
48 template <class TPrecision, class TLabel>
52 {
53  // First, empty the cumulators and residues
54  m_InvCumul.fill(0);
55  m_SecCumul.fill(0);
56  m_Residues.clear();
57 
58  vnl_matrix<PrecisionType> idMinusViViT(3,3);
59  vnl_matrix<PrecisionType> vi(3,1);
60  vnl_vector<PrecisionType> si(3);
61 
62  PointType result;
63 
64  //check inputs
65  if (pointA->GetNumberOfPoints() != pointB->GetNumberOfPoints() ||
66  pointA->GetNumberOfPoints() < 2)
67  {
68  itkExceptionMacro(<<"Points are missing in at least one of the input point sets.");
69  return result;
70  }
71 
72  // iterate over lines of sight
73  PointSetConstIteratorType itPointA = pointA->GetPoints()->Begin();
74  PointSetConstIteratorType itPointB = pointB->GetPoints()->Begin();
75 
76  while (itPointA != pointA->GetPoints()->End() &&
77  itPointB != pointB->GetPoints()->End())
78  {
79  vi(0,0) = itPointB.Value()[0] - itPointA.Value()[0];
80  vi(1,0) = itPointB.Value()[1] - itPointA.Value()[1];
81  vi(2,0) = itPointB.Value()[2] - itPointA.Value()[2];
82 
83  PrecisionType norm_inv = 1. / std::sqrt(vi(0,0)*vi(0,0)+vi(1,0)*vi(1,0)+vi(2,0)*vi(2,0));
84 
85  vi(0,0) *= norm_inv;
86  vi(1,0) *= norm_inv;
87  vi(2,0) *= norm_inv;
88 
89  si(0) = itPointA.Value()[0];
90  si(1) = itPointA.Value()[1];
91  si(2) = itPointA.Value()[2];
92 
93  idMinusViViT = m_Identity - (vi * vi.transpose());
94 
95  m_InvCumul+=idMinusViViT;
96  m_SecCumul+=(idMinusViViT * si);
97 
98  ++itPointA;
99  ++itPointB;
100  }
101 
102  vnl_vector<PrecisionType> intersection = vnl_inverse(m_InvCumul) * m_SecCumul;
103 
104  result[0] = intersection[0];
105  result[1] = intersection[1];
106  result[2] = intersection[2];
107 
108  // Compute residues
109  m_GlobalResidue = 0;
110 
111  vnl_vector<PrecisionType> AB(3);
112  vnl_vector<PrecisionType> AC(3);
113  PrecisionType res2;
114  itPointA = pointA->GetPoints()->Begin();
115  itPointB = pointB->GetPoints()->Begin();
116  while (itPointA != pointA->GetPoints()->End() &&
117  itPointB != pointB->GetPoints()->End())
118  {
119  AB[0] = itPointB.Value()[0] - itPointA.Value()[0];
120  AB[1] = itPointB.Value()[1] - itPointA.Value()[1];
121  AB[2] = itPointB.Value()[2] - itPointA.Value()[2];
122 
123  AC[0] = intersection[0] - itPointA.Value()[0];
124  AC[1] = intersection[1] - itPointA.Value()[1];
125  AC[2] = intersection[2] - itPointA.Value()[2];
126 
127  res2 = std::max(0.0,dot_product(AC,AC) - (dot_product(AB,AC) * dot_product(AB,AC)) / (dot_product(AB,AB)));
128 
129  m_Residues.push_back( std::sqrt( res2 ) );
130  m_GlobalResidue += res2;
131 
132  ++itPointA;
133  ++itPointB;
134  }
135 
136  m_GlobalResidue = std::sqrt(m_GlobalResidue);
137 
138  return result;
139 }
140 
141 }
142 
143 #endif
PointSetType::PointsContainerConstIterator PointSetConstIteratorType
PointType Compute(PointSetPointerType pointA, PointSetPointerType pointB)
PointSetType::PointType PointType