OrderedSparseMatrix.cpp
Go to the documentation of this file.
00001 
00028 #include <string>
00029 #include <cstring> // memset()
00030 #include <fstream>
00031 #include <iostream>
00032 #include <cmath>
00033 
00034 #include "isam/util.h"
00035 
00036 #include "isam/OrderedSparseMatrix.h"
00037 
00038 using namespace std;
00039 
00040 namespace isam {
00041 
00042 void OrderedSparseMatrix::_allocate_OrderedSparseMatrix(bool init_table) {
00043   _r_to_a = new int[_max_num_cols];
00044   _a_to_r = new int[_max_num_cols];
00045   if (init_table) {
00046     for (int col=0; col<_num_cols; col++) {
00047       // reset translation table
00048       _r_to_a[col] = col;
00049       _a_to_r[col] = col;
00050     }
00051   }
00052 }
00053 
00054 void OrderedSparseMatrix::_copy_from_OrderedSparseMatrix(const OrderedSparseMatrix& mat) {
00055   _allocate_OrderedSparseMatrix(false);
00056   memcpy(_r_to_a, mat._r_to_a, _num_cols*sizeof(int));
00057   memcpy(_a_to_r, mat._a_to_r, _num_cols*sizeof(int));
00058 }
00059 
00060 void OrderedSparseMatrix::_dealloc_OrderedSparseMatrix() {
00061   delete[] _r_to_a;
00062   _r_to_a = NULL; // recommended, as freeing a second time has unpredictable consequences
00063   delete[] _a_to_r;
00064   _a_to_r = NULL;
00065 }
00066 
00067 void OrderedSparseMatrix::_calc_reverse_order(int num, const int* order, int* reverse_order) const {
00068   for (int i=0; i<num; i++) {
00069     reverse_order[order[i]] = i;
00070   }
00071 }
00072 
00073 void OrderedSparseMatrix::_set_order(const int* r_to_a) {
00074   memcpy(_r_to_a, r_to_a, sizeof(int)*_num_cols);
00075   _calc_reverse_order(_num_cols, r_to_a, _a_to_r);
00076 }
00077 
00078 OrderedSparseMatrix::OrderedSparseMatrix(int num_rows, int num_cols) : SparseMatrix(num_rows, num_cols) {
00079   _allocate_OrderedSparseMatrix();
00080 }
00081 
00082 OrderedSparseMatrix::OrderedSparseMatrix(const OrderedSparseMatrix& mat) : SparseMatrix(mat) {
00083   _copy_from_OrderedSparseMatrix(mat);
00084 }
00085 
00086 OrderedSparseMatrix::OrderedSparseMatrix(const OrderedSparseMatrix& mat,
00087                                          int num_rows, int num_cols, int first_row, int first_col) :
00088     SparseMatrix(mat, num_rows, num_cols, first_row, first_col)
00089 {
00090   _allocate_OrderedSparseMatrix(); // note: ignores original ordering
00091 }
00092 
00093 OrderedSparseMatrix::OrderedSparseMatrix(int num_rows, int num_cols, SparseVector_p* rows) :
00094     SparseMatrix(num_rows, num_cols, rows)
00095 {
00096   _allocate_OrderedSparseMatrix();
00097 }
00098 
00099 OrderedSparseMatrix::~OrderedSparseMatrix() {
00100   _dealloc_OrderedSparseMatrix();
00101 }
00102 
00103 const OrderedSparseMatrix& OrderedSparseMatrix::operator= (const OrderedSparseMatrix& mat) {
00104   if (this==&mat)
00105     return *this;
00106 
00107   SparseMatrix::operator=(mat);
00108 
00109   _dealloc_OrderedSparseMatrix();
00110   _copy_from_OrderedSparseMatrix(mat);
00111 
00112   return *this;
00113 }
00114 
00115 void OrderedSparseMatrix::set_row(int row, const SparseVector& new_row) {
00116   // translate according to variable order:
00117   SparseVector reordered_row;
00118   for (SparseVectorIter iter(new_row); iter.valid(); iter.next()) {
00119     double val;
00120     int col = iter.get(val);
00121     int trans = a_to_r()[col];
00122     reordered_row.set(trans, val);
00123   }
00124   SparseMatrix::set_row(row, reordered_row);
00125 }
00126 
00127 void OrderedSparseMatrix::import_rows_ordered(int num_rows, int num_cols, SparseVector_p* rows, int* r_to_a) {
00128   _dealloc_OrderedSparseMatrix();
00129   SparseMatrix::import_rows(num_rows, num_cols, rows);
00130   _allocate_OrderedSparseMatrix();
00131   _set_order(r_to_a);
00132 }
00133 
00134 void OrderedSparseMatrix::append_new_cols(int num) {
00135   int orig_num_cols = _num_cols;
00136   int orig_max_num_cols = _max_num_cols;
00137 
00138   SparseMatrix::append_new_cols(num); // _num_cols and _max_num_cols
00139 
00140   if (orig_max_num_cols != _max_num_cols) { // resize arrays if needed
00141     int* new_r_to_a = new int[_max_num_cols];
00142     int* new_a_to_r = new int[_max_num_cols];
00143     memcpy(new_r_to_a, _r_to_a, orig_max_num_cols*sizeof(int));
00144     memcpy(new_a_to_r, _a_to_r, orig_max_num_cols*sizeof(int));
00145     delete[] _r_to_a;
00146     delete[] _a_to_r;
00147     _r_to_a = new_r_to_a;
00148     _a_to_r = new_a_to_r;
00149   }
00150   int next_index = orig_num_cols;
00151   for (int col=orig_num_cols; col<orig_num_cols+num; col++) {
00152     _r_to_a[col] = next_index;
00153     _a_to_r[next_index] = col;
00154     next_index++;
00155   }
00156 }
00157 
00158 const int* OrderedSparseMatrix::a_to_r() const {
00159   return _a_to_r;
00160 }
00161 
00162 const int* OrderedSparseMatrix::r_to_a() const {
00163   return _r_to_a;
00164 }
00165 
00166 }


demo_lidar
Author(s): Ji Zhang
autogenerated on Sun Mar 1 2015 11:30:50