bpcg.h
Go to the documentation of this file.
1  /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 //
36 // block preconditioned conjugate gradient
37 // 6x6 blocks at present, should be templatized
38 //
39 
40 #ifndef _BPCG_H_
41 #define _BPCG_H_
42 
43 #ifndef EIGEN_USE_NEW_STDVECTOR
44 #define EIGEN_USE_NEW_STDVECTOR
45 #endif // EIGEN_USE_NEW_STDVECTOR
46 
47 #include <vector>
48 #include <map>
49 #include <Eigen/Core>
50 #include <Eigen/Geometry>
51 #include <Eigen/LU>
52 #include <Eigen/StdVector>
53 
54 using namespace Eigen;
55 using namespace std;
56 
57 //typedef Matrix<double,6,1> Vector6d;
58 //typedef Vector6d::AlignedMapType AV6d;
59 
60 namespace sba
61 {
63 
64  template <int N>
65  class jacobiBPCG
66  {
67  public:
68  jacobiBPCG() { residual = 0.0; };
69  int doBPCG(int iters, double tol,
70  vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
71  vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
72  VectorXd &x,
73  VectorXd &b,
74  bool abstol = false,
75  bool verbose = false
76  );
77 
78  // uses internal linear storage for Hessian
79  int doBPCG2(int iters, double tol,
80  vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
81  vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
82  VectorXd &x,
83  VectorXd &b,
84  bool abstol = false,
85  bool verbose = false
86  );
87 
88  double residual;
89 
90  private:
91  void mMV(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
92  vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
93  const VectorXd &vin,
94  VectorXd &vout);
95 
96  // uses internal linear storage
97  void mMV2(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
98  const VectorXd &vin,
99  VectorXd &vout);
100 
101  void mD(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
102  VectorXd &vin,
103  VectorXd &vout);
104 
105  vector<int> vcind, vrind;
106  vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > vcols;
107  };
108 
109  template <int N>
110  void jacobiBPCG<N>::mMV(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
111  vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
112  const VectorXd &vin,
113  VectorXd &vout)
114  {
115  // loop over all entries
116  if (cols.size() > 0)
117  for (int i=0; i<(int)cols.size(); i++)
118  {
119  vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N); // only works with cols ordering
120 
121  map<int,Matrix<double,N,N>, less<int>,
122  aligned_allocator<Matrix<double,N,N> > > &col = cols[i];
123  if (col.size() > 0)
124  {
125  typename map<int,Matrix<double,N,N>, less<int>, // need "typename" here, barf
126  aligned_allocator<Matrix<double,N,N > > >::iterator it;
127  for (it = col.begin(); it != col.end(); it++)
128  {
129  int ri = (*it).first; // get row index
130  const Matrix<double,N,N> &M = (*it).second; // matrix
131  vout.segment<N>(i*N) += M.transpose()*vin.segment<N>(ri*N);
132  vout.segment<N>(ri*N) += M*vin.segment<N>(i*N);
133  }
134  }
135  }
136  }
137 
138  //
139  // matrix-vector multiply with linear storage
140  //
141 
142  template <int N>
143  void jacobiBPCG<N>::mMV2(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
144  const VectorXd &vin,
145  VectorXd &vout)
146  {
147  // linear storage for matrices
148  // loop over off-diag entries
149  if (diag.size() > 0)
150  for (int i=0; i<(int)diag.size(); i++)
151  vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N); // only works with cols ordering
152 
153  for (int i=0; i<(int)vcind.size(); i++)
154  {
155  int ri = vrind[i];
156  int ii = vcind[i];
157  const Matrix<double,N,N> &M = vcols[i];
158  vout.segment<N>(ri*N) += M*vin.segment<N>(ii*N);
159  vout.segment<N>(ii*N) += M.transpose()*vin.segment<N>(ri*N);
160  }
161  }
162 
163 
164 
165 
166  template <int N>
167  void jacobiBPCG<N>::mD(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
168  VectorXd &vin,
169  VectorXd &vout)
170  {
171  // loop over diag entries
172  for (int i=0; i<(int)diag.size(); i++)
173  vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N);
174  }
175 
176 
177  template <int N>
178  int jacobiBPCG<N>::doBPCG(int iters, double tol,
179  vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
180  vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
181  VectorXd &x,
182  VectorXd &b,
183  bool abstol,
184  bool verbose)
185  {
186  // set up local vars
187  VectorXd r,d,q,s;
188  int n = diag.size();
189  int n6 = n*N;
190  r.setZero(n6);
191  d.setZero(n6);
192  q.setZero(n6);
193  s.setZero(n6);
194 
195  // set up Jacobi preconditioner
196  vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > J;
197  J.resize(n);
198  for (int i=0; i<n; i++)
199  J[i] = diag[i].inverse();
200 
201  int i;
202  r = b;
203  mD(J,r,d);
204  double dn = r.dot(d);
205  double d0 = tol*dn;
206  if (abstol) // change tolerances
207  {
208  if (residual > d0) d0 = residual;
209  }
210 
211  for (i=0; i<iters; i++)
212  {
213  if (verbose && 0)
214  cout << "[BPCG] residual[" << i << "]: " << dn << " < " << d0 << endl;
215  if (dn < d0) break; // done
216  mMV(diag,cols,d,q);
217  double a = dn / d.dot(q);
218  x += a*d;
219  // TODO: reset residual here every 50 iterations
220  r -= a*q;
221  mD(J,r,s);
222  double dold = dn;
223  dn = r.dot(s);
224  double ba = dn / dold;
225  d = s + ba*d;
226  }
227 
228 
229  if (verbose)
230  cout << "[BPCG] residual[" << i << "]: " << dn << endl;
231  residual = dn/2.0;
232  return i;
233  }
234 
235 
236  // uses internal linear storage for Hessian
237  template <int N>
238  int jacobiBPCG<N>::doBPCG2(int iters, double tol,
239  vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
240  vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
241  VectorXd &x,
242  VectorXd &b,
243  bool abstol,
244  bool verbose)
245  {
246  // set up local vars
247  VectorXd r,d,q,s;
248  int n = diag.size();
249  int n6 = n*N;
250  r.setZero(n6);
251  d.setZero(n6);
252  q.setZero(n6);
253  s.setZero(n6);
254 
255  vcind.clear();
256  vrind.clear();
257  vcols.clear();
258 
259  // set up alternate rep for sparse matrix
260  for (int i=0; i<(int)cols.size(); i++)
261  {
262  map<int,Matrix<double,N,N>, less<int>,
263  aligned_allocator<Matrix<double,N,N> > > &col = cols[i];
264  if (col.size() > 0)
265  {
266  typename map<int,Matrix<double,N,N>, less<int>,
267  aligned_allocator<Matrix<double,N,N> > >::iterator it;
268  for (it = col.begin(); it != col.end(); it++)
269  {
270  int ri = (*it).first; // get row index
271  vrind.push_back(ri);
272  vcind.push_back(i);
273  vcols.push_back((*it).second);
274  }
275  }
276  }
277 
278  // set up Jacobi preconditioner
279  vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > J;
280  J.resize(n);
281  for (int i=0; i<n; i++)
282  J[i] = diag[i].inverse();
283 
284  int i;
285  r = b;
286  mD(J,r,d);
287  double dn = r.dot(d);
288  double d0 = tol*dn;
289  if (abstol) // change tolerances
290  {
291  if (residual > d0) d0 = residual;
292  }
293 
294  for (i=0; i<iters; i++)
295  {
296  if (verbose && 0)
297  cout << "[BPCG] residual[" << i << "]: " << dn << " < " << d0 << endl;
298  if (dn < d0) break; // done
299  mMV2(diag,d,q);
300  double a = dn / d.dot(q);
301  x += a*d;
302  // TODO: reset residual here every 50 iterations
303  r -= a*q;
304  mD(J,r,s);
305  double dold = dn;
306  dn = r.dot(s);
307  double ba = dn / dold;
308  d = s + ba*d;
309  }
310 
311 
312  if (verbose)
313  cout << "[BPCG] residual[" << i << "]: " << dn << endl;
314  residual = dn/2.0;
315  return i;
316  }
317 
318 
319 #if 0
320 //
321 // matrix multiply of compressed column storage + diagonal blocks by a vector
322 //
323 
324 void
325 mMV(vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > &diag,
326  vector< map<int,Matrix<double,6,6>, less<int>, aligned_allocator<Matrix<double,6,6> > > > &cols,
327  const VectorXd &vin,
328  VectorXd &vout);
329 
330 //
331 // jacobi-preconditioned block conjugate gradient
332 // returns number of iterations
333 
334 int
335 bpcg_jacobi(int iters, double tol,
336  vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > &diag,
337  vector< map<int,Matrix<double,6,6>, less<int>, aligned_allocator<Matrix<double,6,6> > > > &cols,
338  VectorXd &x,
339  VectorXd &b,
340  bool abstol = false,
341  bool verbose = false
342  );
343 
344 int
345 bpcg_jacobi_dense(int iters, double tol,
346  MatrixXd &M,
347  VectorXd &x,
348  VectorXd &b);
349 
350 //
351 // jacobi-preconditioned block conjugate gradient
352 // returns number of iterations
353 
354 int
355 bpcg_jacobi3(int iters, double tol,
356  vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > &diag,
357  vector< map<int,Matrix<double,3,3>, less<int>, aligned_allocator<Matrix<double,3,3> > > > &cols,
358  VectorXd &x,
359  VectorXd &b,
360  bool abstol = false,
361  bool verbose = false
362  );
363 
364 int
365 bpcg_jacobi_dense3(int iters, double tol,
366  MatrixXd &M,
367  VectorXd &x,
368  VectorXd &b);
369 
370 #endif
371 
372 } // end namespace sba
373 
374 #endif // BPCG_H
d
d
sba::jacobiBPCG::vcols
vector< Matrix< double, N, N >, aligned_allocator< Matrix< double, N, N > > > vcols
Definition: bpcg.h:106
s
XmlRpcServer s
sba::jacobiBPCG::residual
double residual
Definition: bpcg.h:88
sba::jacobiBPCG
Let&#39;s try templated versions.
Definition: bpcg.h:65
sba
Definition: bpcg.h:60
sba::jacobiBPCG::jacobiBPCG
jacobiBPCG()
Definition: bpcg.h:68
sba::jacobiBPCG::vrind
vector< int > vrind
Definition: bpcg.h:105

sparse_bundle_adjustment
Author(s):
autogenerated on Mon Feb 28 2022 23:48:02