forked from nasa/StereoPipeline
-
Notifications
You must be signed in to change notification settings - Fork 0
/
TiePointTransform.h
109 lines (89 loc) · 3.72 KB
/
TiePointTransform.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__
/// \file TiePointTransform.h
///
#ifndef __TIEPOINTTRANSFORM_H__
#define __TIEPOINTTRANSFORM_H__
#include <vw/Image/Transform.h>
#include <vw/Math/Vector.h>
#include <vw/Math/Matrix.h>
#include <vw/Math/BBox.h>
#include <MBA.h>
/// class TiePointTransform
///
/// A transform functor that maps a set of input pixel locations onto a
/// set of output pixel locations, using a combination of a homography
/// and a multilevel B-spline approximation. The homography is computed
/// first and factored out prior to the B-spline computation, reducing
/// biasing peculiarities of the B-spline warp.
///
/// **NOTE**
/// The homography is also used to compute the forward transform. This
/// gives you a quick way to comptue the approximate an output bounding
/// box using forward_bbox(), but it is only *approximate*! Computing
/// a true forward() mapping function would require tricky nonlinear
/// optimization which we don't bother doing for now.
class TiePointTransform : public vw::TransformHelper<TiePointTransform,vw::ContinuousFunction,vw::ContinuousFunction> {
protected:
vw::Matrix3x3 m_H, m_H_inverse;
UCBspl::SplineSurface m_fx, m_fy;
public:
TiePointTransform( std::vector<vw::Vector2> const& input, std::vector<vw::Vector2> const& output ) {
using namespace vw;
boost::shared_ptr<std::vector<double> > x_arr( new std::vector<double> );
boost::shared_ptr<std::vector<double> > y_arr( new std::vector<double> );
boost::shared_ptr<std::vector<double> > zx_arr( new std::vector<double> );
boost::shared_ptr<std::vector<double> > zy_arr( new std::vector<double> );
BBox2 bbox;
for( unsigned i=0; i<input.size(); ++i ) {
x_arr->push_back( output[i].x() );
y_arr->push_back( output[i].y() );
zx_arr->push_back( input[i].x() );
zy_arr->push_back( input[i].y() );
bbox.grow( output[i] );
}
Matrix<double> inM(3,x_arr->size());
Matrix<double> outM(3,x_arr->size());
for( unsigned i=0; i<x_arr->size(); ++i ) {
inM(0,i) = (*zx_arr)[i];
inM(1,i) = (*zy_arr)[i];
inM(2,i) = 1;
outM(0,i) = (*x_arr)[i];
outM(1,i) = (*y_arr)[i];
outM(2,i) = 1;
}
Matrix3x3 M = inM*pseudoinverse(outM);
inM = M * outM;
for( unsigned i=0; i<x_arr->size(); ++i ) {
(*zx_arr)[i] -= inM(0,i) / inM(2,i);
(*zy_arr)[i] -= inM(1,i) / inM(2,i);
}
MBA mba_x(x_arr,y_arr,zx_arr);
MBA mba_y(x_arr,y_arr,zy_arr);
int m0 = 1, n0 = 1, numIterations = 3;
if (bbox.width() > bbox.height() )
m0 = round( bbox.width() / bbox.height() );
else
n0 = round( bbox.height() / bbox.width() );
mba_x.MBAalg(m0,n0,numIterations);
m_fx = mba_x.getSplineSurface();
mba_y.MBAalg(m0,n0,numIterations);
m_fy = mba_y.getSplineSurface();
m_H_inverse = M;
m_H = inverse( m_H_inverse );
}
inline vw::Vector2 reverse( vw::Vector2 const& p ) const {
double w = m_H_inverse(2,0) * p(0) + m_H_inverse(2,1) * p(1) + m_H_inverse(2,2);
return vw::Vector2( ( m_H_inverse(0,0) * p(0) + m_H_inverse(0,1) * p(1) + m_H_inverse(0,2) ) / w + m_fx.f(p.x(),p.y()),
( m_H_inverse(1,0) * p(0) + m_H_inverse(1,1) * p(1) + m_H_inverse(1,2) ) / w + m_fy.f(p.x(),p.y()));
}
inline vw::Vector2 forward( vw::Vector2 const& p ) const {
double w = m_H(2,0) * p(0) + m_H(2,1) * p(1) + m_H(2,2);
return vw::Vector2( ( m_H(0,0) * p(0) + m_H(0,1) * p(1) + m_H(0,2) ) / w,
( m_H(1,0) * p(0) + m_H(1,1) * p(1) + m_H(1,2) ) / w);
}
};
#endif // __TIEPOINTTRANSFORM__