i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.11.2: First Google Tango release

Browse Source
This commit is contained in:
matlabbe
2016-02-15 19:35:24 -05:00
parent 8dc01c491b
commit c6a46a7238
415 changed files with 73300 additions and 3193 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
corelib/src/toro3d/treeoptimizer3.cpp
View File

@@ -50,8 +50,8 @@ using namespace std;
namespace AISNavigation {
#define DEBUG(i) \
if (verboseLevel>i) cerr
//#define DEBUG(i) \
// if (verboseLevel>i) cerr
TreeOptimizer3::TreeOptimizer3(){
@@ -180,7 +180,7 @@ double TreeOptimizer3::error(const Edge* e) const{
Pose ps=e->informationMatrix*p12;
double err=p12*ps;
DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
//DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
return err;
}
@@ -294,10 +294,10 @@ void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
edgeCompareMode=mode;
// compute the size of the preconditioning matrix
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
//DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
//DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
// sorting edges
@@ -313,9 +313,9 @@ void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
void TreeOptimizer3::initializeOnlineIterations(){
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
//DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
//DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
maxRotationalErrors.clear();
maxTranslationalErrors.clear();
@@ -337,18 +337,18 @@ void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
}
void TreeOptimizer3::onStepStart(Edge* e){
DEBUG(5) << "entering edge" << e << endl;
//DEBUG(5) << "entering edge" << e << endl;
}
void TreeOptimizer3::onStepFinished(Edge* e){
DEBUG(5) << "exiting edge" << e << endl;
//DEBUG(5) << "exiting edge" << e << endl;
}
void TreeOptimizer3::onIterationStart(int iteration){
DEBUG(5) << "entering iteration " << iteration << endl;
//DEBUG(5) << "entering iteration " << iteration << endl;
}
void TreeOptimizer3::onIterationFinished(int iteration){
DEBUG(5) << "exiting iteration " << iteration << endl;
//DEBUG(5) << "exiting iteration " << iteration << endl;
}
void TreeOptimizer3::onRestartBegin(){}