From bbeebe50110bc4dc7e4b1e7965bec39aa645e128 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Thu, 12 Mar 2015 17:14:53 +0000
Subject: [PATCH] =?UTF-8?q?canvis=20a=20odom=20capture=20afegint=20factor?=
=?UTF-8?q?=20de=20soroll=20i=20no=20constant,=20canvis=20als=20cmakes=20i?=
=?UTF-8?q?=20dem=C3=A9s=20per=20instalaci=C3=B3=20correcte=20i=20wolf=5Fm?=
=?UTF-8?q?anager=20a=20arxiu=20propi?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
CMakeLists.txt | 3 +-
Findwolf.cmake | 3 +-
src/CMakeLists.txt | 19 +-
src/capture_base.cpp | 2 +-
src/capture_laser_2D.cpp | 42 ++---
src/capture_odom_2D.cpp | 5 +-
src/capture_odom_2D.h | 21 ++-
src/ceres_wrapper/ceres_manager.cpp | 3 +
src/constraint_base.cpp | 2 +-
src/constraint_corner_2D_theta.h | 2 +-
src/examples/test_ceres_2_lasers.cpp | 268 ++++++---------------------
src/feature_base.cpp | 2 +-
src/frame_base.cpp | 2 +-
src/landmark_base.cpp | 2 +-
src/landmark_base.h | 114 ++++++------
src/map_base.cpp | 2 +-
src/sensor_base.cpp | 33 ++--
src/sensor_base.h | 41 ++--
src/state_base.cpp | 2 +-
src/time_stamp.cpp | 6 +
src/time_stamp.h | 7 +
src/trajectory_base.cpp | 2 +-
src/wolf_manager.h | 173 +++++++++++++++++
src/wolf_problem.cpp | 2 +-
24 files changed, 404 insertions(+), 354 deletions(-)
create mode 100644 src/wolf_manager.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5e8794fb4..e5b9c69c9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -95,7 +95,6 @@ IF (UNIX)
ELSE(UNIX)
ADD_CUSTOM_COMMAND(
COMMENT "packaging only implemented in unix"
- TARGET uninstall
- )
+ TARGET uninstall)
ENDIF(UNIX)
diff --git a/Findwolf.cmake b/Findwolf.cmake
index e54db6d71..4a0c6881a 100644
--- a/Findwolf.cmake
+++ b/Findwolf.cmake
@@ -2,8 +2,7 @@
FIND_PATH(
wolf_INCLUDE_DIR
NAMES wolf.h
- PATHS /usr/local/include/iri-algorithms
-)
+ PATHS /usr/local/include/iri-algorithms/wolf)
FIND_LIBRARY(
wolf_LIBRARY
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 88ff12b8d..88c79484d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -47,6 +47,7 @@ SET(HDRS
constraint_gps_2D.h
constraint_odom_2D_theta.h
constraint_odom_2D_complex_angle.h
+ constraint_corner_2D_theta.h
feature_base.h
feature_corner_2D.h
feature_gps_fix.h
@@ -70,7 +71,8 @@ SET(HDRS
time_stamp.h
trajectory_base.h
wolf.h
- wolf_problem.h)
+ wolf_problem.h
+ wolf_manager.h)
#sources
SET(SRCS
@@ -102,16 +104,19 @@ SET(SRCS
#optional HDRS and SRCS
IF (Ceres_FOUND)
- SET(HDRS ${HDRS}
+ SET(HDRS_WRAPPER
ceres_wrapper/complex_angle_parametrization.h
ceres_wrapper/ceres_manager.h )
- SET(SRCS ${SRCS}
+ SET(SRCS_WRAPPER
ceres_wrapper/complex_angle_parametrization.cpp
ceres_wrapper/ceres_manager.cpp)
+ELSE(Ceres_FOUND)
+ SET(HDRS_WRAPPER)
+ SET(SRCS_WRAPPER)
ENDIF(Ceres_FOUND)
IF (laser_scan_utils_FOUND)
- SET(HDRS ${HDRS}
+ SET(HDRS ${HDRS}
capture_laser_2D.h
sensor_laser_2D.h)
SET(SRCS ${SRCS}
@@ -120,7 +125,7 @@ IF (laser_scan_utils_FOUND)
ENDIF(laser_scan_utils_FOUND)
# create the shared library
-ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS})
+ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS} ${SRCS_WRAPPER})
#Link the created library with ceres
IF (Ceres_FOUND)
@@ -140,7 +145,9 @@ INSTALL(TARGETS ${PROJECT_NAME}
#install headers
INSTALL(FILES ${HDRS}
- DESTINATION include/iri-algorithms)
+ DESTINATION include/iri-algorithms/wolf)
+INSTALL(FILES ${HDRS_WRAPPER}
+ DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
INSTALL(FILES ../Findwolf.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index e0cec08b2..60cb39225 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -29,7 +29,7 @@ CaptureBase::CaptureBase(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Ei
CaptureBase::~CaptureBase()
{
- std::cout << "deleting CaptureBase " << nodeId() << std::endl;
+ //std::cout << "deleting CaptureBase " << nodeId() << std::endl;
}
void CaptureBase::linkToFrame(FrameBase* _frm_ptr)
diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index d8c6461ce..4c51f0ce3 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -111,10 +111,10 @@ void CaptureLaser2D::establishConstraints()
fmod(feature_global_orientation+M_PI, 2 * M_PI)-M_PI :
fmod(feature_global_orientation-M_PI, 2 * M_PI)+M_PI);
- std::cout << "-------- Feature: " << (*feature_it)->nodeId() << std::endl <<
- feature_global_position.transpose() <<
- "\t" << feature_global_orientation <<
- "\t" << feature_aperture << std::endl;
+// std::cout << "-------- Feature: " << (*feature_it)->nodeId() << std::endl <<
+// feature_global_position.transpose() <<
+// "\t" << feature_global_orientation <<
+// "\t" << feature_aperture << std::endl;
for (auto landmark_it = getTop()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != getTop()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ )
{
Eigen::Map<Eigen::Vector2s> landmark_position((*landmark_it)->getPPtr()->getPtr());
@@ -133,39 +133,35 @@ void CaptureLaser2D::establishConstraints()
if (distance_sq < min_distance_sq)
{
- std::cout << "Close landmark candidate: " << (*landmark_it)->nodeId() << std::endl <<
- landmark_position.transpose() <<
- "\t" << landmark_orientation <<
- "\t" << landmark_aperture << std::endl;
+// std::cout << "Close landmark candidate: " << (*landmark_it)->nodeId() << std::endl <<
+// landmark_position.transpose() <<
+// "\t" << landmark_orientation <<
+// "\t" << landmark_aperture << std::endl;
//std::cout << " OK!" << std::endl;
// Third check: ORIENTATION
WolfScalar theta_distance = fabs(fmod(fabs(landmark_orientation-feature_global_orientation)+M_PI, 2 * M_PI)-M_PI);// fit in (-pi, pi]
- if (theta_distance < 0 || theta_distance > M_PI)
- std::cout << "/////////////////////////////////////// theta_distance < 0 || theta_distance > M_PI" << std::endl
- << landmark_orientation << "\t" << feature_global_orientation << std::endl;
-
- std::cout << " orientation diff: " << theta_distance;
+// std::cout << " orientation diff: " << theta_distance;
if (theta_distance < max_theta_matching)
{
- std::cout << " OK!" << std::endl;
- std::cout << "Closer landmark found (satisfying orientation and aperture)" << std::endl;
+// std::cout << " OK!" << std::endl;
+// std::cout << "Closer landmark found (satisfying orientation and aperture)" << std::endl;
correspondent_landmark = (LandmarkCorner2D*)(*landmark_it);
min_distance_sq = distance_sq;
}
- else
- std::cout << " KO" << std::endl;
- }
+// else
+// std::cout << " KO" << std::endl;
// else
// std::cout << " KO" << std::endl;
+ }
}
// else
// std::cout << " KO" << std::endl;
}
if (correspondent_landmark == nullptr)
{
- std::cout << "+++++ No landmark found. Creating a new one..." << std::endl;
+// std::cout << "+++++ No landmark found. Creating a new one..." << std::endl;
StateBase* new_landmark_state_position = new StatePoint2D(getTop()->getNewStatePtr());
getTop()->addState(new_landmark_state_position, feature_global_position);
StateBase* new_landmark_state_orientation = new StateTheta(getTop()->getNewStatePtr());
@@ -175,10 +171,10 @@ void CaptureLaser2D::establishConstraints()
LandmarkBase* corr_landmark(correspondent_landmark);
getTop()->getMapPtr()->addLandmark(corr_landmark);
- std::cout << "Landmark created: " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << std::endl <<
- feature_global_position.transpose() <<
- "\t" << feature_global_orientation <<
- "\t" << feature_aperture << std::endl;
+// std::cout << "Landmark created: " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << std::endl <<
+// feature_global_position.transpose() <<
+// "\t" << feature_global_orientation <<
+// "\t" << feature_aperture << std::endl;
}
else
correspondent_landmark->hit();
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index b4736054c..884d68c45 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -9,7 +9,10 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr) :
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data) :
CaptureRelative(_ts, _sensor_ptr, _data)
{
- //
+ data_covariance_ = Eigen::Matrix2s::Zero();
+ data_covariance_(0,0) = _data(0)*((SensorOdom2D*)_sensor_ptr)->getDisplacementNoiseFactor();
+ data_covariance_(1,1) = _data(1)*((SensorOdom2D*)_sensor_ptr)->getRotationNoiseFactor();
+ std::cout << data_covariance_ << std::endl;
}
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
diff --git a/src/capture_odom_2D.h b/src/capture_odom_2D.h
index 624cd00c3..035804e61 100644
--- a/src/capture_odom_2D.h
+++ b/src/capture_odom_2D.h
@@ -8,27 +8,28 @@
//Wolf includes
#include "capture_relative.h"
#include "feature_odom_2D.h"
+#include "sensor_odom_2D.h"
//class CaptureGPSFix
class CaptureOdom2D : public CaptureRelative
{
public:
- CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr);
+ CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr);
- CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data);
+ CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data);
- CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
+ CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
- virtual ~CaptureOdom2D();
-
- virtual void processCapture();
+ virtual ~CaptureOdom2D();
+
+ virtual void processCapture();
- virtual Eigen::VectorXs computePrior() const;
+ virtual Eigen::VectorXs computePrior() const;
- virtual void addConstraints();
+ virtual void addConstraints();
- virtual void integrateCapture(CaptureRelative* _new_capture);
+ virtual void integrateCapture(CaptureRelative* _new_capture);
- //virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
+ //virtual void printSelf(unsigned int _ntabs = 0, std::ostream & _ost = std::cout) const;
};
#endif
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 4d34b7c5f..0cc8ea757 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -91,6 +91,9 @@ void CeresManager::update(WolfProblem* _problem_ptr)
_problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
addConstraint(*ctr_it);
+
+ // set the wolf problem reallocation flag to false
+ _problem_ptr->reallocationDone();
}
else
{
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index 6d1f1c614..050b4d8ac 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -11,7 +11,7 @@ ConstraintBase::ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp) :
ConstraintBase::~ConstraintBase()
{
- std::cout << "deleting ConstraintBase " << nodeId() << std::endl;
+ //std::cout << "deleting ConstraintBase " << nodeId() << std::endl;
}
ConstraintType ConstraintBase::getConstraintType() const
diff --git a/src/constraint_corner_2D_theta.h b/src/constraint_corner_2D_theta.h
index 06a3a370c..94767310b 100644
--- a/src/constraint_corner_2D_theta.h
+++ b/src/constraint_corner_2D_theta.h
@@ -30,7 +30,7 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
virtual ~ConstraintCorner2DTheta()
{
- std::cout << "deleting ConstraintCorner2DTheta " << nodeId() << std::endl;
+ //std::cout << "deleting ConstraintCorner2DTheta " << nodeId() << std::endl;
if (lmk_ptr_->getHits() == 1)
getTop()->getMapPtr()->removeLandmark(lmk_ptr_);
else
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 874d524f2..5530fb4bd 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -13,33 +13,10 @@
#include <eigen3/Eigen/Geometry>
//Ceres includes
-#include "ceres/ceres.h"
#include "glog/logging.h"
//Wolf includes
-#include "wolf.h"
-#include "sensor_base.h"
-#include "sensor_odom_2D.h"
-#include "sensor_gps_fix.h"
-#include "feature_base.h"
-#include "frame_base.h"
-#include "state_point.h"
-#include "state_complex_angle.h"
-#include "capture_base.h"
-#include "capture_relative.h"
-#include "capture_odom_2D.h"
-#include "capture_gps_fix.h"
-#include "capture_laser_2D.h"
-#include "state_base.h"
-#include "constraint_sparse.h"
-#include "constraint_gps_2D.h"
-#include "constraint_odom_2D_theta.h"
-#include "constraint_odom_2D_complex_angle.h"
-#include "trajectory_base.h"
-#include "map_base.h"
-#include "wolf_problem.h"
-
-// ceres wrapper include
+#include "wolf_manager.h"
#include "ceres_wrapper/ceres_manager.h"
//C includes for sleep, time and main args
@@ -57,187 +34,56 @@
//function travel around
void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_)
{
- if (ii<=120)
- {
- displacement_ = 0.1;
- rotation_ = 0;
- }
- else if ( (ii>120) && (ii<=170) )
- {
- displacement_ = 0.2;
- rotation_ = 1.8*M_PI/180;
- }
- else if ( (ii>170) && (ii<=220) )
- {
- displacement_ = 0;
- rotation_ = -1.8*M_PI/180;
- }
- else if ( (ii>220) && (ii<=310) )
- {
- displacement_ = 0.1;
- rotation_ = 0;
- }
- else if ( (ii>310) && (ii<=487) )
- {
- displacement_ = 0.1;
- rotation_ = -1.*M_PI/180;
- }
- else if ( (ii>487) && (ii<=600) )
- {
- displacement_ = 0.2;
- rotation_ = 0;
- }
- else if ( (ii>600) && (ii<=700) )
- {
- displacement_ = 0.1;
- rotation_ = -1.*M_PI/180;
- }
- else if ( (ii>700) && (ii<=780) )
- {
- displacement_ = 0;
- rotation_ = -1.*M_PI/180;
- }
- else
- {
- displacement_ = 0.3;
- rotation_ = 0.0*M_PI/180;
- }
-
- pose.moveForward(displacement_);
- pose.rt.setEuler( pose.rt.head()+rotation_, pose.rt.pitch(), pose.rt.roll() );
+ if (ii<=120)
+ {
+ displacement_ = 0.1;
+ rotation_ = 0;
+ }
+ else if ( (ii>120) && (ii<=170) )
+ {
+ displacement_ = 0.2;
+ rotation_ = 1.8*M_PI/180;
+ }
+ else if ( (ii>170) && (ii<=220) )
+ {
+ displacement_ = 0;
+ rotation_ = -1.8*M_PI/180;
+ }
+ else if ( (ii>220) && (ii<=310) )
+ {
+ displacement_ = 0.1;
+ rotation_ = 0;
+ }
+ else if ( (ii>310) && (ii<=487) )
+ {
+ displacement_ = 0.1;
+ rotation_ = -1.*M_PI/180;
+ }
+ else if ( (ii>487) && (ii<=600) )
+ {
+ displacement_ = 0.2;
+ rotation_ = 0;
+ }
+ else if ( (ii>600) && (ii<=700) )
+ {
+ displacement_ = 0.1;
+ rotation_ = -1.*M_PI/180;
+ }
+ else if ( (ii>700) && (ii<=780) )
+ {
+ displacement_ = 0;
+ rotation_ = -1.*M_PI/180;
+ }
+ else
+ {
+ displacement_ = 0.3;
+ rotation_ = 0.0*M_PI/180;
+ }
+
+ pose.moveForward(displacement_);
+ pose.rt.setEuler( pose.rt.head()+rotation_, pose.rt.pitch(), pose.rt.roll() );
}
-class WolfManager
-{
- protected:
- bool use_complex_angles_;
- WolfProblem* problem_;
- std::queue<CaptureBase*> new_captures_;
- SensorBase* sensor_prior_;
- unsigned int window_size_;
- FrameBaseIter first_window_frame_;
- CaptureRelative* last_capture_relative_;
-
- public:
- WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length, const Eigen::VectorXs& _init_frame, const unsigned int& _w_size=10) :
- use_complex_angles_(_complex_angle),
- problem_(new WolfProblem(_state_length)),
- sensor_prior_(_sensor_prior),
- window_size_(_w_size)
- {
- createFrame(_init_frame, TimeStamp());
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix();
- }
-
- virtual ~WolfManager()
- {
- delete problem_;
- }
-
- void createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
- {
- // Create frame and add it to the trajectory
- StateBase* new_position = new StatePoint2D(problem_->getNewStatePtr());
- problem_->addState(new_position, _frame_state.head(2));
-
- StateBase* new_orientation;
- if (use_complex_angles_)
- new_orientation = new StateComplexAngle(problem_->getNewStatePtr());
- else
- new_orientation = new StateTheta(problem_->getNewStatePtr());
-
- problem_->addState(new_orientation, _frame_state.tail(new_orientation->getStateSize()));
-
- problem_->getTrajectoryPtr()->addFrame(new FrameBase(_time_stamp, new_position, new_orientation));
-
- // add a zero odometry capture (in order to integrate)
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new CaptureOdom2D(_time_stamp,
- sensor_prior_,
- Eigen::Vector2s::Zero(),
- Eigen::Matrix2s::Zero()));
- last_capture_relative_ = (CaptureRelative*)(problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->front());
- }
-
- void addCapture(CaptureBase* _capture)
- {
- new_captures_.push(_capture);
- //std::cout << "added new capture: " << _capture->nodeId() << std::endl;
- }
-
- void update()
- {
- while (!new_captures_.empty())
- {
- // EXTRACT NEW CAPTURE
- CaptureBase* new_capture = new_captures_.front();
- new_captures_.pop();
-
- // ODOMETRY SENSOR
- if (new_capture->getSensorPtr() == sensor_prior_)
- {
- // INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR
- //std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl;
- last_capture_relative_->integrateCapture((CaptureRelative*)(new_capture));
-
- // NEW KEY FRAME (if enough time from last frame)
- //std::cout << "new TimeStamp - last Frame TimeStamp = " << new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() << std::endl;
- if (new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() > 0.1)
- {
- //std::cout << "store prev frame" << std::endl;
- auto previous_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back();
-
- // NEW FRAME
- //std::cout << "new frame" << std::endl;
- createFrame(last_capture_relative_->computePrior(), new_capture->getTimeStamp());
-
- // COMPUTE PREVIOUS FRAME CAPTURES
- //std::cout << "compute prev frame" << std::endl;
- for (auto capture_it = previous_frame_ptr->getCaptureListPtr()->begin(); capture_it != previous_frame_ptr->getCaptureListPtr()->end(); capture_it++)
- (*capture_it)->processCapture();
-
- // WINDOW of FRAMES (remove or fix old frames)
- //std::cout << "frame window" << std::endl;
- if (problem_->getTrajectoryPtr()->getFrameListPtr()->size() > window_size_)
- {
- //std::cout << "frame fixing" << std::endl;
- //problem_->getTrajectoryPtr()->removeFrame(problem_->getTrajectoryPtr()->getFrameListPtr()->begin());
- (*first_window_frame_)->fix();
- first_window_frame_++;
- }
- else
- first_window_frame_ = problem_->getTrajectoryPtr()->getFrameListPtr()->begin();
- }
- }
- else
- {
- // ADD CAPTURE TO THE LAST FRAME (or substitute the same sensor previous capture)
- //std::cout << "adding not odometry capture..." << std::endl;
- for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++)
- {
- if ((*capture_it)->getSensorPtr() == new_capture->getSensorPtr())
- {
- //std::cout << "removing previous capture" << std::endl;
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->removeCapture(capture_it);
- //std::cout << "removed!" << std::endl;
- break;
- }
- }
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new_capture);
- }
- }
- }
-
- Eigen::VectorXs getVehiclePose()
- {
- return last_capture_relative_->computePrior();
- }
-
- WolfProblem* getProblemPtr()
- {
- return problem_;
- }
-};
-
int main(int argc, char** argv)
{
std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
@@ -259,14 +105,14 @@ int main(int argc, char** argv)
// INITIALIZATION ============================================================================================
//init random generators
- WolfScalar odom_std= 0.01;
+ WolfScalar odom_std_factor= 0.1;
WolfScalar gps_std= 1;
std::default_random_engine generator(1);
- std::normal_distribution<WolfScalar> distribution_odom(0.0, odom_std); //odometry noise
+ std::normal_distribution<WolfScalar> distribution_odom(0.0, odom_std_factor); //odometry noise
std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise
//init google log
- google::InitGoogleLogging(argv[0]);
+ //google::InitGoogleLogging(argv[0]);
// Ceres initialization
ceres::Solver::Options ceres_options;
@@ -316,7 +162,7 @@ int main(int argc, char** argv)
clock_t t1, t2;
// Wolf manager initialization
- SensorOdom2D odom_sensor(Eigen::Vector6s::Zero(), odom_std, odom_std);
+ SensorOdom2D odom_sensor(Eigen::Vector6s::Zero(), odom_std_factor, odom_std_factor);
SensorGPSFix gps_sensor(Eigen::Vector6s::Zero(), gps_std);
Eigen::Vector6s laser_1_pose, laser_2_pose;
laser_1_pose << 1.2,0,0,0,0,0; //laser 1
@@ -329,7 +175,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, window_size);
+ WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e9, pose_odom, 0.1, window_size);
std::cout << "START TRAJECTORY..." << std::endl;
// START TRAJECTORY ============================================================================================
@@ -352,8 +198,8 @@ int main(int argc, char** argv)
ground_truth.segment(step*3,3) << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
// compute odometry
- odom_reading(0) += distribution_odom(generator);
- odom_reading(1) += distribution_odom(generator);
+ odom_reading(0) += distribution_odom(generator)*(odom_reading(0)+1e-3);
+ odom_reading(1) += distribution_odom(generator)*(odom_reading(1)+1e-3);
// odometry integration
pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2));
@@ -390,7 +236,7 @@ int main(int argc, char** argv)
//std::cout << "ADD CAPTURES..." << std::endl;
t1=clock();
// adding new sensor captures
- wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading, odom_std * Eigen::MatrixXs::Identity(2,2)));
+ wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading));//, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
// wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1_reading));
wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2_reading));
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index fadc2ffac..8f91fbf4c 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -17,7 +17,7 @@ FeatureBase::FeatureBase(const Eigen::VectorXs& _measurement, const Eigen::Matri
FeatureBase::~FeatureBase()
{
- std::cout << "deleting FeatureBase " << nodeId() << std::endl;
+ //std::cout << "deleting FeatureBase " << nodeId() << std::endl;
}
void FeatureBase::addConstraint(ConstraintBase* _co_ptr)
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index ea848b395..cb75f37b5 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -31,7 +31,7 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_
FrameBase::~FrameBase()
{
- std::cout << "deleting FrameBase " << nodeId() << std::endl;
+ //std::cout << "deleting FrameBase " << nodeId() << std::endl;
// Remove Frame State Units
if (p_ptr_ != nullptr)
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index dd9b2076a..655a8ad0b 100644
--- a/src/landmark_base.cpp
+++ b/src/landmark_base.cpp
@@ -26,7 +26,7 @@ LandmarkBase::LandmarkBase(const LandmarkType & _tp, StateBase* _p_ptr, StateBas
LandmarkBase::~LandmarkBase()
{
- std::cout << "deleting LandmarkBase " << nodeId() << std::endl;
+ //std::cout << "deleting LandmarkBase " << nodeId() << std::endl;
// Remove Landmark State Units
if (p_ptr_ != nullptr)
diff --git a/src/landmark_base.h b/src/landmark_base.h
index ea4cfe7da..c23d4fe98 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -21,61 +21,61 @@ class NodeTerminus;
//class LandmarkBase
class LandmarkBase : public NodeLinked<MapBase,NodeTerminus>
{
- protected:
- LandmarkType type_; //type of landmark. (types defined at wolf.h)
- LandmarkStatus status_; //status of the landmark. (types defined at wolf.h)
- unsigned int hit_count_; //counts how many features has been associated to this landmark
- TimeStamp stamp_; // stamp of the creation of the landmark (and stamp of destruction when status is LANDMARK_OLD)
- //StateBaseList st_list_; //List of pointers to the state corresponding to the landmark estimation
- StateBase* p_ptr_; // Position state unit pointer
- StateBase* o_ptr_; // Orientation state unit pointer
- StateBase* v_ptr_; // Velocity state unit pointer
- StateBase* w_ptr_; // Angular velocity state unit pointer
- //TODO: accelerations?
- Eigen::VectorXs descriptor_;//TODO: agree?
-
- public:
- /** \brief Constructor with type, time stamp and the position state pointer
- *
- * Constructor with type, and state pointer
- * \param _tp indicates landmark type.(types defined at wolf.h)
- * \param _p_ptr StateBase pointer to the position
- * \param _o_ptr StateBase pointer to the orientation (default: nullptr)
- * \param _v_ptr StateBase pointer to the velocity (default: nullptr)
- * \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
- *
- **/
- LandmarkBase(const LandmarkType & _tp , StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
-
- /** \brief Constructor with type, time stamp and state list
- *
- * Constructor with type and state pointer list
- * \param _tp indicates frame type. Generally either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
- * \param _stp_list StateBase list of the landmark estimation
- *
- **/
- //LandmarkBase(const LandmarkType & _tp, const StateBaseList& _stp_list);
-
- /** \brief Destructor
- *
- * Destructor
- *
- **/
- virtual ~LandmarkBase();
-
- void setStatus(LandmarkStatus _st);
-
- void hit();
-
- void unhit();
-
- void fix();
-
- void unfix();
-
- unsigned int getHits() const;
-
- StateBase* getPPtr() const;
+ protected:
+ LandmarkType type_; //type of landmark. (types defined at wolf.h)
+ LandmarkStatus status_; //status of the landmark. (types defined at wolf.h)
+ unsigned int hit_count_; //counts how many features has been associated to this landmark
+ TimeStamp stamp_; // stamp of the creation of the landmark (and stamp of destruction when status is LANDMARK_OLD)
+ //StateBaseList st_list_; //List of pointers to the state corresponding to the landmark estimation
+ StateBase* p_ptr_; // Position state unit pointer
+ StateBase* o_ptr_; // Orientation state unit pointer
+ StateBase* v_ptr_; // Velocity state unit pointer
+ StateBase* w_ptr_; // Angular velocity state unit pointer
+ //TODO: accelerations?
+ Eigen::VectorXs descriptor_;//TODO: agree?
+
+ public:
+ /** \brief Constructor with type, time stamp and the position state pointer
+ *
+ * Constructor with type, and state pointer
+ * \param _tp indicates landmark type.(types defined at wolf.h)
+ * \param _p_ptr StateBase pointer to the position
+ * \param _o_ptr StateBase pointer to the orientation (default: nullptr)
+ * \param _v_ptr StateBase pointer to the velocity (default: nullptr)
+ * \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
+ *
+ **/
+ LandmarkBase(const LandmarkType & _tp , StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
+
+ /** \brief Constructor with type, time stamp and state list
+ *
+ * Constructor with type and state pointer list
+ * \param _tp indicates frame type. Generally either REGULAR_FRAME or KEY_FRAME. (types defined at wolf.h)
+ * \param _stp_list StateBase list of the landmark estimation
+ *
+ **/
+ //LandmarkBase(const LandmarkType & _tp, const StateBaseList& _stp_list);
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ virtual ~LandmarkBase();
+
+ void setStatus(LandmarkStatus _st);
+
+ void hit();
+
+ void unhit();
+
+ void fix();
+
+ void unfix();
+
+ unsigned int getHits() const;
+
+ StateBase* getPPtr() const;
StateBase* getOPtr() const;
@@ -87,8 +87,8 @@ class LandmarkBase : public NodeLinked<MapBase,NodeTerminus>
const Eigen::VectorXs& getDescriptor() const;
- //const StateBase* getStatePtr() const;
+ //const StateBase* getStatePtr() const;
- //const StateBaseList* getStateListPtr() const;
+ //const StateBaseList* getStateListPtr() const;
};
#endif
diff --git a/src/map_base.cpp b/src/map_base.cpp
index 816211e0b..f892f9da2 100644
--- a/src/map_base.cpp
+++ b/src/map_base.cpp
@@ -8,7 +8,7 @@ MapBase::MapBase() :
MapBase::~MapBase()
{
- std::cout << "deleting MapBase " << nodeId() << std::endl;
+ //std::cout << "deleting MapBase " << nodeId() << std::endl;
}
void MapBase::addLandmark(LandmarkBase* _landmark_ptr)
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 7c2bafe8e..fa72cfed0 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -2,47 +2,56 @@
SensorBase::SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, const Eigen::VectorXs & _params) :
NodeBase("SENSOR"),
- type_(_tp),
+ type_(_tp),
sensor_position_vehicle_(_pose.head(3)),
params_(_params.size())
{
- params_ = _params;
+ params_ = _params;
sensor_rotation_vehicle_ = Eigen::AngleAxisd(_pose(3), Eigen::Vector3d::UnitX()) *
- Eigen::AngleAxisd(_pose(4), Eigen::Vector3d::UnitY()) *
- Eigen::AngleAxisd(_pose(5), Eigen::Vector3d::UnitZ());
+ Eigen::AngleAxisd(_pose(4), Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxisd(_pose(5), Eigen::Vector3d::UnitZ());
}
SensorBase::SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, unsigned int _params_size) :
NodeBase("SENSOR"),
- type_(_tp),
+ type_(_tp),
sensor_position_vehicle_(_pose.head(3)),
- params_(_params_size)
+ params_(_params_size)
{
sensor_rotation_vehicle_ = Eigen::AngleAxisd(_pose(3), Eigen::Vector3d::UnitX()) *
- Eigen::AngleAxisd(_pose(4), Eigen::Vector3d::UnitY()) *
- Eigen::AngleAxisd(_pose(5), Eigen::Vector3d::UnitZ());
+ Eigen::AngleAxisd(_pose(4), Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxisd(_pose(5), Eigen::Vector3d::UnitZ());
}
SensorBase::~SensorBase()
{
- std::cout << "deleting SensorBase " << nodeId() << std::endl;
+ //std::cout << "deleting SensorBase " << nodeId() << std::endl;
}
const SensorType SensorBase::getSensorType() const
{
- return type_;
+ return type_;
}
const Eigen::Vector3s * SensorBase::getSensorPosition() const
{
//std::cout << "getSensorPosition: " << sensor_position_vehicle_.transpose() << std::endl;
- return & sensor_position_vehicle_;
+ return & sensor_position_vehicle_;
}
const Eigen::Matrix3s * SensorBase::getSensorRotation() const
{
//std::cout << "getSensorRotation: " << sensor_rotation_vehicle_ << std::endl;
- return & sensor_rotation_vehicle_;
+ return & sensor_rotation_vehicle_;
}
+void SensorBase::setSensorPose(const Eigen::Vector6s & _pose)
+{
+ sensor_position_vehicle_ = _pose.head(3);
+ sensor_rotation_vehicle_ = Eigen::AngleAxisd(_pose(3), Eigen::Vector3d::UnitX()) *
+ Eigen::AngleAxisd(_pose(4), Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxisd(_pose(5), Eigen::Vector3d::UnitZ());
+}
+
+
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 0bab63e41..e7b1f55c7 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -12,29 +12,30 @@
class SensorBase : public NodeBase
{
- protected:
- SensorType type_; //indicates sensor type. Enum defined at wolf.h
- Eigen::Vector3s sensor_position_vehicle_;//sensor position in the vehicle frame
- Eigen::Matrix3s sensor_rotation_vehicle_;//sensor rotation in the vehicle frame
- // TODO:
+ protected:
+ SensorType type_; //indicates sensor type. Enum defined at wolf.h
+ Eigen::Vector3s sensor_position_vehicle_;//sensor position in the vehicle frame
+ Eigen::Matrix3s sensor_rotation_vehicle_;//sensor rotation in the vehicle frame
+ // TODO:
// StateBase* p_ptr_; // sensor position state unit pointer
// StateOrientation* o_ptr_; // sensor orientation state unit pointer
- Eigen::VectorXs params_; //sensor intrinsic params: offsets, scale factors, sizes, ...
- //bool generate_prior_; //flag indicating if this sensor generates the prior or not
+ Eigen::VectorXs params_; //sensor intrinsic params: offsets, scale factors, sizes, ...
+ //bool generate_prior_; //flag indicating if this sensor generates the prior or not
+
+ public:
+ SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, const Eigen::VectorXs & _params);
- public:
- SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, const Eigen::VectorXs & _params);
-
- SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, unsigned int _params_size);
-
- ~SensorBase();
-
- const SensorType getSensorType() const;
-
- const Eigen::Vector3s * getSensorPosition() const;
-
- const Eigen::Matrix3s * getSensorRotation() const;
-
+ SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, unsigned int _params_size);
+
+ ~SensorBase();
+
+ const SensorType getSensorType() const;
+
+ const Eigen::Vector3s * getSensorPosition() const;
+
+ const Eigen::Matrix3s * getSensorRotation() const;
+
+ void setSensorPose(const Eigen::Vector6s & _pose);
};
#endif
diff --git a/src/state_base.cpp b/src/state_base.cpp
index 52ecb845c..1b2f5c6cd 100644
--- a/src/state_base.cpp
+++ b/src/state_base.cpp
@@ -20,7 +20,7 @@ StateBase::StateBase(WolfScalar* _st_ptr) :
StateBase::~StateBase()
{
- std::cout << "deleting StateBase " << nodeId() << std::endl;
+ //std::cout << "deleting StateBase " << nodeId() << std::endl;
}
WolfScalar* StateBase::getPtr()
diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp
index 409847e16..f8f1b62b7 100644
--- a/src/time_stamp.cpp
+++ b/src/time_stamp.cpp
@@ -13,6 +13,12 @@ TimeStamp::TimeStamp(const WolfScalar _ts) :
//
}
+TimeStamp::TimeStamp(const unsigned long int _sec, const unsigned long int _nsec) :
+ time_stamp_((WolfScalar)(_sec + _nsec / 1e9))
+{
+ //
+}
+
TimeStamp::~TimeStamp()
{
//nothing to do
diff --git a/src/time_stamp.h b/src/time_stamp.h
index bd2d2da30..ff86f03e9 100644
--- a/src/time_stamp.h
+++ b/src/time_stamp.h
@@ -40,6 +40,13 @@ class TimeStamp
*/
TimeStamp(const WolfScalar _ts);
+ /** \brief Constructor from sec and nsec
+ *
+ * Constructor from sec and nsec
+ *
+ */
+ TimeStamp(const unsigned long int _sec, const unsigned long int _nsec);
+
/** \brief Destructor
*
* Destructor
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 2612317cb..5cbf0036e 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -8,7 +8,7 @@ TrajectoryBase::TrajectoryBase() :
TrajectoryBase::~TrajectoryBase()
{
- std::cout << "deleting TrajectoryBase " << nodeId() << std::endl;
+ //std::cout << "deleting TrajectoryBase " << nodeId() << std::endl;
}
void TrajectoryBase::addFrame(FrameBase* _frame_ptr)
diff --git a/src/wolf_manager.h b/src/wolf_manager.h
new file mode 100644
index 000000000..a1ec47431
--- /dev/null
+++ b/src/wolf_manager.h
@@ -0,0 +1,173 @@
+//std includes
+#include <cstdlib>
+#include <iostream>
+#include <fstream>
+#include <memory>
+#include <random>
+#include <typeinfo>
+#include <ctime>
+#include <queue>
+
+// Eigen includes
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+
+//Ceres includes
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+//Wolf includes
+#include "wolf.h"
+#include "sensor_base.h"
+#include "sensor_odom_2D.h"
+#include "sensor_gps_fix.h"
+#include "feature_base.h"
+#include "frame_base.h"
+#include "state_point.h"
+#include "state_complex_angle.h"
+#include "capture_base.h"
+#include "capture_relative.h"
+#include "capture_odom_2D.h"
+#include "capture_gps_fix.h"
+#include "capture_laser_2D.h"
+#include "state_base.h"
+#include "constraint_sparse.h"
+#include "constraint_gps_2D.h"
+#include "constraint_odom_2D_theta.h"
+#include "constraint_odom_2D_complex_angle.h"
+#include "trajectory_base.h"
+#include "map_base.h"
+#include "wolf_problem.h"
+
+class WolfManager
+{
+ protected:
+ bool use_complex_angles_;
+ WolfProblem* problem_;
+ std::queue<CaptureBase*> new_captures_;
+ SensorBase* sensor_prior_;
+ unsigned int window_size_;
+ FrameBaseIter first_window_frame_;
+ CaptureRelative* last_capture_relative_;
+ WolfScalar new_frame_elapsed_time_;
+
+ public:
+ WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length, const Eigen::VectorXs& _init_frame, const WolfScalar& _new_frame_elapsed_time=0.1, const unsigned int& _w_size=10) :
+ use_complex_angles_(_complex_angle),
+ problem_(new WolfProblem(_state_length)),
+ sensor_prior_(_sensor_prior),
+ window_size_(_w_size),
+ new_frame_elapsed_time_(_new_frame_elapsed_time)
+ {
+ createFrame(_init_frame, TimeStamp());
+ problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix();
+ }
+
+ virtual ~WolfManager()
+ {
+ delete problem_;
+ }
+
+ void createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
+ {
+ // Create frame and add it to the trajectory
+ StateBase* new_position = new StatePoint2D(problem_->getNewStatePtr());
+ problem_->addState(new_position, _frame_state.head(2));
+
+ StateBase* new_orientation;
+ if (use_complex_angles_)
+ new_orientation = new StateComplexAngle(problem_->getNewStatePtr());
+ else
+ new_orientation = new StateTheta(problem_->getNewStatePtr());
+
+ problem_->addState(new_orientation, _frame_state.tail(new_orientation->getStateSize()));
+
+ problem_->getTrajectoryPtr()->addFrame(new FrameBase(_time_stamp, new_position, new_orientation));
+
+ // add a zero odometry capture (in order to integrate)
+ problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new CaptureOdom2D(_time_stamp,
+ sensor_prior_,
+ Eigen::Vector2s::Zero(),
+ Eigen::Matrix2s::Zero()));
+ last_capture_relative_ = (CaptureRelative*)(problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->front());
+ }
+
+ void addCapture(CaptureBase* _capture)
+ {
+ new_captures_.push(_capture);
+ //std::cout << "added new capture: " << _capture->nodeId() << std::endl;
+ }
+
+ void update()
+ {
+ while (!new_captures_.empty())
+ {
+ // EXTRACT NEW CAPTURE
+ CaptureBase* new_capture = new_captures_.front();
+ new_captures_.pop();
+
+ // ODOMETRY SENSOR
+ if (new_capture->getSensorPtr() == sensor_prior_)
+ {
+ // INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR
+ //std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl;
+ last_capture_relative_->integrateCapture((CaptureRelative*)(new_capture));
+
+ // NEW KEY FRAME (if enough time from last frame)
+ //std::cout << "new TimeStamp - last Frame TimeStamp = " << new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() << std::endl;
+ if (new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() > new_frame_elapsed_time_)
+ {
+ //std::cout << "store prev frame" << std::endl;
+ auto previous_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back();
+
+ // NEW FRAME
+ //std::cout << "new frame" << std::endl;
+ createFrame(last_capture_relative_->computePrior(), new_capture->getTimeStamp());
+
+ // COMPUTE PREVIOUS FRAME CAPTURES
+ //std::cout << "compute prev frame" << std::endl;
+ for (auto capture_it = previous_frame_ptr->getCaptureListPtr()->begin(); capture_it != previous_frame_ptr->getCaptureListPtr()->end(); capture_it++)
+ (*capture_it)->processCapture();
+
+ // WINDOW of FRAMES (remove or fix old frames)
+ //std::cout << "frame window" << std::endl;
+ if (problem_->getTrajectoryPtr()->getFrameListPtr()->size() > window_size_)
+ {
+ //std::cout << "frame fixing" << std::endl;
+ //problem_->getTrajectoryPtr()->removeFrame(problem_->getTrajectoryPtr()->getFrameListPtr()->begin());
+ (*first_window_frame_)->fix();
+ first_window_frame_++;
+ }
+ else
+ first_window_frame_ = problem_->getTrajectoryPtr()->getFrameListPtr()->begin();
+ }
+ }
+ else
+ {
+ // ADD CAPTURE TO THE LAST FRAME (or substitute the same sensor previous capture)
+ //std::cout << "adding not odometry capture..." << std::endl;
+ for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++)
+ {
+ if ((*capture_it)->getSensorPtr() == new_capture->getSensorPtr())
+ {
+ //std::cout << "removing previous capture" << std::endl;
+ problem_->getTrajectoryPtr()->getFrameListPtr()->back()->removeCapture(capture_it);
+ //std::cout << "removed!" << std::endl;
+ break;
+ }
+ }
+ problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new_capture);
+ }
+ }
+ }
+
+ Eigen::VectorXs getVehiclePose()
+ {
+ return last_capture_relative_->computePrior();
+ }
+
+ WolfProblem* getProblemPtr()
+ {
+ return problem_;
+ }
+};
diff --git a/src/wolf_problem.cpp b/src/wolf_problem.cpp
index 81f863274..d60d725e7 100644
--- a/src/wolf_problem.cpp
+++ b/src/wolf_problem.cpp
@@ -67,7 +67,7 @@ bool WolfProblem::addState(StateBase* _new_state_ptr, const Eigen::VectorXs& _ne
void WolfProblem::removeState(StateBase* _state_ptr)
{
- // TODO: Reordering?
+ // TODO: Reordering? Mandatory for filtering...
state_list_.remove(_state_ptr);
removed_state_ptr_list_.push_back(_state_ptr->getPtr());
delete _state_ptr;
--
GitLab