Skip to content
Snippets Groups Projects
Commit be4607a4 authored by jvallve's avatar jvallve
Browse files

buidat de classe qr_solver de test_iQ2_wolf2

parent b5501fde
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 757 additions and 729 deletions
src/capture_odom_2D.cpp
+ 5
− 3
View file @ be4607a4
......@@ -10,9 +10,9 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, cons
CaptureRelative(_ts, _sensor_ptr, _data)
{
data_covariance_ = Eigen::Matrix3s::Zero();
data_covariance_(0, 0) = std::max(1e-6, fabs(_data(0)) * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor());
data_covariance_(1, 1) = std::max(1e-6, fabs(_data(1)) * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor());
data_covariance_(2, 2) = std::max(1e-6, fabs(_data(2)) * ((SensorOdom2D*) _sensor_ptr)->getRotationNoiseFactor());
data_covariance_(0, 0) = std::max(1e-10, _data(0) * _data(0) * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor() * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor());
data_covariance_(1, 1) = std::max(1e-10, _data(1) * _data(1) * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor() * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor());
data_covariance_(2, 2) = std::max(1e-10, _data(2) * _data(2) * ((SensorOdom2D*) _sensor_ptr)->getRotationNoiseFactor() * ((SensorOdom2D*) _sensor_ptr)->getRotationNoiseFactor());
// std::cout << data_covariance_ << std::endl;
}
......@@ -100,6 +100,8 @@ void CaptureOdom2D::integrateCapture(CaptureRelative* _new_capture)
data_(0) += (_new_capture->getData()(0) * cos(data_(2)) - _new_capture->getData()(1) * sin(data_(2)));
data_(1) += (_new_capture->getData()(0) * sin(data_(2)) + _new_capture->getData()(1) * cos(data_(2)));
data_(2) += _new_capture->getData()(2);
// TODO Jacobians!
data_covariance_ += _new_capture->getDataCovariance();
getFeatureListPtr()->front()->setMeasurement(data_);
......
src/examples/test_ccolamd_blocks.cpp
+ 7
− 7
View file @ be4607a4
......@@ -26,7 +26,7 @@
using namespace Eigen;
void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols)
void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols)
{
for (uint i = row; i < row + Nrows; i++)
for (uint j = col; j < row + Ncols; j++)
......@@ -35,7 +35,7 @@ void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row,
original.makeCompressed();
}
void add_sparse_block(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
{
for (uint r=0; r<ins.rows(); ++r)
for (uint c = 0; c < ins.cols(); c++)
......@@ -84,19 +84,19 @@ int main(int argc, char *argv[])
//Fill H & b
for (unsigned int i = 0; i < size; i++)
{
add_sparse_block(5*omega, H, i*dim, i*dim);
addSparseBlock(5*omega, H, i*dim, i*dim);
FactorMatrix.insert(i,i) = 1;
if (i > 0)
{
add_sparse_block(omega, H, i*dim, (i-1)*dim);
add_sparse_block(omega, H, (i-1)*dim, i*dim);
addSparseBlock(omega, H, i*dim, (i-1)*dim);
addSparseBlock(omega, H, (i-1)*dim, i*dim);
FactorMatrix.insert(i,i-1) = 1;
FactorMatrix.insert(i-1,i) = 1;
}
b.segment(i*dim, dim) = VectorXd::Constant(dim, i+1);
}
add_sparse_block(2*omega, H, 0, (size - 1)*dim);
add_sparse_block(2*omega, H, (size-1)*dim, 0);
addSparseBlock(2*omega, H, 0, (size - 1)*dim);
addSparseBlock(2*omega, H, (size-1)*dim, 0);
FactorMatrix.insert(0,size-1) = 1;
FactorMatrix.insert(size-1,0) = 1;
......
src/examples/test_iQR.cpp
+ 6
− 6
View file @ be4607a4
......@@ -51,7 +51,7 @@ class block_pruning
}
};
void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
{
// prune all non-zero elements that not satisfy the 'keep' operand
// elements that are not in the block rows or are not in the block columns should be kept
......@@ -67,7 +67,7 @@ void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row,
// original.prune(0);
}
void add_sparse_block(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
{
for (uint r=0; r<ins.rows(); ++r)
for (uint c = 0; c < ins.cols(); c++)
......@@ -197,8 +197,8 @@ int main(int argc, char *argv[])
{
int ordered_node = acc_permutation_nodes_matrix.indices()(measurement.at(j));
add_sparse_block(2*omega, A, A.rows()-dim, measurement.at(j) * dim);
add_sparse_block(2*omega, A_ordered, A_ordered.rows()-dim, ordered_node * dim);
addSparseBlock(2*omega, A, A.rows()-dim, measurement.at(j) * dim);
addSparseBlock(2*omega, A_ordered, A_ordered.rows()-dim, ordered_node * dim);
A_nodes_ordered.coeffRef(A_nodes_ordered.rows()-1, ordered_node) = 1;
......@@ -274,8 +274,8 @@ int main(int argc, char *argv[])
x_ordered_partial.tail(ordered_nodes * dim) = solver_ordered_partial.solve(b.tail(ordered_nodes * dim));
std::cout << "x_ordered_partial.tail(ordered_nodes * dim)" << std::endl << x_ordered_partial.tail(ordered_nodes * dim).transpose() << std::endl;
// store new part of R (equivalent to R.bottomRightCorner(ordered_nodes * dim, ordered_nodes * dim) = solver3.matrixR();)
erase_sparse_block(R, unordered_nodes * dim, unordered_nodes * dim, ordered_nodes * dim, ordered_nodes * dim);
add_sparse_block(solver_ordered_partial.matrixR(), R, unordered_nodes * dim, unordered_nodes * dim);
eraseSparseBlock(R, unordered_nodes * dim, unordered_nodes * dim, ordered_nodes * dim, ordered_nodes * dim);
addSparseBlock(solver_ordered_partial.matrixR(), R, unordered_nodes * dim, unordered_nodes * dim);
std::cout << "R" << std::endl << MatrixXd::Identity(R.rows(), R.rows()) * R << std::endl;
R.makeCompressed();
......
src/examples/test_iQR_wolf.cpp
+ 41
− 41
View file @ be4607a4
......@@ -45,7 +45,7 @@ class block_pruning
}
};
void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
{
// prune all non-zero elements that not satisfy the 'keep' operand
// elements that are not in the block rows or are not in the block columns should be kept
......@@ -61,7 +61,7 @@ void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row,
// original.prune(0);
}
void add_sparse_block(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
{
for (uint r=0; r<ins.rows(); ++r)
for (uint c = 0; c < ins.cols(); c++)
......@@ -134,10 +134,10 @@ class SolverQR
// ordering
SparseMatrix<int> A_nodes_;
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_;
PermutationMatrix<Dynamic, Dynamic, int> acc_node_permutation_;
CCOLAMDOrdering<int> ordering_;
VectorXi nodes_ordering_constraints_;
CCOLAMDOrdering<int> orderer_;
VectorXi node_ordering_restrictions_;
int first_ordered_node_;
// time
......@@ -154,7 +154,7 @@ class SolverQR
n_measurements(0),
n_nodes_(0),
A_nodes_(0,0),
acc_permutation_nodes_(0),
acc_node_permutation_(0),
// nodes_(0),
// measurements_(0),
time_ordering_(0),
......@@ -177,7 +177,7 @@ class SolverQR
nodes_.push_back(node(node_idx, node_dim, x_incr_.size(), n_nodes_-1));
// Resize accumulated permutations
augment_permutation(acc_permutation_nodes_, n_nodes_);
augment_permutation(acc_node_permutation_, n_nodes_);
// Resize state
x_incr_.conservativeResize(x_incr_.size() + node_dim);
......@@ -210,11 +210,11 @@ class SolverQR
first_ordered_node_ = n_nodes_;
for (unsigned int j = 0; j < _meas.nodes_idx.size(); j++)
{
assert(acc_permutation_nodes_.indices()(_meas.nodes_idx.at(j)) == nodes_.at(_meas.nodes_idx.at(j)).order);
assert(acc_node_permutation_.indices()(_meas.nodes_idx.at(j)) == nodes_.at(_meas.nodes_idx.at(j)).order);
int ordered_node = nodes_.at(_meas.nodes_idx.at(j)).order;//acc_permutation_nodes_.indices()(_nodes_idx.at(j));
add_sparse_block(_meas.jacobians.at(j), A_, A_.rows()-_meas.dim, nodes_.at(_meas.nodes_idx.at(j)).location);
addSparseBlock(_meas.jacobians.at(j), A_, A_.rows()-_meas.dim, nodes_.at(_meas.nodes_idx.at(j)).location);
A_nodes_.coeffRef(A_nodes_.rows()-1, ordered_node) = 1;
......@@ -241,24 +241,24 @@ class SolverQR
if (_first_ordered_node == 0)
{
// ordering ordering constraints
nodes_ordering_constraints_.resize(n_nodes_);
nodes_ordering_constraints_ = A_nodes_.bottomRows(1).transpose();
node_ordering_restrictions_.resize(n_nodes_);
node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
// computing nodes partial ordering_
A_nodes_.makeCompressed();
PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(n_nodes_);
ordering_(A_nodes_, incr_permutation_nodes, nodes_ordering_constraints_.data());
orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data());
// node ordering to variable ordering
PermutationMatrix<Dynamic, Dynamic, int> incr_permutation(A_.cols());
permutation_2_block_permutation(incr_permutation_nodes, incr_permutation);
nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation);
// apply partial_ordering orderings
A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
A_ = (A_ * incr_permutation.transpose()).sparseView();
// ACCUMULATING PERMUTATIONS
accumulate_permutation(incr_permutation_nodes);
accumulatePermutation(incr_permutation_nodes);
}
// partial ordering
......@@ -273,17 +273,17 @@ class SolverQR
SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
// _partial_ordering ordering_ constraints
nodes_ordering_constraints_.resize(ordered_nodes);
nodes_ordering_constraints_ = sub_A_nodes_.bottomRows(1).transpose();
node_ordering_restrictions_.resize(ordered_nodes);
node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
// computing nodes partial ordering_
sub_A_nodes_.makeCompressed();
PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes(ordered_nodes);
ordering_(sub_A_nodes_, partial_permutation_nodes, nodes_ordering_constraints_.data());
orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data());
// node ordering to variable ordering
PermutationMatrix<Dynamic, Dynamic, int> partial_permutation(A_.cols());
permutation_2_block_permutation(partial_permutation_nodes, partial_permutation);
nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation);
// apply partial_ordering orderings
int ordered_variables = A_.cols() - nodes_.at(_first_ordered_node).location;
......@@ -292,7 +292,7 @@ class SolverQR
R_.rightCols(ordered_variables) = (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
// ACCUMULATING PERMUTATIONS
accumulate_permutation(partial_permutation_nodes);
accumulatePermutation(partial_permutation_nodes);
}
}
time_ordering_ += ((double) clock() - t_ordering_) / CLOCKS_PER_SEC;
......@@ -358,9 +358,9 @@ class SolverQR
x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements));
// store new part of R
erase_sparse_block(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
add_sparse_block(solver_.matrixR(), R_, unordered_variables, unordered_variables);
addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
R_.makeCompressed();
......@@ -384,7 +384,7 @@ class SolverQR
}
// UNDO ORDERING FOR RESULT
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
permutation_2_block_permutation(acc_permutation_nodes_, acc_permutation); // TODO via pointers
nodePermutation2VariablesPermutation(acc_node_permutation_, acc_permutation); // TODO via pointers
x_incr_ = acc_permutation.inverse() * x_incr_;
time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
......@@ -393,7 +393,7 @@ class SolverQR
}
void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables)
void nodePermutation2VariablesPermutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables)
{
ArrayXi locations = perm_nodes_2_locations(_perm_nodes);
......@@ -426,48 +426,48 @@ class SolverQR
perm.indices() = new_indices;
}
void accumulate_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm)
void accumulatePermutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm)
{
print_name();
printName();
//std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
//std::cout << "incr perm " << perm.indices().transpose() << std::endl;
// acumulate permutation
if (perm.size() == acc_permutation_nodes_.size()) //full permutation
acc_permutation_nodes_ = perm * acc_permutation_nodes_;
if (perm.size() == acc_node_permutation_.size()) //full permutation
acc_node_permutation_ = perm * acc_node_permutation_;
else //partial permutation
{
PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(VectorXi::LinSpaced(n_nodes_, 0, n_nodes_ - 1)); // identity permutation
incr_permutation_nodes.indices().tail(perm.size()) = perm.indices() + VectorXi::Constant(perm.size(), n_nodes_ - perm.size());
//std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl;
acc_permutation_nodes_ = incr_permutation_nodes * acc_permutation_nodes_;
acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_;
}
//std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
// update nodes orders and locations
ArrayXi locations = perm_nodes_2_locations(acc_permutation_nodes_);
ArrayXi locations = perm_nodes_2_locations(acc_node_permutation_);
for (int i = 0; i < nodes_.size(); i++)
{
nodes_.at(i).order = acc_permutation_nodes_.indices()(i);
nodes_.at(i).order = acc_node_permutation_.indices()(i);
nodes_.at(i).location = locations(i);
}
}
void print_name()
void printName()
{
std::cout << name_;
}
void print_results()
void printResults()
{
print_name();
printName();
std::cout << " solved in " << time_solving_*1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"<< std::endl;
std::cout << "x = " << x_incr_.transpose() << std::endl;
}
void print_problem()
void printProblem()
{
print_name();
printName();
std::cout << std::endl << "A_nodes_: " << std::endl << MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl << std::endl;
std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl;
std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl;
......@@ -535,9 +535,9 @@ int main(int argc, char *argv[])
solver_ordered_partial.addConstraint(measurements.at(i));
// PRINT PROBLEM
solver_unordered.print_problem();
solver_ordered.print_problem();
solver_ordered_partial.print_problem();
solver_unordered.printProblem();
solver_ordered.printProblem();
solver_ordered_partial.printProblem();
// SOLVING
solver_unordered.solve(0);
......@@ -546,9 +546,9 @@ int main(int argc, char *argv[])
// RESULTS ------------------------------------
std::cout << "========================= RESULTS " << i << ":" << std::endl;
solver_unordered.print_results();
solver_ordered.print_results();
solver_ordered_partial.print_results();
solver_unordered.printResults();
solver_ordered.printResults();
solver_ordered_partial.printResults();
// if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10)
// std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff() << ")" << std::endl;
......
src/examples/test_iQR_wolf2.cpp
+ 23
− 659
View file @ be4607a4
This diff is collapsed.
src/examples/test_incremental_ccolamd_blocks.cpp
+ 8
− 8
View file @ be4607a4
......@@ -26,7 +26,7 @@
using namespace Eigen;
void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols)
void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols)
{
for (uint i = row; i < row + Nrows; i++)
for (uint j = col; j < row + Ncols; j++)
......@@ -35,7 +35,7 @@ void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row,
original.makeCompressed();
}
void add_sparse_block(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
{
for (uint r=0; r<ins.rows(); ++r)
for (uint c = 0; c < ins.cols(); c++)
......@@ -101,7 +101,7 @@ int main(int argc, char *argv[])
double time1=0, time2=0, time3=0;
// INITIAL STATE
add_sparse_block(5*omega, H, 0, 0);
addSparseBlock(5*omega, H, 0, 0);
factors.insert(0,0) = 1;
b.head(dim) = VectorXd::LinSpaced(Sequential, dim, 0, dim-1);
......@@ -124,9 +124,9 @@ int main(int argc, char *argv[])
factors.conservativeResize(i+1, i+1);
// Odometry
add_sparse_block(5*omega, H, i*dim, i*dim);
add_sparse_block(omega, H, i*dim, (i-1)*dim);
add_sparse_block(omega, H, (i-1)*dim, i*dim);
addSparseBlock(5*omega, H, i*dim, i*dim);
addSparseBlock(omega, H, i*dim, (i-1)*dim);
addSparseBlock(omega, H, (i-1)*dim, i*dim);
factors.insert(i,i) = 1;
factors.insert(i,i-1) = 1;
factors.insert(i-1,i) = 1;
......@@ -134,8 +134,8 @@ int main(int argc, char *argv[])
// Loop Closure
if (i == size-1)
{
add_sparse_block(2*omega, H, 0, i*dim);
add_sparse_block(2*omega, H, i*dim, 0);
addSparseBlock(2*omega, H, 0, i*dim);
addSparseBlock(2*omega, H, i*dim, 0);
factors.insert(0,i) = 1;
factors.insert(i,0) = 1;
}
......
src/solver/qr_solver.h 0 → 100644
+ 613
− 0
View file @ be4607a4
This diff is collapsed.
src/solver/sparse_utils.h 0 → 100644
+ 49
− 0
View file @ be4607a4
/*
* sparse_utils.h
*
* Created on: Jul 2, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_SOLVER_SPARSE_UTILS_H_
#define TRUNK_SRC_SOLVER_SPARSE_UTILS_H_
// eigen includes
#include <eigen3/Eigen/Sparse>
class SparseBlockPruning
{
public:
unsigned int col, row, Nrows, Ncols;
SparseBlockPruning(unsigned int _col, unsigned int _row, unsigned int _Nrows, unsigned int _Ncols) :
col(_col),
row(_row),
Nrows(_Nrows),
Ncols(_Ncols)
{
//
}
bool operator()(unsigned int i, unsigned int j, double) const
{
return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1);
}
};
void eraseSparseBlock(Eigen::SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
{
// prune all non-zero elements that not satisfy the 'keep' operand
// elements that are not in the block rows or are not in the block columns should be kept
SparseBlockPruning bp(row, col, Nrows, Ncols);
original.prune(bp);
}
void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
{
for (unsigned int r=0; r<ins.rows(); ++r)
for (unsigned int c = 0; c < ins.cols(); c++)
if (ins(r,c) != 0)
original.coeffRef(r + row, c + col) += ins(r,c);
}
#endif /* TRUNK_SRC_SOLVER_SPARSE_UTILS_H_ */
src/wolf_manager.h
+ 5
− 5
View file @ be4607a4
......@@ -138,7 +138,7 @@ class WolfManager
//std::cout << "last_frame_" << std::endl;
// Fixing or removing old frames
manage_window();
manageWindow();
std::cout << "new frame created" << std::endl;
}
......@@ -155,7 +155,7 @@ class WolfManager
_capture->getTimeStamp().print();
std::cout << std::endl;
}
void manage_window()
void manageWindow()
{
std::cout << "managing window..." << std::endl;
// WINDOW of FRAMES (remove or fix old frames)
......@@ -169,7 +169,7 @@ class WolfManager
std::cout << "window managed" << std::endl;
}
bool check_new_frame(CaptureBase* new_capture)
bool checkNewFrame(CaptureBase* new_capture)
{
std::cout << "checking if new frame..." << std::endl;
// TODO: not only time, depending on the sensor...
......@@ -198,7 +198,7 @@ class WolfManager
{
std::cout << "adding odometry capture..." << new_capture->nodeId() << std::endl;
// NEW KEY FRAME ?
if (check_new_frame(new_capture))
if (checkNewFrame(new_capture))
createFrame(new_capture->getTimeStamp());
// ADD/INTEGRATE NEW ODOMETRY TO THE LAST FRAME
......@@ -217,7 +217,7 @@ class WolfManager
{
std::cout << "adding not odometry capture..." << new_capture->nodeId() << std::endl;
// NEW KEY FRAME ?
if (check_new_frame(new_capture))
if (checkNewFrame(new_capture))
createFrame(new_capture->getTimeStamp());
// ADD CAPTURE TO THE CURRENT FRAME (or substitute the same sensor previous capture)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment