OTB  9.0.0
Orfeo Toolbox
otbLineOfSightOptimizer.hxx
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2005-2022 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>
33 {
34  m_Residues.clear();
35 
36  m_GlobalResidue = 0;
37 
38  m_InvCumul = vnl_matrix<PrecisionType>(3, 3);
39 
40  m_Identity = vnl_matrix<PrecisionType>(3, 3);
41  m_Identity.fill(0);
42  m_Identity.fill_diagonal(1.);
43 
44  m_SecCumul = vnl_vector<PrecisionType>(3);
45 }
46 
47 template <class TPrecision, class TLabel>
49  PointSetPointerType pointB)
50 {
51  // First, empty the cumulators and residues
52  m_InvCumul.fill(0);
53  m_SecCumul.fill(0);
54  m_Residues.clear();
55 
56  vnl_matrix<PrecisionType> idMinusViViT(3, 3);
57  vnl_matrix<PrecisionType> vi(3, 1);
58  vnl_vector<PrecisionType> si(3);
59 
60  PointType result;
61 
62  // check inputs
63  if (pointA->GetNumberOfPoints() != pointB->GetNumberOfPoints() || pointA->GetNumberOfPoints() < 2)
64  {
65  itkExceptionMacro(<< "Points are missing in at least one of the input point sets.");
66  return result;
67  }
68 
69  // iterate over lines of sight
70  PointSetConstIteratorType itPointA = pointA->GetPoints()->Begin();
71  PointSetConstIteratorType itPointB = pointB->GetPoints()->Begin();
72 
73  while (itPointA != pointA->GetPoints()->End() && itPointB != pointB->GetPoints()->End())
74  {
75  vi(0, 0) = itPointB.Value()[0] - itPointA.Value()[0];
76  vi(1, 0) = itPointB.Value()[1] - itPointA.Value()[1];
77  vi(2, 0) = itPointB.Value()[2] - itPointA.Value()[2];
78 
79  PrecisionType norm_inv = 1. / std::sqrt(vi(0, 0) * vi(0, 0) + vi(1, 0) * vi(1, 0) + vi(2, 0) * vi(2, 0));
80 
81  vi(0, 0) *= norm_inv;
82  vi(1, 0) *= norm_inv;
83  vi(2, 0) *= norm_inv;
84 
85  si(0) = itPointA.Value()[0];
86  si(1) = itPointA.Value()[1];
87  si(2) = itPointA.Value()[2];
88 
89  idMinusViViT = m_Identity - (vi * vi.transpose());
90 
91  m_InvCumul += idMinusViViT;
92  m_SecCumul += (idMinusViViT * si);
93 
94  ++itPointA;
95  ++itPointB;
96  }
97 
98  vnl_vector<PrecisionType> intersection = vnl_inverse(m_InvCumul) * m_SecCumul;
99 
100  result[0] = intersection[0];
101  result[1] = intersection[1];
102  result[2] = intersection[2];
103 
104  // Compute residues
105  m_GlobalResidue = 0;
106 
107  vnl_vector<PrecisionType> AB(3);
108  vnl_vector<PrecisionType> AC(3);
109  PrecisionType res2;
110  itPointA = pointA->GetPoints()->Begin();
111  itPointB = pointB->GetPoints()->Begin();
112  while (itPointA != pointA->GetPoints()->End() && itPointB != pointB->GetPoints()->End())
113  {
114  AB[0] = itPointB.Value()[0] - itPointA.Value()[0];
115  AB[1] = itPointB.Value()[1] - itPointA.Value()[1];
116  AB[2] = itPointB.Value()[2] - itPointA.Value()[2];
117 
118  AC[0] = intersection[0] - itPointA.Value()[0];
119  AC[1] = intersection[1] - itPointA.Value()[1];
120  AC[2] = intersection[2] - itPointA.Value()[2];
121 
122  res2 = std::max(0.0, dot_product(AC, AC) - (dot_product(AB, AC) * dot_product(AB, AC)) / (dot_product(AB, AB)));
123 
124  m_Residues.push_back(std::sqrt(res2));
125  m_GlobalResidue += res2;
126 
127  ++itPointA;
128  ++itPointB;
129  }
130 
131  m_GlobalResidue = std::sqrt(m_GlobalResidue);
132 
133  return result;
134 }
135 }
136 
137 #endif
otb::LineOfSightOptimizer::PointSetConstIteratorType
PointSetType::PointsContainerConstIterator PointSetConstIteratorType
Definition: otbLineOfSightOptimizer.h:64
otb::LineOfSightOptimizer::Compute
PointType Compute(PointSetPointerType pointA, PointSetPointerType pointB)
Definition: otbLineOfSightOptimizer.hxx:48
otb
The "otb" namespace contains all Orfeo Toolbox (OTB) classes.
Definition: otbJoinContainer.h:32
otb::LineOfSightOptimizer::LineOfSightOptimizer
LineOfSightOptimizer()
Definition: otbLineOfSightOptimizer.hxx:32
otb::LineOfSightOptimizer::PointType
PointSetType::PointType PointType
Definition: otbLineOfSightOptimizer.h:62
otbLineOfSightOptimizer.h
otb::LineOfSightOptimizer::PointSetPointerType
PointSetType::Pointer PointSetPointerType
Definition: otbLineOfSightOptimizer.h:63
otb::LineOfSightOptimizer::PrecisionType
TPrecision PrecisionType
Definition: otbLineOfSightOptimizer.h:55