diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 91026341ed8d4a78b8dd6dd9ade25e4e2e902d46..432bb2234aa2517b941258babcb5470a6f188f03 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -50,15 +50,15 @@ void CaptureLaser2D::processCapture()
//extract corners from range data
extractCorners(corners);
- std::cout << corners.size() << " corners extracted" << std::endl;
+ //std::cout << corners.size() << " corners extracted" << std::endl;
//generate a feature for each corner
createFeatures(corners);
- std::cout << getFeatureListPtr()->size() << " Features created" << std::endl;
+ //std::cout << getFeatureListPtr()->size() << " Features created" << std::endl;
//Establish constraints for each feature
establishConstraints();
- std::cout << "Constraints created" << std::endl;
+ //std::cout << "Constraints created" << std::endl;
}
unsigned int CaptureLaser2D::extractCorners(std::list<laserscanutils::Corner> & _corner_list) const
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index 5c7b366cf74c8e300bdaff56688abf2e303fbaa3..c784db7bb02ae5b323d52e8862ec269268ddd7d6 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -6,16 +6,17 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr) :
//
}
-CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data) :
+CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::Vector3s& _data) :
CaptureRelative(_ts, _sensor_ptr, _data)
{
- data_covariance_ = Eigen::Matrix2s::Zero();
+ data_covariance_ = Eigen::Matrix3s::Zero();
data_covariance_(0,0) = (_data(0) == 0 ? 1e-6 : _data(0))*((SensorOdom2D*)_sensor_ptr)->getDisplacementNoiseFactor();
- data_covariance_(1,1) = (_data(1) == 0 ? 1e-6 : _data(1))*((SensorOdom2D*)_sensor_ptr)->getRotationNoiseFactor();
+ data_covariance_(1,1) = (_data(1) == 0 ? 1e-6 : _data(1))*((SensorOdom2D*)_sensor_ptr)->getDisplacementNoiseFactor();
+ data_covariance_(2,2) = (_data(2) == 0 ? 1e-6 : _data(2))*((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) :
+CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::Vector3s& _data, const Eigen::Matrix3s& _data_covariance) :
CaptureRelative(_ts, _sensor_ptr, _data, _data_covariance)
{
//
@@ -28,13 +29,13 @@ CaptureOdom2D::~CaptureOdom2D()
inline void CaptureOdom2D::processCapture()
{
- //std::cout << "CaptureOdom2D::addFeature..." << std::endl;
+ //std::cout << "CaptureOdom2D::addFeature..." << std::endl;
// ADD FEATURE
- addFeature(new FeatureOdom2D(data_,data_covariance_));
+ addFeature(new FeatureOdom2D(data_,data_covariance_));
- //std::cout << "CaptureOdom2D::addConstraints..." << std::endl;
- // ADD CONSTRAINT
- addConstraints();
+ //std::cout << "CaptureOdom2D::addConstraints..." << std::endl;
+ // ADD CONSTRAINT
+ addConstraints();
}
Eigen::VectorXs CaptureOdom2D::computePrior() const
@@ -43,29 +44,43 @@ Eigen::VectorXs CaptureOdom2D::computePrior() const
if (getFramePtr()->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
{
- Eigen::VectorXs prior(4);
- Eigen::Map<Eigen::VectorXs> initial_pose(getFramePtr()->getPPtr()->getPtr(), 4);
-
- double prior_2 = initial_pose(2) * cos(data_(1)) - initial_pose(3) * sin(data_(1));
- double prior_3 = initial_pose(2) * sin(data_(1)) + initial_pose(3) * cos(data_(1));
- prior(0) = initial_pose(0) + data_(0) * prior_2;
- prior(1) = initial_pose(1) + data_(0) * prior_3;
- prior(2) = prior_2;
- prior(3) = prior_3;
+ Eigen::Vector4s prior;
+ Eigen::Map<Eigen::Vector4s> initial_pose(getFramePtr()->getPPtr()->getPtr());
+ ///std::cout << "initial_pose: " << initial_pose.transpose() << std::endl;
+// WolfScalar prior_2 = initial_pose(2) * cos(data_(1)) - initial_pose(3) * sin(data_(1));
+// WolfScalar prior_3 = initial_pose(2) * sin(data_(1)) + initial_pose(3) * cos(data_(1));
+// prior(0) = initial_pose(0) + data_(0) * prior_2;
+// prior(1) = initial_pose(1) + data_(0) * prior_3;
+// prior(2) = prior_2;
+// prior(3) = prior_3;
+ prior(0) = initial_pose(0) + data_(0) * initial_pose(2) - data_(1) * initial_pose(3);
+ prior(1) = initial_pose(1) + data_(0) * initial_pose(3) + data_(1) * initial_pose(2);
+ prior(2) = initial_pose(2) * cos(data_(2)) - initial_pose(3) * sin(data_(2));
+ prior(3) = initial_pose(2) * sin(data_(2)) + initial_pose(3) * cos(data_(2));
+ //std::cout << "data_: " << data_.transpose() << std::endl;
+ //std::cout << "prior: " << prior.transpose() << std::endl;
return prior;
}
else
{
- Eigen::VectorXs prior(3);
- Eigen::Map<Eigen::VectorXs> initial_pose(getFramePtr()->getPPtr()->getPtr(), 3);
-
- prior(0) = initial_pose(0) + data_(0) * cos(initial_pose(2) + (data_(1)));
- prior(1) = initial_pose(1) + data_(0) * sin(initial_pose(2) + (data_(1)));
- prior(2) = initial_pose(2) + data_(1);
+ Eigen::Vector3s prior;
+ Eigen::Map<Eigen::Vector3s> initial_pose(getFramePtr()->getPPtr()->getPtr());
+ //std::cout << "initial_pose: " << initial_pose.transpose() << std::endl;
+
+// prior(0) = initial_pose(0) + data_(0) * cos(initial_pose(2) + (data_(1)));
+// prior(1) = initial_pose(1) + data_(0) * sin(initial_pose(2) + (data_(1)));
+// prior(2) = initial_pose(2) + data_(1);
+ prior(0) = initial_pose(0) + data_(0) * cos(initial_pose(2)) - data_(1) * sin(initial_pose(2));
+ prior(1) = initial_pose(1) + data_(0) * sin(initial_pose(2)) + data_(1) * cos(initial_pose(2));
+ prior(2) = initial_pose(2) + data_(2);
+ //std::cout << "data_: " << data_.transpose() << std::endl;
+ //std::cout << "prior: " << prior.transpose() << std::endl;
return prior;
}
+
+
}
void CaptureOdom2D::addConstraints()
@@ -75,18 +90,18 @@ void CaptureOdom2D::addConstraints()
if (getFramePtr()->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
{
getFeatureListPtr()->front()->addConstraint(new ConstraintOdom2DComplexAngle(getFeatureListPtr()->front(),
- getFramePtr()->getPPtr(),
- getFramePtr()->getOPtr(),
- getFramePtr()->getNextFrame()->getPPtr(),
- getFramePtr()->getNextFrame()->getOPtr()));
+ getFramePtr()->getPPtr(),
+ getFramePtr()->getOPtr(),
+ getFramePtr()->getNextFrame()->getPPtr(),
+ getFramePtr()->getNextFrame()->getOPtr()));
}
else
{
getFeatureListPtr()->front()->addConstraint(new ConstraintOdom2DTheta(getFeatureListPtr()->front(),
- getFramePtr()->getPPtr(),
- getFramePtr()->getOPtr(),
- getFramePtr()->getNextFrame()->getPPtr(),
- getFramePtr()->getNextFrame()->getOPtr()));
+ getFramePtr()->getPPtr(),
+ getFramePtr()->getOPtr(),
+ getFramePtr()->getNextFrame()->getPPtr(),
+ getFramePtr()->getNextFrame()->getOPtr()));
}
}
@@ -94,8 +109,18 @@ void CaptureOdom2D::integrateCapture(CaptureRelative* _new_capture)
{
//std::cout << "Trying to integrate CaptureOdom2D" << std::endl;
assert(dynamic_cast<CaptureOdom2D*>(_new_capture) && "Trying to integrate with a CaptureOdom2D a CaptureRelativePtr which is not CaptureOdom2D");
- data_(0) += _new_capture->getData()(0);
- data_(1) += _new_capture->getData()(1);
+
+// data_(0) += _new_capture->getData()(0);
+// data_(1) += _new_capture->getData()(1);
+
+ //std::cout << "Integrate odoms: " << std::endl << data_.transpose() << std::endl << _new_capture->getData().transpose() << std::endl;
+
+ 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);
+
+ //std::cout << "Integrated odoms: " << std::endl << data_.transpose() << std::endl;
+
data_covariance_ += _new_capture->getDataCovariance();
//std::cout << "integrated!" << std::endl;
}
diff --git a/src/capture_odom_2D.h b/src/capture_odom_2D.h
index 035804e614c9a7c4b774b866e47c705e5db3d86f..11ac37349be6e527d4e6e1bdf473359029fd8686 100644
--- a/src/capture_odom_2D.h
+++ b/src/capture_odom_2D.h
@@ -16,9 +16,9 @@ class CaptureOdom2D : public CaptureRelative
public:
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::Vector3s& _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::Vector3s& _data, const Eigen::Matrix3s& _data_covariance);
virtual ~CaptureOdom2D();
diff --git a/src/constraint_odom_2D_complex_angle.h b/src/constraint_odom_2D_complex_angle.h
index 320e86f0ac4f54db8f1b4ddf76a9f4e5baf6da9b..928df9f4947396c5e5eb13cb71d254f6470023c6 100644
--- a/src/constraint_odom_2D_complex_angle.h
+++ b/src/constraint_odom_2D_complex_angle.h
@@ -6,19 +6,19 @@
#include "wolf.h"
#include "constraint_sparse.h"
-class ConstraintOdom2DComplexAngle: public ConstraintSparse<2,2,2,2,2>
+class ConstraintOdom2DComplexAngle: public ConstraintSparse<3,2,2,2,2>
{
public:
- static const unsigned int N_BLOCKS = 2;
+ static const unsigned int N_BLOCKS = 4;
ConstraintOdom2DComplexAngle(FeatureBase* _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
- ConstraintSparse<2,2,2,2,2>(_ftr_ptr,CTR_ODOM_2D_COMPLEX_ANGLE, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
+ ConstraintSparse<3,2,2,2,2>(_ftr_ptr,CTR_ODOM_2D_COMPLEX_ANGLE, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
{
//
}
ConstraintOdom2DComplexAngle(FeatureBase* _ftr_ptr, StateBase* _state0Ptr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr) :
- ConstraintSparse<2,2,2,2,2>(_ftr_ptr,CTR_ODOM_2D_COMPLEX_ANGLE, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
+ ConstraintSparse<3,2,2,2,2>(_ftr_ptr,CTR_ODOM_2D_COMPLEX_ANGLE, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
{
//
}
@@ -32,12 +32,22 @@ class ConstraintOdom2DComplexAngle: public ConstraintSparse<2,2,2,2,2>
bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
{
// Expected measurement
- T expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
- T expected_rotation = atan2(_o2[1]*_o1[0] - _o2[0]*_o1[1], _o1[0]*_o2[0] + _o1[1]*_o2[1]);
+ // rotar per menys l'angle de primer _o1
+ T expected_longitudinal = _o1[0]*(_p2[0]-_p1[0])+_o1[1]*(_p2[1]-_p1[1]); // cos(-o1)(x2-x1) - sin(-o1)(y2-y1)
+ T expected_lateral = -_o1[1]*(_p2[0]-_p1[0])+_o1[0]*(_p2[1]-_p1[1]); // sin(-o1)(x2-x1) + cos(-o1)(y2-y1)
+ T expected_rotation = atan2(_o2[1]*_o1[0] - _o2[0]*_o1[1], _o1[0]*_o2[0] + _o1[1]*_o2[1]);
+
+ // Residuals
+ _residuals[0] = (expected_longitudinal - T(measurement_(0))) / T(measurement_covariance_(0,0));
+ _residuals[1] = (expected_lateral - T(measurement_(1))) / T(measurement_covariance_(1,1));
+ _residuals[2] = (expected_rotation - T(measurement_(2))) / T(measurement_covariance_(2,2));
+
+// T expected_longitudinal = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
+// T expected_rotation = atan2(_o2[1]*_o1[0] - _o2[0]*_o1[1], _o1[0]*_o2[0] + _o1[1]*_o2[1]);
// Residuals
- _residuals[0] = (expected_range - T(measurement_(0))*T(measurement_(0))) / T(measurement_covariance_(0,0));
- _residuals[1] = (expected_rotation - T(measurement_(1))) / T(measurement_covariance_(1,1));
+// _residuals[0] = (expected_longitudinal - T(measurement_(0))*T(measurement_(0))) / T(measurement_covariance_(0,0));
+// _residuals[1] = (expected_rotation - T(measurement_(1))) / T(measurement_covariance_(1,1));
return true;
}
diff --git a/src/constraint_odom_2D_theta.h b/src/constraint_odom_2D_theta.h
index f637169c3156d499f82f147c7f1f3259d21bbad8..f632e1b0e0cf7a6b0c08da076fdc7eca2fb43acd 100644
--- a/src/constraint_odom_2D_theta.h
+++ b/src/constraint_odom_2D_theta.h
@@ -6,19 +6,19 @@
#include "wolf.h"
#include "constraint_sparse.h"
-class ConstraintOdom2DTheta: public ConstraintSparse<2,2,1,2,1>
+class ConstraintOdom2DTheta: public ConstraintSparse<3,2,1,2,1>
{
public:
- static const unsigned int N_BLOCKS = 2;
+ static const unsigned int N_BLOCKS = 4;
ConstraintOdom2DTheta(FeatureBase*_ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
- ConstraintSparse<2,2,1,2,1>(_ftr_ptr,CTR_ODOM_2D_THETA, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
+ ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_ODOM_2D_THETA, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
{
//
}
ConstraintOdom2DTheta(FeatureBase* _ftr_ptr, StateBase* _state0Ptr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr) :
- ConstraintSparse<2,2,1,2,1>(_ftr_ptr,CTR_ODOM_2D_THETA, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
+ ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_ODOM_2D_THETA, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
{
//
}
@@ -32,12 +32,23 @@ class ConstraintOdom2DTheta: public ConstraintSparse<2,2,1,2,1>
bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
{
// Expected measurement
- T expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
- T expected_rotation = _o2[0]-_o1[0];
-
- // Residuals
- _residuals[0] = (expected_range - T(measurement_(0))*T(measurement_(0))) / T(measurement_covariance_(0,0));
- _residuals[1] = (expected_rotation - T(measurement_(1))) / T(measurement_covariance_(1,1));
+ // rotar per menys l'angle de primer _o1
+ T expected_longitudinal = cos(_o1[0])*(_p2[0]-_p1[0])+sin(_o1[0])*(_p2[1]-_p1[1]); // cos(-o1)(x2-x1) - sin(-o1)(y2-y1)
+ T expected_lateral = -sin(_o1[0])*(_p2[0]-_p1[0])+cos(_o1[0])*(_p2[1]-_p1[1]); // sin(-o1)(x2-x1) + cos(-o1)(y2-y1)
+ T expected_rotation = _o2[0]-_o1[0];
+
+ // Residuals
+ _residuals[0] = (expected_longitudinal - T(measurement_(0))) / T(measurement_covariance_(0,0));
+ _residuals[1] = (expected_lateral - T(measurement_(1))) / T(measurement_covariance_(1,1));
+ _residuals[2] = (expected_rotation - T(measurement_(2))) / T(measurement_covariance_(2,2));
+
+ // Expected measurement
+// T expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range
+// T expected_rotation = _o2[0]-_o1[0];
+//
+// // Residuals
+// _residuals[0] = (expected_range - T(measurement_(0))*T(measurement_(0))) / T(measurement_covariance_(0,0));
+// _residuals[1] = (expected_rotation - T(measurement_(1))) / T(measurement_covariance_(1,1));
return true;
}
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 849bbf0537273a806af510f5c3d4b6b7f548280b..5f0cafb7e9d06ac6e6ff7043ec6942b8772067cc 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -49,11 +49,11 @@ TARGET_LINK_LIBRARIES(test_ceres_covariance ${PROJECT_NAME})
IF(faramotics_FOUND)
IF (laser_scan_utils_FOUND)
- ADD_EXECUTABLE(test_ceres_laser test_ceres_laser.cpp)
- TARGET_LINK_LIBRARIES(test_ceres_laser
- ${pose_state_time_LIBRARIES}
- ${faramotics_LIBRARIES}
- ${PROJECT_NAME})
+# ADD_EXECUTABLE(test_ceres_laser test_ceres_laser.cpp)
+# TARGET_LINK_LIBRARIES(test_ceres_laser
+# ${pose_state_time_LIBRARIES}
+# ${faramotics_LIBRARIES}
+# ${PROJECT_NAME})
ADD_EXECUTABLE(test_ceres_2_lasers test_ceres_2_lasers.cpp)
TARGET_LINK_LIBRARIES(test_ceres_2_lasers
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index bf0a8a8cc349257129574792818b0fb3f9ca7ec3..6ca7d84363634c8e9e4dea24c97c0582bfa31345 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -154,7 +154,8 @@ int main(int argc, char** argv)
myScanner->loadAssimpModel(modelFileName);
//variables
- Eigen::Vector2s odom_reading, gps_fix_reading;
+ Eigen::Vector3s odom_reading;
+ Eigen::Vector2s gps_fix_reading;
Eigen::VectorXs pose_odom(3); //current odometry integred pose
Eigen::VectorXs ground_truth(n_execution*3); //all true poses
Eigen::VectorXs odom_trajectory(n_execution*3); //open loop trajectory
@@ -175,9 +176,9 @@ 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.1, window_size);
+ WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e9, pose_odom, 0.3, window_size);
- std::cout << "START TRAJECTORY..." << std::endl;
+ //std::cout << "START TRAJECTORY..." << std::endl;
// START TRAJECTORY ============================================================================================
for (uint step=1; step < n_execution; step++)
{
@@ -188,7 +189,8 @@ int main(int argc, char** argv)
//std::cout << "ROBOT MOVEMENT..." << std::endl;
// moves the device position
t1=clock();
- motionCampus(step, devicePose, odom_reading(0), odom_reading(1));
+ motionCampus(step, devicePose, odom_reading(0), odom_reading(2));
+ odom_reading(1) = 0;
devicePoses.push_back(devicePose);
@@ -198,12 +200,13 @@ 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(0) == 0 ? 1e-6 : odom_reading(0));
- odom_reading(1) += distribution_odom(generator)*(odom_reading(1) == 0 ? 1e-6 : odom_reading(1));
+ odom_reading(0) += distribution_odom(generator)*(odom_reading(0) == 0 ? 1e-6 : odom_reading(0));
+ odom_reading(1) += distribution_odom(generator)*1e-6;
+ odom_reading(2) += distribution_odom(generator)*(odom_reading(2) == 0 ? 1e-6 : odom_reading(2));
// odometry integration
- pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2));
- pose_odom(1) = pose_odom(1) + odom_reading(0) * sin(pose_odom(2));
+ pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2)) - odom_reading(1) * sin(pose_odom(2));
+ pose_odom(1) = pose_odom(1) + odom_reading(0) * sin(pose_odom(2)) + odom_reading(1) * cos(pose_odom(2));
pose_odom(2) = pose_odom(2) + odom_reading(1);
odom_trajectory.segment(step*3,3) = pose_odom;
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 2068eb45f079f296abfda6951367b38358c717fe..4d539d6752a2929c3795dbff4ca93f3e6b2d9a27 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -114,15 +114,18 @@ StateStatus FrameBase::getStatus() const
void FrameBase::setState(const Eigen::VectorXs& _st)
{
- assert(_st.size() == ((!p_ptr_ ? 0 : p_ptr_->getStateSize()) +
- (!o_ptr_ ? 0 : o_ptr_->getStateSize()) +
- (!v_ptr_ ? 0 : v_ptr_->getStateSize()) +
- (!w_ptr_ ? 0 : w_ptr_->getStateSize())) &&
- "In FrameBase::setState wrong state size");
+
+ assert(_st.size() == ((p_ptr_==nullptr ? 0 : p_ptr_->getStateSize()) +
+ (o_ptr_==nullptr ? 0 : o_ptr_->getStateSize()) +
+ (v_ptr_==nullptr ? 0 : v_ptr_->getStateSize()) +
+ (w_ptr_==nullptr ? 0 : w_ptr_->getStateSize())) &&
+ "In FrameBase::setState wrong state size");
assert(p_ptr_!=nullptr && "in FrameBase::setState(), p_ptr_ is nullptr");
Eigen::Map<Eigen::VectorXs> state_map(p_ptr_->getPtr(), _st.size());
+ //std::cout << "setting state\noriginal: " << state_map.transpose() << "\nnew: " << _st.transpose() << std::endl;
state_map = _st;
+ //std::cout << "setted state: " << *p_ptr_->getPtr() << " " << *(p_ptr_->getPtr()+1) << std::endl;
}
void FrameBase::addCapture(CaptureBase* _capt_ptr)
diff --git a/src/wolf_manager.h b/src/wolf_manager.h
index ddee86ca39a23d045a463a0a7ca33bfc63a4e985..8e5e30f862b4491a03741aa364879964e06c6805 100644
--- a/src/wolf_manager.h
+++ b/src/wolf_manager.h
@@ -49,6 +49,7 @@ class WolfManager
unsigned int window_size_;
FrameBaseIter first_window_frame_;
CaptureRelative* last_capture_relative_;
+ CaptureRelative* second_last_capture_relative_;
WolfScalar new_frame_elapsed_time_;
public:
@@ -57,7 +58,9 @@ class WolfManager
problem_(new WolfProblem(_state_length)),
sensor_prior_(_sensor_prior),
window_size_(_w_size),
- new_frame_elapsed_time_(_new_frame_elapsed_time)
+ new_frame_elapsed_time_(_new_frame_elapsed_time),
+ last_capture_relative_(nullptr),
+ second_last_capture_relative_(nullptr)
{
createFrame(_init_frame, TimeStamp(0));
problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix();
@@ -85,11 +88,10 @@ class WolfManager
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());
+ CaptureOdom2D* empty_odom = new CaptureOdom2D(_time_stamp,sensor_prior_,Eigen::Vector3s::Zero(),Eigen::Matrix3s::Zero());
+ problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(empty_odom);
+ second_last_capture_relative_ = last_capture_relative_;
+ last_capture_relative_ = (CaptureRelative*)(empty_odom);
}
void addCapture(CaptureBase* _capture)
@@ -109,27 +111,32 @@ class WolfManager
// ODOMETRY SENSOR
if (new_capture->getSensorPtr() == sensor_prior_)
{
- // INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR
- //std::cout << "integrating captures " << last_capture_relative_->nodeId() << " " << new_capture->nodeId() << std::endl;
+ // UPDATE LAST STATE FROM SECOND LAST (optimized)
+ if (second_last_capture_relative_ != nullptr)
+ problem_->getTrajectoryPtr()->getFrameListPtr()->back()->setState(second_last_capture_relative_->computePrior());
+
+ // INTEGRATE NEW ODOMETRY TO LAST FRAME
last_capture_relative_->integrateCapture((CaptureRelative*)(new_capture));
- // NEW KEY FRAME (if enough time from last frame)
+ // INITIALIZE STAMP OF FIRST FRAME
//std::cout << "new TimeStamp - last Frame TimeStamp = " << new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() << std::endl;
if (problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() == 0)
problem_->getTrajectoryPtr()->getFrameListPtr()->back()->setTimeStamp(new_capture->getTimeStamp());
- else if (new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() > new_frame_elapsed_time_)
+
+ // NEW KEY FRAME (if enough time from last frame)
+ 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();
+ auto second_last_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back();
- // NEW FRAME
+ // NEW CURRENT 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();
+ for (auto capture_it = second_last_frame_ptr->getCaptureListPtr()->begin(); capture_it != second_last_frame_ptr->getCaptureListPtr()->end(); capture_it++)
+ (*capture_it)->processCapture();
// WINDOW of FRAMES (remove or fix old frames)
//std::cout << "frame window" << std::endl;
@@ -168,6 +175,9 @@ class WolfManager
Eigen::VectorXs getVehiclePose()
{
+ if (second_last_capture_relative_ != nullptr)
+ problem_->getTrajectoryPtr()->getFrameListPtr()->back()->setState(second_last_capture_relative_->computePrior());
+ //std::cout << "getVehiclePose +++++++++++++++++++++++++\n";
return last_capture_relative_->computePrior();
}
@@ -175,4 +185,14 @@ class WolfManager
{
return problem_;
}
+
+ void setWindowSize(const unsigned int& _size)
+ {
+ window_size_ = _size;
+ }
+
+ void setNewFrameElapsedTime( const WolfScalar& _dt)
+ {
+ new_frame_elapsed_time_ = _dt;
+ }
};