38template <
typename TCost>
39class TLSDImageCost:
public TCost
46 class CRefPrepare :
public mia::TFilter<void>
49 CRefPrepare(std::vector<double>& QtQinv, std::vector<int>& Q_mappping):
51 m_Q_mappping(Q_mappping)
54 template <
typename DataTempl>
55 void operator()(
const DataTempl& ref);
57 std::vector<double>& m_QtQinv;
58 std::vector<int>& m_Q_mappping;
61 class RunCost : mia::TFilter<double>
65 RunCost(
const std::vector<double>& QtQinv,
const std::vector<int>& Q_mappping);
67 template <
typename DataTempl>
68 double operator()(
const DataTempl& ref)
const;
70 template <
typename DataTempl>
71 double operator()(
const DataTempl& ref, Force& force)
const;
73 const std::vector<double>& m_QtQinv;
74 const std::vector<int>& m_Q_mappping;
79 virtual double do_value(
const Data& a,
const Data& b)
const;
81 virtual double do_evaluate_force(
const Data& a,
const Data& , Force& force)
const;
83 virtual void post_set_reference(
const Data& ref);
85 std::vector<double> m_QtQinv;
86 std::vector<int> m_Q_mappping;
90inline bool operator < (
const valpos& a,
const valpos& b)
95template <
typename CP,
typename C>
96class TLSDImageCostPlugin:
public CP
99 TLSDImageCostPlugin();
100 virtual TLSDImageCost<C> *do_create()
const;
101 const std::string do_get_descr()
const;
104template <
typename TCost>
105template <
typename DataTempl>
106void TLSDImageCost<TCost>::CRefPrepare::operator()(
const DataTempl& ref)
108 size_t npixels = ref.get_size().x * ref.get_size().y;
109 std::vector<valpos> buffer(npixels);
110 m_Q_mappping.resize(npixels);
111 m_QtQinv.resize(npixels);
114 for (
auto x = ref.begin(); x != ref.end(); ++x, ++idx) {
116 valpos vp = {value, idx};
120 std::sort(buffer.begin(), buffer.end());
122 auto x = buffer.begin();
123 double oldv = x->val;
127 m_Q_mappping[x->pos] = idx;
129 while ( x != buffer.end() ) {
130 if (x->val != oldv) {
136 m_Q_mappping[x->pos] = idx;
141 m_QtQinv.resize(idx);
142 std::transform(m_QtQinv.begin(), m_QtQinv.end(), m_QtQinv.begin(),
148template <
typename TCost>
149void TLSDImageCost<TCost>::post_set_reference(
const Data& ref)
151 CRefPrepare rp(m_QtQinv, m_Q_mappping);
156template <
typename TCost>
157double TLSDImageCost<TCost>::do_value(
const Data& a,
const Data& )
const
159 RunCost rf(m_QtQinv, m_Q_mappping);
163template <
typename TCost>
164double TLSDImageCost<TCost>::do_evaluate_force(
const Data& a,
const Data& , Force& force)
const
166 RunCost rf(m_QtQinv, m_Q_mappping);
170template <
typename TCost>
171TLSDImageCost<TCost>::RunCost::RunCost(
const std::vector<double>& QtQinv,
const std::vector<int>& Q_mappping):
173 m_Q_mappping(Q_mappping)
177template <
typename TCost>
178template <
typename DataTempl>
179double TLSDImageCost<TCost>::RunCost::operator()(
const DataTempl& a)
const
183 std::vector<double> sums(m_QtQinv.size(), 0.0);
184 auto idx = m_Q_mappping.begin();
186 for (
auto ia = a.begin(); ia != a.end(); ++ia, ++idx) {
191 for (
size_t i = 0; i < sums.size(); ++i)
192 val2 += sums[i] * sums[i] * m_QtQinv[i];
194 return 0.5 * (val1 - val2);
197template <
typename TCost>
198template <
typename DataTempl>
199double TLSDImageCost<TCost>::RunCost::operator()(
const DataTempl& a, Force& force)
const
202 std::vector<double> sums(m_QtQinv.size(), 0.0);
203 auto idx = m_Q_mappping.begin();
205 for (
auto ia = a.begin(); ia != a.end(); ++ia, ++idx) {
210 auto s = sums.begin();
212 for (
auto q = m_QtQinv.begin(); q != m_QtQinv.end(); ++q, ++s) {
213 value -= *q * *s * *s;
218 auto iforce = force.begin();
219 auto igrad = gradient.begin();
220 idx = m_Q_mappping.begin();
222 for (
auto ia = a.begin(); ia != a.end(); ++ia, ++iforce, ++igrad, ++idx)
223 *iforce = *igrad * (*ia - sums[*idx]);
228template <
typename CP,
typename C>
229TLSDImageCostPlugin<CP, C>::TLSDImageCostPlugin():
234template <
typename CP,
typename C>
235TLSDImageCost<C> *TLSDImageCostPlugin<CP, C>::do_create()
const
237 return new TLSDImageCost<C>();
240template <
typename CP,
typename C>
241const std::string TLSDImageCostPlugin<CP, C>::do_get_descr()
const
243 return "Least-Squares Distance measure";
EXPORT_2D C2DFVectorfield get_gradient(const C2DImage &image)
bool operator<(const T2DVector< T > &a, const T2DVector< S > &b)
The generic cost function interface.
V Force
typedef for generic programming: The gradient forca type create by the cost function
T Data
typedef for generic programming: The data type used by the cost function
#define NS_BEGIN(NS)
conveniance define to start a namespace
#define NS_END
conveniance define to end a namespace
static F::result_type filter_and_output(const F &f, const B &a, O &b)
static F::result_type filter(const F &f, const B &b)
static F::result_type accumulate(F &f, const B &data)
R result_type
defines the return type of the filter function