diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index 6ca7d84363634c8e9e4dea24c97c0582bfa31345..12ad327fe25930ff0efe21bb564a0de8139f91ac 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -135,9 +135,9 @@ int main(int argc, char** argv) string modelFileName; //model and initial view point - modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"; + //modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"; //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj"; - //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj"; + modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj"; devicePose.setPose(2,8,0.2,0,0,0); viewPoint.setPose(devicePose); viewPoint.moveForward(10); @@ -176,7 +176,7 @@ int main(int argc, char** argv) ground_truth.head(3) = pose_odom; odom_trajectory.head(3) = pose_odom; - WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e9, pose_odom, 0.3, window_size); + WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e6, pose_odom, 0.3, window_size); //std::cout << "START TRAJECTORY..." << std::endl; // START TRAJECTORY ============================================================================================ diff --git a/src/examples/test_ceres_laser.cpp b/src/examples/test_ceres_laser.cpp index 424b5c535bd63d648ed6682eaecb34c98bc068d2..cf8375e6dfd842e8406c0ca9ae3e9842a2359515 100644 --- a/src/examples/test_ceres_laser.cpp +++ b/src/examples/test_ceres_laser.cpp @@ -318,9 +318,9 @@ int main(int argc, char** argv) string modelFileName; //model and initial view point - modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"; + //modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"; //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj"; - //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj"; + modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj"; devicePose.setPose(2,8,0.2,0,0,0); //glut initialization //glutInit(&argc, argv); diff --git a/src/landmark_base.h b/src/landmark_base.h index c23d4fe98d947f17e0b1064ea4483091c2fa3e05..33d678bb8db027b6de29bb3be113e9864b464c70 100644 --- a/src/landmark_base.h +++ b/src/landmark_base.h @@ -18,6 +18,12 @@ class NodeTerminus; #include "node_linked.h" #include "map_base.h" +// why v, w and a ? +// add descriptor as a StateBase -> Could be estimated or not. Aperture could be one case of "descriptor"that can be estimated or not +// orientation ? -> Is it also a descriptor ? +// init and end Time stamps +// + //class LandmarkBase class LandmarkBase : public NodeLinked<MapBase,NodeTerminus> { diff --git a/src/map_base.cpp b/src/map_base.cpp index f892f9da2a1d5e586361db28e0e774b3a2cf03c9..02d80921512556db14063fe55a371a5667bda7f2 100644 --- a/src/map_base.cpp +++ b/src/map_base.cpp @@ -3,7 +3,7 @@ MapBase::MapBase() : NodeLinked(MID, "MAP") { - // + std::cout << "MapBase::MapBase(): " << __LINE__ << std::endl; } MapBase::~MapBase() diff --git a/src/node_base.cpp b/src/node_base.cpp index 8423ac3c7ccb1c9b4e188111dc4fcc276dff2aaa..57bcc414c424c5d4fa0980879515acb95209fadb 100644 --- a/src/node_base.cpp +++ b/src/node_base.cpp @@ -3,12 +3,13 @@ //init static node counter unsigned int NodeBase::node_id_count_ = 0; -NodeBase::NodeBase(std::string _label) : - label_(_label), // +NodeBase::NodeBase(std::string _label, bool _verbose) : + label_(_label), node_id_(++node_id_count_), - node_pending_(ADD_PENDING) + node_pending_(ADD_PENDING), + verbose_(_verbose) { - // std::cout << "NodeID::constructor. Id: " << node_id_ << std::endl; + if (verbose_) std::cout << "NodeBase::NodeBase(). Id: " << node_id_ << " Label: " << label_ << std::endl; } NodeBase::~NodeBase() diff --git a/src/node_base.h b/src/node_base.h index afe5b7621655c5b18755ed7d2bbb581f45ad686f..0716fff5118be42392977308365732eb047714fa 100644 --- a/src/node_base.h +++ b/src/node_base.h @@ -19,6 +19,7 @@ class NodeBase unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory) PendingStatus node_pending_; ///< Pending to be added/updated in the filter or optimizer + bool verbose_; public: @@ -27,7 +28,7 @@ class NodeBase * Constructor from label * */ - NodeBase(std::string _label); + NodeBase(std::string _label, bool _verbose = false); /** \brief Default destructor * diff --git a/src/wolf_problem.cpp b/src/wolf_problem.cpp index d60d725e793942300a2613c5ddcc654138b9cf1e..73e45e1f4b0d39123c823e1a216f939cf056be79 100644 --- a/src/wolf_problem.cpp +++ b/src/wolf_problem.cpp @@ -11,8 +11,11 @@ WolfProblem::WolfProblem(unsigned int _size) : { // map_ptr_ = new MapBase; // trajectory_ptr_ = new TrajectoryBase; + std::cout << "WolfProblem::WolfProblem(): " << __LINE__ << std::endl; map_ptr_->linkToUpperNode( this ); + std::cout << "WolfProblem::WolfProblem(): " << __LINE__ << std::endl; trajectory_ptr_->linkToUpperNode( this ); + std::cout << "WolfProblem::WolfProblem(): " << __LINE__ << std::endl; } WolfProblem::WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr, unsigned int _size) :