From 40c9e189072bbb7704de31377e0ac8230f3f2a8f Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Fri, 27 Mar 2015 17:47:31 +0000
Subject: [PATCH] states units in sensor base (and constructors of derived),
getting and setting covariance blocks and computing mahalanobis distance (in
progress...)
---
src/capture_base.cpp | 2 +-
src/capture_base.h | 43 +-
src/capture_gps_fix.h | 11 +-
src/capture_laser_2D.cpp | 336 +++++++-----
src/capture_laser_2D.h | 2 +
src/capture_odom_2D.cpp | 161 +++---
src/ceres_wrapper/ceres_manager.cpp | 73 ++-
src/constraint_base.h | 47 +-
src/constraint_corner_2D_theta.h | 10 +-
src/constraint_gps_2D.h | 4 +-
src/constraint_odom_2D_complex_angle.h | 71 ++-
src/constraint_odom_2D_theta.h | 79 ++-
src/examples/CMakeLists.txt | 8 +-
src/examples/test_ceres_2_lasers.cpp | 711 ++++++++++++-------------
src/frame_base.h | 4 +-
src/landmark_base.h | 140 +++--
src/node_linked.h | 1 -
src/sensor_base.cpp | 56 +-
src/sensor_base.h | 60 ++-
src/sensor_gps_fix.cpp | 4 +-
src/sensor_gps_fix.h | 5 +-
src/sensor_laser_2D.cpp | 4 +-
src/sensor_laser_2D.h | 21 +-
src/sensor_odom_2D.cpp | 4 +-
src/sensor_odom_2D.h | 5 +-
src/state_base.h | 7 +
src/state_complex_angle.cpp | 5 +
src/state_complex_angle.h | 9 +-
src/state_orientation.h | 40 +-
src/state_point.h | 8 +-
src/state_theta.cpp | 56 +-
src/state_theta.h | 79 +--
src/wolf.h | 74 ++-
src/wolf_manager.h | 307 ++++++-----
src/wolf_problem.cpp | 64 ++-
src/wolf_problem.h | 22 +-
36 files changed, 1370 insertions(+), 1163 deletions(-)
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index 60cb39225..f2ce6f2a7 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -24,7 +24,7 @@ CaptureBase::CaptureBase(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Ei
data_(_data),
data_covariance_(_data_covariance)
{
- //
+ //std::cout << "created CaptureBase " << nodeId() << std::endl << "covariance: " << std::endl << data_covariance_ << std::endl;
}
CaptureBase::~CaptureBase()
diff --git a/src/capture_base.h b/src/capture_base.h
index a19f47213..64cda3a3b 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -1,4 +1,3 @@
-
#ifndef CAPTURE_BASE_H_
#define CAPTURE_BASE_H_
@@ -18,7 +17,7 @@ class FeatureBase;
#include "sensor_base.h"
//class CaptureBase
-class CaptureBase : public NodeLinked<FrameBase,FeatureBase>
+class CaptureBase : public NodeLinked<FrameBase, FeatureBase>
{
protected:
TimeStamp time_stamp_; ///< Time stamp
@@ -27,44 +26,44 @@ class CaptureBase : public NodeLinked<FrameBase,FeatureBase>
Eigen::MatrixXs data_covariance_; ///< Noise of the capture
Eigen::Vector3s sensor_pose_global_; ///< Sensor pose in world frame: composition of the frame pose and the sensor pose. TODO: use state units
Eigen::Vector3s inverse_sensor_pose_; ///< World pose in the sensor frame: inverse of the global_pose_. TODO: use state units
-
+
public:
- CaptureBase(const TimeStamp& _ts, SensorBase* _sensor_ptr);//TODO: to be removed ??
-
+ CaptureBase(const TimeStamp& _ts, SensorBase* _sensor_ptr); //TODO: to be removed ??
+
CaptureBase(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data);
CaptureBase(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
virtual ~CaptureBase();
-
+
/** \brief Set link to Frame
- *
+ *
* Set link to Frame
*
**/
void linkToFrame(FrameBase* _frm_ptr);
-
+
/** \brief Adds a Feature to the down node list
- *
+ *
* Adds a Feature to the down node list
*
- **/
+ **/
void addFeature(FeatureBase* _ft_ptr);
/** \brief Gets up_node_ptr_
- *
+ *
* Gets up_node_ptr_, which is a pointer to the Frame owning of this Capture
*
- **/
+ **/
FrameBase* getFramePtr() const;
-
+
/** \brief Gets a pointer to feature list
- *
+ *
* Gets a pointer to feature list
*
- **/
+ **/
FeatureBaseList* getFeatureListPtr();
-
+
/** \brief Fills the provided list with all constraints related to this capture
*
* Fills the provided list with all constraints related to this capture
@@ -75,11 +74,11 @@ class CaptureBase : public NodeLinked<FrameBase,FeatureBase>
TimeStamp getTimeStamp() const;
SensorBase* getSensorPtr() const;
-
+
SensorType getSensorType() const;
-
+
void setTimeStamp(const TimeStamp & _ts);
-
+
void setTimeStampToNow();
Eigen::VectorXs getData();
@@ -87,12 +86,12 @@ class CaptureBase : public NodeLinked<FrameBase,FeatureBase>
Eigen::MatrixXs getDataCovariance();
void setData(unsigned int _size, const WolfScalar *_data);
-
+
void setData(const Eigen::VectorXs& _data);
void setDataCovariance(const Eigen::MatrixXs& _data_cov);
-
- virtual void processCapture();// = 0;
+
+ virtual void processCapture(); // = 0;
virtual Eigen::VectorXs computePrior() const = 0;
diff --git a/src/capture_gps_fix.h b/src/capture_gps_fix.h
index cd516619a..42fdd7dca 100644
--- a/src/capture_gps_fix.h
+++ b/src/capture_gps_fix.h
@@ -1,4 +1,3 @@
-
#ifndef CAPTURE_GPS_FIX_H_
#define CAPTURE_GPS_FIX_H_
@@ -13,14 +12,14 @@
class CaptureGPSFix : public CaptureBase
{
public:
- CaptureGPSFix(const TimeStamp& _ts, SensorBase* _sensor_ptr);
+ CaptureGPSFix(const TimeStamp& _ts, SensorBase* _sensor_ptr);
+
+ CaptureGPSFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data);
- CaptureGPSFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data);
+ CaptureGPSFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
- CaptureGPSFix(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
-
virtual ~CaptureGPSFix();
-
+
virtual void processCapture();
virtual Eigen::VectorXs computePrior() const;
diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 432bb2234..d6255c7e2 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -1,6 +1,5 @@
#include "capture_laser_2D.h"
-
// unsigned int CaptureLaser2D::segment_window_size = 8;//window size to extract segments
// double CaptureLaser2D::theta_min = 0.4; //minimum theta between consecutive segments to detect corner. PI/8=0.39
// double CaptureLaser2D::theta_max_parallel = 0.1; //maximum theta between consecutive segments to fuse them in a single line.
@@ -15,11 +14,10 @@
//{
// laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
//}
-CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges):
- CaptureBase(_ts, _sensor_ptr),
- ranges_(_ranges)
+CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges) :
+ CaptureBase(_ts, _sensor_ptr), ranges_(_ranges)
{
- laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
+ laser_ptr_ = (SensorLaser2D*) sensor_ptr_;
}
//CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities):
@@ -29,12 +27,10 @@ CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, c
//{
// laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
//}
-CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges, const std::vector<float>& _intensities):
- CaptureBase(_ts, _sensor_ptr),
- ranges_(_ranges),
- intensities_(_intensities)
+CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges, const std::vector<float>& _intensities) :
+ CaptureBase(_ts, _sensor_ptr), ranges_(_ranges), intensities_(_intensities)
{
- laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
+ laser_ptr_ = (SensorLaser2D*) sensor_ptr_;
}
CaptureLaser2D::~CaptureLaser2D()
@@ -46,16 +42,16 @@ void CaptureLaser2D::processCapture()
{
//variables
//std::list<Eigen::Vector4s> corners;
- std::list<laserscanutils::Corner> corners;
-
+ std::list < laserscanutils::Corner > corners;
+
//extract corners from range data
- extractCorners(corners);
+ extractCorners (corners);
//std::cout << corners.size() << " corners extracted" << std::endl;
-
+
//generate a feature for each corner
createFeatures(corners);
//std::cout << getFeatureListPtr()->size() << " Features created" << std::endl;
-
+
//Establish constraints for each feature
establishConstraints();
//std::cout << "Constraints created" << std::endl;
@@ -63,9 +59,9 @@ void CaptureLaser2D::processCapture()
unsigned int CaptureLaser2D::extractCorners(std::list<laserscanutils::Corner> & _corner_list) const
{
- std::list<laserscanutils::Line> line_list;
- laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, line_list);
- return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), line_list, _corner_list);
+ std::list < laserscanutils::Line > line_list;
+ laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, line_list);
+ return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), line_list, _corner_list);
}
unsigned int CaptureLaser2D::extractLines(std::list<laserscanutils::Line> & _line_list) const
@@ -75,144 +71,226 @@ unsigned int CaptureLaser2D::extractLines(std::list<laserscanutils::Line> & _lin
void CaptureLaser2D::createFeatures(std::list<laserscanutils::Corner> & _corner_list)
{
- // TODO: Sensor model
+ // TODO: Sensor model
Eigen::Matrix4s cov_mat;
Eigen::Vector4s meas;
-
+
//init constant cov
- cov_mat << 0.01, 0, 0, 0,
- 0, 0.01, 0, 0,
- 0, 0, 0.01, 0,
- 0, 0, 0, 0.01;
-
+ cov_mat << 0.01, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0.01;
+
//for each corner in the list create a feature
- for (auto corner_it = _corner_list.begin(); corner_it != _corner_list.end(); corner_it ++)
+ for (auto corner_it = _corner_list.begin(); corner_it != _corner_list.end(); corner_it++)
{
- meas.head(2) = (*corner_it).pt_.head(2);
- meas(2) = (*corner_it).orientation_;
- meas(3) = (*corner_it).aperture_;
- //TODO: add the rest of descriptors and struct atributes
- this->addFeature( (FeatureBase*)(new FeatureCorner2D( meas, cov_mat ) ) );
+ meas.head(2) = (*corner_it).pt_.head(2);
+ meas(2) = (*corner_it).orientation_;
+ meas(3) = (*corner_it).aperture_;
+ //TODO: add the rest of descriptors and struct atributes
+ this->addFeature((FeatureBase*) (new FeatureCorner2D(meas, cov_mat)));
}
}
void CaptureLaser2D::establishConstraints()
{
- // Global transformation TODO: Change by a function
- Eigen::Vector2s t_robot(*getFramePtr()->getPPtr()->getPtr(),*(getFramePtr()->getPPtr()->getPtr()+1));
- Eigen::Matrix2s R_robot = ((StateOrientation*)(getFramePtr()->getOPtr()))->getRotationMatrix().topLeftCorner<2,2>();
- WolfScalar& robot_orientation = *(getFramePtr()->getOPtr()->getPtr());
+ // Global transformation TODO: Change by a function
+ Eigen::Vector2s t_robot = getFramePtr()->getPPtr()->getVector();
+ Eigen::Matrix2s R_robot = ((StateOrientation*) (getFramePtr()->getOPtr()))->getRotationMatrix().topLeftCorner<2, 2>();
+ WolfScalar& robot_orientation = *(getFramePtr()->getOPtr()->getPtr());
- // Sensor transformation
- Eigen::Vector2s t_sensor = getSensorPtr()->getSensorPosition()->head(2);
- Eigen::Matrix2s R_sensor = getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>();
+ // Sensor transformation
+ Eigen::Vector2s t_sensor = getSensorPtr()->getPPtr()->getVector();
+ Eigen::Matrix2s R_sensor = getSensorPtr()->getOPtr()->getRotationMatrix().topLeftCorner<2, 2>();
//Brute force closest (xy and theta) landmark search //TODO: B&B
// std::cout << "Brute force closest (xy and theta) landmark search: N features:" << getFeatureListPtr()->size() << std::endl;
-// std::cout << "N landmark:" << getTop()->getMapPtr()->getLandmarkListPtr()->size() << std::endl;
-// std::cout << "Vehicle transformation: " << std::endl;
+// std::cout << "N landmark:" << getTop()->getMapPtr()->getLandmarkListPtr()->size() << std::endl;
+// std::cout << "Vehicle transformation: " << std::endl;
// std::cout << "t: " << t.transpose() << std::endl;
// std::cout << "rot:" << R << std::endl;
- for (auto feature_it = getFeatureListPtr()->begin(); feature_it != getFeatureListPtr()->end(); feature_it++ )
- {
- WolfScalar max_distance_matching_sq = 0.5;
- WolfScalar max_theta_matching = M_PI / 16;
- WolfScalar min_distance_sq = max_distance_matching_sq;
-
- //Find the closest landmark to the feature
- LandmarkCorner2D* correspondent_landmark = nullptr;
- const Eigen::Vector2s& feature_position = (*feature_it)->getMeasurement().head(2);
- const WolfScalar& feature_orientation = (*feature_it)->getMeasurement()(2);
- const WolfScalar& feature_aperture = (*feature_it)->getMeasurement()(3);
-
- Eigen::Vector2s feature_global_position = R_robot * (R_sensor * feature_position + t_sensor) + t_robot;
- WolfScalar feature_global_orientation = feature_orientation + robot_orientation + atan2(R_sensor(1,0),R_sensor(0,0));
- feature_global_orientation = (feature_global_orientation > 0 ? // fit in (-pi, pi]
- 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;
- 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());
- WolfScalar& landmark_orientation = *((*landmark_it)->getOPtr()->getPtr());
- const WolfScalar& landmark_aperture = (*landmark_it)->getDescriptor()(0);
-
- // First check: APERTURE
- //std::cout << " aperture diff: " << fabs(feature_aperture - landmark_aperture);
- if (fabs(feature_aperture - landmark_aperture) < max_theta_matching)
- {
- //std::cout << " OK!" << std::endl;
-
- // Second check: SQUARED DISTANCE
- WolfScalar distance_sq = (landmark_position-feature_global_position).squaredNorm();
- //std::cout << " distance squared: " << distance_sq;
- 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 << " 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]
+ for (auto feature_it = getFeatureListPtr()->begin(); feature_it != getFeatureListPtr()->end(); feature_it++)
+ {
+ WolfScalar max_distance_matching_sq = 0.5;
+ WolfScalar max_theta_matching = M_PI / 16;
+ WolfScalar min_distance_sq = max_distance_matching_sq;
+
+ //Find the closest landmark to the feature
+ LandmarkCorner2D* correspondent_landmark = nullptr;
+ const Eigen::Vector2s& feature_position = (*feature_it)->getMeasurement().head(2);
+ const WolfScalar& feature_orientation = (*feature_it)->getMeasurement()(2);
+ const WolfScalar& feature_aperture = (*feature_it)->getMeasurement()(3);
+
+ Eigen::Vector2s feature_global_position = R_robot * (R_sensor * feature_position + t_sensor) + t_robot;
+ WolfScalar feature_global_orientation = feature_orientation + robot_orientation + atan2(R_sensor(1, 0), R_sensor(0, 0));
+ feature_global_orientation = (feature_global_orientation > 0 ? // fit in (-pi, pi]
+ 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;
+ 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());
+ WolfScalar& landmark_orientation = *((*landmark_it)->getOPtr()->getPtr());
+ const WolfScalar& landmark_aperture = (*landmark_it)->getDescriptor()(0);
+
+ // First check: APERTURE
+ // std::cout << " aperture diff: " << fabs(feature_aperture - landmark_aperture);
+ if (fabs(feature_aperture - landmark_aperture) < max_theta_matching)
+ {
+ //std::cout << " OK!" << std::endl;
+
+ // MAHALANOBIS DISTANCE
+ WolfScalar mahalanobis_distance = computeMahalanobisDistance((*feature_it),(*landmark_it));
+
+ // Second check: SQUARED DISTANCE
+ WolfScalar distance_sq = (landmark_position - feature_global_position).squaredNorm();
+ //std::cout << " distance squared: " << distance_sq;
+ 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 << " 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]
// 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;
-
- 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;
- }
- if (correspondent_landmark == nullptr)
- {
+ if (theta_distance < max_theta_matching)
+ {
+ // 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;
+ }
+ if (correspondent_landmark == nullptr)
+ {
// 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());
- getTop()->addState(new_landmark_state_orientation, Eigen::Map<Eigen::Vector1s>(&feature_global_orientation,1));
+ 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());
+ getTop()->addState(new_landmark_state_orientation, Eigen::Map < Eigen::Vector1s > (&feature_global_orientation, 1));
- correspondent_landmark = new LandmarkCorner2D(new_landmark_state_position, new_landmark_state_orientation, feature_aperture);
- LandmarkBase* corr_landmark(correspondent_landmark);
- getTop()->getMapPtr()->addLandmark(corr_landmark);
+ correspondent_landmark = new LandmarkCorner2D(new_landmark_state_position, new_landmark_state_orientation, feature_aperture);
+ LandmarkBase* corr_landmark(correspondent_landmark);
+ getTop()->getMapPtr()->addLandmark(corr_landmark);
-// std::cout << "Landmark created: " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << 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();
-
-// std::cout << "Creating new constraint: Landmark " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << " & feature " << (*feature_it)->nodeId() << std::endl;
-
- // Add constraint to the correspondent landmark
- (*feature_it)->addConstraint(new ConstraintCorner2DTheta(*feature_it,
- correspondent_landmark,
- getFramePtr()->getPPtr(),//_robotPPtr,
- getFramePtr()->getOPtr(),//_robotOPtr,
- correspondent_landmark->getPPtr(), //_landmarkPPtr,
- correspondent_landmark->getOPtr())); //_landmarkOPtr,
- }
+ }
+ else
+ correspondent_landmark->hit();
+
+ // std::cout << "Creating new constraint: Landmark " << getTop()->getMapPtr()->getLandmarkListPtr()->back()->nodeId() << " & feature "
+ // << (*feature_it)->nodeId() << std::endl;
+
+ // Add constraint to the correspondent landmark
+ (*feature_it)->addConstraint(new ConstraintCorner2DTheta(*feature_it, correspondent_landmark, getFramePtr()->getPPtr(), //_robotPPtr,
+ getFramePtr()->getOPtr(), //_robotOPtr,
+ correspondent_landmark->getPPtr(), //_landmarkPPtr,
+ correspondent_landmark->getOPtr())); //_landmarkOPtr,
+ }
}
Eigen::VectorXs CaptureLaser2D::computePrior() const
{
- return Eigen::Vector3s(1,2,3);
+ return Eigen::Vector3s(1, 2, 3);
}
+WolfScalar CaptureLaser2D::computeMahalanobisDistance(const FeatureBase* _feature_ptr, const LandmarkBase* _landmark_ptr)
+{
+ FrameBase* frame_ptr = _feature_ptr->getFramePtr();
+ unsigned int frame_p_size = frame_ptr->getPPtr()->getStateSize();
+ unsigned int frame_o_size = frame_ptr->getOPtr()->getStateSize();
+ unsigned int landmark_p_size = _landmark_ptr->getPPtr()->getStateSize();
+ unsigned int landmark_o_size = _landmark_ptr->getOPtr()->getStateSize();
+
+ Eigen::Vector2s p_robot = getFramePtr()->getPPtr()->getVector();
+ Eigen::Matrix2s R_robot = ((StateOrientation*) (getFramePtr()->getOPtr()))->getRotationMatrix().topLeftCorner<2,2>();
+ Eigen::Vector2s p_sensor = getSensorPtr()->getPPtr()->getVector();
+ Eigen::Matrix2s R_sensor = getSensorPtr()->getOPtr()->getRotationMatrix().topLeftCorner<2, 2>();
+ Eigen::Vector2s p_landmark = _landmark_ptr->getPPtr()->getVector();
+
+ // Fill sigma matrix (upper diagonal only)
+ Eigen::MatrixXs Sigma(frame_p_size + frame_o_size + landmark_p_size + landmark_o_size, frame_p_size + frame_o_size + landmark_p_size + landmark_o_size);
+
+ Eigen::Map<Eigen::MatrixXs> frame_p_frame_p_cov(&Sigma(0,0),frame_p_size,frame_p_size);
+ Eigen::Map<Eigen::MatrixXs> frame_p_frame_o_cov(&Sigma(0,frame_p_size),frame_p_size,frame_o_size);
+ Eigen::Map<Eigen::MatrixXs> frame_p_landmark_p_cov(&Sigma(0,frame_p_size+frame_o_size),frame_p_size,landmark_p_size);
+ Eigen::Map<Eigen::MatrixXs> frame_p_landmark_o_cov(&Sigma(0,frame_p_size+frame_o_size+landmark_p_size),frame_p_size,landmark_o_size);
+ getTop()->getCovarianceBlock(frame_ptr->getPPtr(), frame_ptr->getPPtr(), frame_p_frame_p_cov);
+ getTop()->getCovarianceBlock(frame_ptr->getPPtr(), frame_ptr->getOPtr(), frame_p_frame_o_cov);
+ getTop()->getCovarianceBlock(frame_ptr->getPPtr(), _landmark_ptr->getPPtr(), frame_p_landmark_p_cov);
+ getTop()->getCovarianceBlock(frame_ptr->getPPtr(), _landmark_ptr->getOPtr(), frame_p_landmark_o_cov);
+
+ Eigen::Map<Eigen::MatrixXs> frame_o_frame_o_cov(&Sigma(frame_p_size,frame_p_size),frame_o_size,frame_o_size);
+ Eigen::Map<Eigen::MatrixXs> frame_o_landmark_p_cov(&Sigma(frame_p_size,frame_p_size+frame_o_size),frame_o_size,landmark_p_size);
+ Eigen::Map<Eigen::MatrixXs> frame_o_landmark_o_cov(&Sigma(frame_p_size,frame_p_size+frame_o_size+landmark_p_size),frame_o_size,landmark_o_size);
+ getTop()->getCovarianceBlock(frame_ptr->getOPtr(), frame_ptr->getOPtr(), frame_o_frame_o_cov);
+ getTop()->getCovarianceBlock(frame_ptr->getOPtr(), _landmark_ptr->getPPtr(), frame_o_landmark_p_cov);
+ getTop()->getCovarianceBlock(frame_ptr->getOPtr(), _landmark_ptr->getOPtr(), frame_o_landmark_o_cov);
+
+ Eigen::Map<Eigen::MatrixXs> landmark_p_landmark_p_cov(&Sigma(frame_p_size+frame_o_size,frame_p_size+frame_o_size),landmark_p_size,landmark_p_size);
+ Eigen::Map<Eigen::MatrixXs> landmark_p_landmark_o_cov(&Sigma(frame_p_size+frame_o_size,frame_p_size+frame_o_size+landmark_p_size),landmark_p_size,landmark_o_size);
+ getTop()->getCovarianceBlock(_landmark_ptr->getPPtr(), _landmark_ptr->getPPtr(), landmark_p_landmark_p_cov);
+ getTop()->getCovarianceBlock(_landmark_ptr->getPPtr(), _landmark_ptr->getOPtr(), landmark_p_landmark_o_cov);
+
+ Eigen::Map<Eigen::MatrixXs> landmark_o_landmark_o_cov(&Sigma(frame_p_size+frame_o_size+landmark_p_size,frame_p_size+frame_o_size+landmark_p_size),landmark_o_size,landmark_o_size);
+ getTop()->getCovarianceBlock(_landmark_ptr->getOPtr(), _landmark_ptr->getOPtr(), landmark_o_landmark_o_cov);
+
+ // Jacobian
+ if (_landmark_ptr->getOPtr()->getStateType() == ST_THETA && frame_ptr->getOPtr()->getStateType() == ST_THETA)
+ {
+ Eigen::MatrixXs J = Eigen::MatrixXs::Zero(6,3);
+ WolfScalar theta_s_r = *getSensorPtr()->getOPtr()->getPtr() + *frame_ptr->getOPtr()->getPtr(); // Sum of theta_sensor and theta_robot
+
+ J(0,0) = -cos(theta_s_r);
+ J(0,1) = sin(theta_s_r);
+ J(1,0) = -sin(theta_s_r);
+ J(1,1) = -cos(theta_s_r);
+ J(2,0) = -sin(theta_s_r) * (p_landmark(0) - p_robot(0)) - cos(theta_s_r) * (p_landmark(1) - p_robot(1));
+ J(2,1) = cos(theta_s_r) * (p_landmark(0) - p_robot(0)) - sin(theta_s_r) * (p_landmark(1) - p_robot(1));
+ J(2,2) = -1;
+ J(3,0) = cos(theta_s_r);
+ J(3,1) = -sin(theta_s_r);
+ J(4,0) = sin(theta_s_r);
+ J(4,1) = cos(theta_s_r);
+ J(5,2) = 1;
+
+ // Feature-Landmark (euclidean) distance
+ const Eigen::Vector2s& feature_position = _feature_ptr->getMeasurement().head(2);
+ const WolfScalar& feature_orientation = _feature_ptr->getMeasurement()(2);
+
+ Eigen::Vector2s landmark_local_position = R_sensor.transpose() * (R_robot.transpose() * (p_landmark - p_robot) - p_sensor);
+ WolfScalar landmark_local_orientation = *_landmark_ptr->getOPtr()->getPtr() - *frame_ptr->getOPtr()->getPtr() - *getSensorPtr()->getOPtr()->getPtr();
+
+ Eigen::Vector3s d_euclidean;
+ d_euclidean.head(2) = feature_position - landmark_local_position;
+ d_euclidean(2) = feature_orientation - landmark_local_orientation;
+ // fit in (-pi, pi]
+ d_euclidean(2) = (d_euclidean(2) > 0 ? fmod(d_euclidean(2) + M_PI, 2 * M_PI) - M_PI : fmod(d_euclidean(2) - M_PI, 2 * M_PI) + M_PI);
+
+ // Feature-Landmark Mahalanobis distance
+ // covariance: Eigen::Matrix3s S_m = J * Sigma.selfadjointView<Eigen::Upper>() * J.transpose() + _feature_ptr->getMeasurementCovariance().topLeftCorner<3,3>();
+ WolfScalar mahalanobis_distance = d_euclidean.transpose() * (J.transpose() * Sigma.selfadjointView<Eigen::Upper>() * J + _feature_ptr->getMeasurementCovariance().topLeftCorner<3,3>()).inverse() * d_euclidean;
+ std::cout << "mahalanobis_distance = " << mahalanobis_distance << std::endl;
+ return mahalanobis_distance;
+ }
+ else
+ {
+ std::cout << "ERROR: Jacobian using complex angles not computed..." << std::endl; //TODO
+ assert(false);
+ return 0;
+ }
+}
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index 7cbc10f49..27d94f439 100644
--- a/src/capture_laser_2D.h
+++ b/src/capture_laser_2D.h
@@ -105,5 +105,7 @@ class CaptureLaser2D : public CaptureBase
void establishConstraints();
virtual Eigen::VectorXs computePrior() const;
+
+ WolfScalar computeMahalanobisDistance(const FeatureBase* _feature, const LandmarkBase* _landmark);
};
#endif /* CAPTURE_LASER_2D_H_ */
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index c784db7bb..d4d914a26 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -1,128 +1,109 @@
#include "capture_odom_2D.h"
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr) :
- CaptureRelative(_ts, _sensor_ptr)
+ CaptureRelative(_ts, _sensor_ptr)
{
//
}
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::Vector3s& _data) :
- CaptureRelative(_ts, _sensor_ptr, _data)
+ CaptureRelative(_ts, _sensor_ptr, _data)
{
- 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)->getDisplacementNoiseFactor();
- data_covariance_(2,2) = (_data(2) == 0 ? 1e-6 : _data(2))*((SensorOdom2D*)_sensor_ptr)->getRotationNoiseFactor();
+ data_covariance_ = Eigen::Matrix3s::Zero();
+ data_covariance_(0, 0) = (_data(0) == 0 ? 1e-6 : fabs(_data(0))) * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor();
+ data_covariance_(1, 1) = (_data(1) == 0 ? 1e-6 : fabs(_data(1))) * ((SensorOdom2D*) _sensor_ptr)->getDisplacementNoiseFactor();
+ data_covariance_(2, 2) = (_data(2) == 0 ? 1e-6 : fabs(_data(2))) * ((SensorOdom2D*) _sensor_ptr)->getRotationNoiseFactor();
// std::cout << data_covariance_ << std::endl;
}
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _ts, SensorBase* _sensor_ptr, const Eigen::Vector3s& _data, const Eigen::Matrix3s& _data_covariance) :
- CaptureRelative(_ts, _sensor_ptr, _data, _data_covariance)
+ CaptureRelative(_ts, _sensor_ptr, _data, _data_covariance)
{
- //
+ //
}
CaptureOdom2D::~CaptureOdom2D()
{
- //std::cout << "Destroying GPS fix capture...\n";
+ //std::cout << "Destroying GPS fix capture...\n";
}
inline void CaptureOdom2D::processCapture()
{
- //std::cout << "CaptureOdom2D::addFeature..." << std::endl;
- // ADD FEATURE
- addFeature(new FeatureOdom2D(data_,data_covariance_));
+ //std::cout << "CaptureOdom2D::addFeature..." << std::endl;
+ // ADD FEATURE
+ 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
{
- assert(up_node_ptr_ != nullptr && "This Capture is not linked to any frame");
-
- if (getFramePtr()->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
- {
- 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::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;
- }
-
+ assert(up_node_ptr_ != nullptr && "This Capture is not linked to any frame");
+
+ if (getFramePtr()->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
+ {
+ Eigen::Vector4s prior;
+ Eigen::Map<Eigen::Vector4s> initial_pose(getFramePtr()->getPPtr()->getPtr());
+ ///std::cout << "initial_pose: " << initial_pose.transpose() << std::endl;
+ 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::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) * 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()
{
- assert(getFramePtr()->getNextFrame() != nullptr && "Trying to add odometry constraint in the last frame (there is no next frame)");
-
- if (getFramePtr()->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
- {
- getFeatureListPtr()->front()->addConstraint(new ConstraintOdom2DComplexAngle(getFeatureListPtr()->front(),
- 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()));
- }
+ assert(getFramePtr()->getNextFrame() != nullptr && "Trying to add odometry constraint in the last frame (there is no next frame)");
+
+ if (getFramePtr()->getOPtr()->getStateType() == ST_COMPLEX_ANGLE)
+ {
+ getFeatureListPtr()->front()->addConstraint(new ConstraintOdom2DComplexAngle(getFeatureListPtr()->front(),
+ 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()));
+ }
}
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);
-
- //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;
+ assert(dynamic_cast<CaptureOdom2D*>(_new_capture) && "Trying to integrate with a CaptureOdom2D a CaptureRelativePtr which is not CaptureOdom2D");
+
+ //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);
+ data_covariance_ += _new_capture->getDataCovariance();
+ //std::cout << "Integrated odoms: " << std::endl << data_.transpose() << std::endl << data_covariance_ << std::endl;
}
//void CaptureOdom2D::printSelf(unsigned int _ntabs, std::ostream & _ost) const
@@ -134,5 +115,3 @@ void CaptureOdom2D::integrateCapture(CaptureRelative* _new_capture)
// //_ost << "\tSensor intrinsic : ( " << sensor_ptr_->intrinsic().transpose() << " )" << std::endl;
//}
-
-
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 0cc8ea757..190eff4d0 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -4,8 +4,8 @@ CeresManager::CeresManager(ceres::Problem::Options _options) :
ceres_problem_(new ceres::Problem(_options))
{
ceres::Covariance::Options covariance_options;
- covariance_options.algorithm_type = ceres::DENSE_SVD;
- covariance_options.null_space_rank = -1;
+ covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
+ //covariance_options.null_space_rank = -1;
covariance_ = new ceres::Covariance(covariance_options);
}
@@ -37,11 +37,14 @@ ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_
void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
{
+ // CREATE DESIRED COVARIANCES LIST
std::vector<std::pair<const double*, const double*>> covariance_blocks;
- // Last frame state units
- double* current_position_ptr = _problem_ptr->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr()->getPtr();
- double* current_orientation_ptr = _problem_ptr->getTrajectoryPtr()->getFrameListPtr()->back()->getOPtr()->getPtr();
+ // Last frame
+ StateBase* current_position = _problem_ptr->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr();
+ StateBase* current_orientation = _problem_ptr->getTrajectoryPtr()->getFrameListPtr()->back()->getOPtr();
+ double* current_position_ptr = current_position->getPtr();
+ double* current_orientation_ptr = current_orientation->getPtr();
covariance_blocks.push_back(std::make_pair(current_position_ptr,current_position_ptr));
covariance_blocks.push_back(std::make_pair(current_position_ptr,current_orientation_ptr));
covariance_blocks.push_back(std::make_pair(current_orientation_ptr,current_orientation_ptr));
@@ -53,25 +56,63 @@ void CeresManager::computeCovariances(WolfProblem* _problem_ptr)
double* landmark_orientation_ptr = (*landmark_it)->getOPtr()->getPtr();
covariance_blocks.push_back(std::make_pair(landmark_position_ptr,landmark_position_ptr));
+ covariance_blocks.push_back(std::make_pair(landmark_position_ptr,landmark_orientation_ptr));
+ covariance_blocks.push_back(std::make_pair(landmark_orientation_ptr,landmark_orientation_ptr));
covariance_blocks.push_back(std::make_pair(landmark_position_ptr,current_position_ptr));
covariance_blocks.push_back(std::make_pair(landmark_position_ptr,current_orientation_ptr));
- covariance_blocks.push_back(std::make_pair(landmark_position_ptr,landmark_orientation_ptr));
- covariance_blocks.push_back(std::make_pair(landmark_orientation_ptr,landmark_orientation_ptr));
covariance_blocks.push_back(std::make_pair(landmark_orientation_ptr,current_position_ptr));
covariance_blocks.push_back(std::make_pair(landmark_orientation_ptr,current_orientation_ptr));
}
+
+ // COMPUTE DESIRED COVARIANCES
covariance_->Compute(covariance_blocks, ceres_problem_);
- double c11[2 * 2];
- double c22[1 * 1];
- double c12[2 * 1];
- covariance_->GetCovarianceBlock(current_position_ptr, current_position_ptr, c11);
- covariance_->GetCovarianceBlock(current_position_ptr, current_orientation_ptr, c12);
- covariance_->GetCovarianceBlock(current_orientation_ptr, current_orientation_ptr, c22);
+ // STORE DESIRED COVARIANCES
+ // Last frame
+ Eigen::MatrixXs m_pp(current_position->getStateSize(),current_position->getStateSize());
+ Eigen::MatrixXs m_oo(current_orientation->getStateSize(),current_orientation->getStateSize());
+ Eigen::MatrixXs m_po(current_position->getStateSize(),current_orientation->getStateSize());
+
+ covariance_->GetCovarianceBlock(current_position_ptr, current_position_ptr, m_pp.data());
+ covariance_->GetCovarianceBlock(current_position_ptr, current_orientation_ptr, m_po.data());
+ covariance_->GetCovarianceBlock(current_orientation_ptr, current_orientation_ptr, m_oo.data());
-// std::cout << "COV\n: " << c11[0] << " " << c11[1] << " " << c12[0] << std::endl <<
-// c11[2] << " " << c11[3] << " " << c12[1] << std::endl <<
-// c12[0] << " " << c12[1] << " " << c22[0] << std::endl;
+ _problem_ptr->addCovarianceBlock(current_position, current_position, m_pp);
+ _problem_ptr->addCovarianceBlock(current_orientation, current_orientation, m_oo);
+ _problem_ptr->addCovarianceBlock(current_position, current_orientation, m_po);
+
+ // Landmarks and cross-covariance with current frame
+ for(auto landmark_it = _problem_ptr->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it!=_problem_ptr->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
+ {
+ StateBase* landmark_position = (*landmark_it)->getPPtr();
+ StateBase* landmark_orientation = (*landmark_it)->getOPtr();
+ double* landmark_position_ptr = landmark_position->getPtr();
+ double* landmark_orientation_ptr = landmark_orientation->getPtr();
+
+ Eigen::MatrixXs m_landmark_pp(landmark_position->getStateSize(),landmark_position->getStateSize());
+ Eigen::MatrixXs m_landmark_po(landmark_position->getStateSize(),landmark_orientation->getStateSize());
+ Eigen::MatrixXs m_landmark_oo(landmark_orientation->getStateSize(),landmark_orientation->getStateSize());
+ Eigen::MatrixXs m_landmark_p_frame_p(landmark_position->getStateSize(),current_position->getStateSize());
+ Eigen::MatrixXs m_landmark_p_frame_o(landmark_position->getStateSize(),current_orientation->getStateSize());
+ Eigen::MatrixXs m_landmark_o_frame_p(landmark_orientation->getStateSize(),current_position->getStateSize());
+ Eigen::MatrixXs m_landmark_o_frame_o(landmark_orientation->getStateSize(),current_orientation->getStateSize());
+
+ covariance_->GetCovarianceBlock(landmark_position_ptr, landmark_position_ptr, m_landmark_pp.data());
+ covariance_->GetCovarianceBlock(landmark_position_ptr, landmark_orientation_ptr, m_landmark_po.data());
+ covariance_->GetCovarianceBlock(landmark_orientation_ptr, landmark_orientation_ptr, m_landmark_oo.data());
+ covariance_->GetCovarianceBlock(landmark_position_ptr, current_position_ptr, m_landmark_p_frame_p.data());
+ covariance_->GetCovarianceBlock(landmark_position_ptr, current_orientation_ptr, m_landmark_p_frame_o.data());
+ covariance_->GetCovarianceBlock(landmark_orientation_ptr, current_position_ptr, m_landmark_o_frame_p.data());
+ covariance_->GetCovarianceBlock(landmark_orientation_ptr, current_orientation_ptr, m_landmark_o_frame_o.data());
+
+ _problem_ptr->addCovarianceBlock(landmark_position, landmark_position, m_landmark_pp);
+ _problem_ptr->addCovarianceBlock(landmark_position, landmark_orientation, m_landmark_po);
+ _problem_ptr->addCovarianceBlock(landmark_orientation, landmark_orientation, m_landmark_oo);
+ _problem_ptr->addCovarianceBlock(landmark_position, current_position, m_landmark_p_frame_p);
+ _problem_ptr->addCovarianceBlock(landmark_position, current_orientation, m_landmark_p_frame_o);
+ _problem_ptr->addCovarianceBlock(landmark_orientation, current_position, m_landmark_o_frame_p);
+ _problem_ptr->addCovarianceBlock(landmark_orientation, current_orientation, m_landmark_o_frame_o);
+ }
}
void CeresManager::update(WolfProblem* _problem_ptr)
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 4824c8996..4bf7666b5 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -1,4 +1,3 @@
-
#ifndef CONSTRAINT_BASE_H_
#define CONSTRAINT_BASE_H_
@@ -17,26 +16,26 @@ class NodeTerminus;
#include "node_terminus.h"
//class ConstraintBase
-class ConstraintBase : public NodeLinked<FeatureBase,NodeTerminus>
+class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus>
{
protected:
ConstraintType type_; //type of constraint (types defined at wolf.h)
const Eigen::VectorXs& measurement_; // TODO:TBD: pointer, map or copy of the feature measurement?
const Eigen::MatrixXs& measurement_covariance_; // TODO:TBD: pointer, map or copy of the feature measurement covariance?
-
+
public:
/** \brief Constructor
*
* Constructor
*
- **/
+ **/
ConstraintBase(FeatureBase* _ftr_ptr, ConstraintType _tp);
/** \brief Destructor
*
* Destructor
*
- **/
+ **/
virtual ~ConstraintBase();
/** \brief Returns the constraint type
@@ -45,34 +44,34 @@ class ConstraintBase : public NodeLinked<FeatureBase,NodeTerminus>
*
**/
ConstraintType getConstraintType() const;
-
+
/** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
- *
- * Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint.
- *
- **/
+ *
+ * Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint.
+ *
+ **/
virtual const std::vector<WolfScalar*> getStateBlockPtrVector() = 0;
/** \brief Returns a pointer to the feature measurement
- *
- * Returns a pointer to the feature measurement
- *
- **/
+ *
+ * Returns a pointer to the feature measurement
+ *
+ **/
const Eigen::VectorXs& getMeasurement();
/** \brief Returns a pointer to its capture
- *
- * Returns a pointer to its capture
- *
- **/
- FeatureBase* getFeaturePtr() const;
+ *
+ * Returns a pointer to its capture
+ *
+ **/
+ FeatureBase* getFeaturePtr() const;
/** \brief Returns a pointer to its capture
- *
- * Returns a pointer to its capture
- *
- **/
- CaptureBase* getCapturePtr() const;
+ *
+ * Returns a pointer to its capture
+ *
+ **/
+ CaptureBase* getCapturePtr() const;
};
#endif
diff --git a/src/constraint_corner_2D_theta.h b/src/constraint_corner_2D_theta.h
index 94767310b..d3a0e6bbf 100644
--- a/src/constraint_corner_2D_theta.h
+++ b/src/constraint_corner_2D_theta.h
@@ -57,8 +57,8 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
// std::cout << "atan2: " << atan2(getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,1),getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,0)) << std::endl;
// sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getSensorPosition()->head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>().transpose()).cast<T>();
+ Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getVector().head(2).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = (getCapturePtr()->getSensorPtr()->getOPtr()->getRotationMatrix().topLeftCorner<2,2>().transpose()).cast<T>();
Eigen::Matrix<T,2,2> inverse_R_robot;
inverse_R_robot << cos(*_robotO), sin(*_robotO),
@@ -74,9 +74,9 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
expected_landmark_relative_orientation = expected_landmark_relative_orientation + T(2*M_PI);
// Residuals
- _residuals[0] = (expected_landmark_relative_position(0) - T(measurement_(0))) / T(measurement_covariance_(0,0));
- _residuals[1] = (expected_landmark_relative_position(1) - T(measurement_(1))) / T(measurement_covariance_(1,1));
- _residuals[2] = (expected_landmark_relative_orientation - T(measurement_(2))) / T(100*measurement_covariance_(2,2));
+ _residuals[0] = (expected_landmark_relative_position(0) - T(measurement_(0))) / T(sqrt(measurement_covariance_(0,0)));
+ _residuals[1] = (expected_landmark_relative_position(1) - T(measurement_(1))) / T(sqrt(measurement_covariance_(1,1)));
+ _residuals[2] = (expected_landmark_relative_orientation - T(measurement_(2))) / T(sqrt(measurement_covariance_(2,2)));
// std::cout << "\nCONSTRAINT: " << nodeId() << std::endl;
// std::cout << "Feature: " << getFeaturePtr()->nodeId() << std::endl;
diff --git a/src/constraint_gps_2D.h b/src/constraint_gps_2D.h
index f3e82559f..9b94e4a09 100644
--- a/src/constraint_gps_2D.h
+++ b/src/constraint_gps_2D.h
@@ -31,8 +31,8 @@ class ConstraintGPS2D: public ConstraintSparse<2,2>
template <typename T>
bool operator()(const T* const _x, T* _residuals) const
{
- _residuals[0] = (T(measurement_(0)) - _x[0]) / T(measurement_covariance_(0,0));
- _residuals[1] = (T(measurement_(1)) - _x[1]) / T(measurement_covariance_(1,1));
+ _residuals[0] = (T(measurement_(0)) - _x[0]) / T(sqrt(measurement_covariance_(0,0)));
+ _residuals[1] = (T(measurement_(1)) - _x[1]) / T(sqrt(measurement_covariance_(1,1)));
return true;
}
};
diff --git a/src/constraint_odom_2D_complex_angle.h b/src/constraint_odom_2D_complex_angle.h
index 928df9f49..4597709e0 100644
--- a/src/constraint_odom_2D_complex_angle.h
+++ b/src/constraint_odom_2D_complex_angle.h
@@ -1,4 +1,3 @@
-
#ifndef CONSTRAINT_ODOM_2D_COMPLEX_ANGLE_H_
#define CONSTRAINT_ODOM_2D_COMPLEX_ANGLE_H_
@@ -6,50 +5,50 @@
#include "wolf.h"
#include "constraint_sparse.h"
-class ConstraintOdom2DComplexAngle: public ConstraintSparse<3,2,2,2,2>
+class ConstraintOdom2DComplexAngle : public ConstraintSparse<3, 2, 2, 2, 2>
{
- public:
- static const unsigned int N_BLOCKS = 4;
+ public:
+ static const unsigned int N_BLOCKS = 4;
- ConstraintOdom2DComplexAngle(FeatureBase* _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
- ConstraintSparse<3,2,2,2,2>(_ftr_ptr,CTR_ODOM_2D_COMPLEX_ANGLE, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
- {
- //
- }
+ ConstraintOdom2DComplexAngle(FeatureBase* _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _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<3,2,2,2,2>(_ftr_ptr,CTR_ODOM_2D_COMPLEX_ANGLE, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
- {
- //
- }
+ ConstraintOdom2DComplexAngle(FeatureBase* _ftr_ptr, StateBase* _state0Ptr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr) :
+ ConstraintSparse<3, 2, 2, 2, 2>(_ftr_ptr, CTR_ODOM_2D_COMPLEX_ANGLE, _state0Ptr->getPtr(), _state1Ptr->getPtr(), _state2Ptr->getPtr(), _state3Ptr->getPtr())
+ {
+ //
+ }
virtual ~ConstraintOdom2DComplexAngle()
- {
- //
- }
-
- template <typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
- {
- // Expected measurement
- // 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
+ {
+ //
+ }
+
+ template<typename T>
+ bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
+ {
+ // Expected measurement
+ // 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(sqrt(measurement_covariance_(0, 0)));
+ _residuals[1] = (expected_lateral - T(measurement_(1))) / T(sqrt(measurement_covariance_(1, 1)));
+ _residuals[2] = (expected_rotation - T(measurement_(2))) / T(sqrt(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
// _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;
- }
+ return true;
+ }
};
#endif
diff --git a/src/constraint_odom_2D_theta.h b/src/constraint_odom_2D_theta.h
index f632e1b0e..9e2024207 100644
--- a/src/constraint_odom_2D_theta.h
+++ b/src/constraint_odom_2D_theta.h
@@ -1,4 +1,3 @@
-
#ifndef CONSTRAINT_ODOM_2D_THETA_H_
#define CONSTRAINT_ODOM_2D_THETA_H_
@@ -6,51 +5,51 @@
#include "wolf.h"
#include "constraint_sparse.h"
-class ConstraintOdom2DTheta: public ConstraintSparse<3,2,1,2,1>
+class ConstraintOdom2DTheta : public ConstraintSparse<3, 2, 1, 2, 1>
{
- public:
- static const unsigned int N_BLOCKS = 4;
-
- ConstraintOdom2DTheta(FeatureBase*_ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _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<3,2,1,2,1>(_ftr_ptr,CTR_ODOM_2D_THETA, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr())
- {
- //
- }
-
- virtual ~ConstraintOdom2DTheta()
- {
- //
- }
-
- template <typename T>
- bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
- {
- // Expected measurement
- // 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
+ public:
+ static const unsigned int N_BLOCKS = 4;
+
+ ConstraintOdom2DTheta(FeatureBase*_ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _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<3, 2, 1, 2, 1>(_ftr_ptr, CTR_ODOM_2D_THETA, _state0Ptr->getPtr(), _state1Ptr->getPtr(), _state2Ptr->getPtr(), _state3Ptr->getPtr())
+ {
+ //
+ }
+
+ virtual ~ConstraintOdom2DTheta()
+ {
+ //
+ }
+
+ template<typename T>
+ bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const
+ {
+ // Expected measurement
+ // 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(sqrt(measurement_covariance_(0, 0)));
+ _residuals[1] = (expected_lateral - T(measurement_(1))) / T(sqrt(measurement_covariance_(1, 1)));
+ _residuals[2] = (expected_rotation - T(measurement_(2))) / T(sqrt(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];
+// 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;
- }
+ return true;
+ }
};
#endif
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 5f0cafb7e..d9a608578 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -19,8 +19,8 @@ ADD_EXECUTABLE(test_ceres_wrapper_jet_2_sensors test_ceres_wrapper_jet_2_sensors
TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet_2_sensors ${PROJECT_NAME})
# jet test manager
-ADD_EXECUTABLE(test_ceres_manager test_ceres_manager.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_manager ${PROJECT_NAME})
+#ADD_EXECUTABLE(test_ceres_manager test_ceres_manager.cpp)
+#TARGET_LINK_LIBRARIES(test_ceres_manager ${PROJECT_NAME})
# Testing a full wolf tree avoiding template classes for NodeLinked derived classes
ADD_EXECUTABLE(test_ceres_odom_batch test_ceres_odom_batch.cpp)
@@ -39,8 +39,8 @@ TARGET_LINK_LIBRARIES(test_ceres_odom_iterative ${PROJECT_NAME})
#TARGET_LINK_LIBRARIES(test_ceres_manager_tree ${PROJECT_NAME})
# test ceres covariance
-ADD_EXECUTABLE(test_ceres_covariance test_ceres_covariance.cpp)
-TARGET_LINK_LIBRARIES(test_ceres_covariance ${PROJECT_NAME})
+#ADD_EXECUTABLE(test_ceres_covariance test_ceres_covariance.cpp)
+#TARGET_LINK_LIBRARIES(test_ceres_covariance ${PROJECT_NAME})
# IF (laser_scan_utils_FOUND)
# ADD_EXECUTABLE(test_capture_laser_2D test_capture_laser_2D.cpp)
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 12ad327fe..a6c34e50b 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -34,389 +34,384 @@
//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());
}
int main(int argc, char** argv)
{
- std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
-
- // USER INPUT ============================================================================================
- if (argc!=4 || atoi(argv[1])<1 || atoi(argv[1])>1100 || atoi(argv[2]) < 0 || atoi(argv[3]) < 0 || atoi(argv[3]) > 1)
- {
- std::cout << "Please call me with: [./test_ceres_manager NI PRINT ORIENTATION_MODE], where:" << std::endl;
- std::cout << " - NI is the number of iterations (0 < NI < 1100)" << std::endl;
- std::cout << " - WS is the window size (0 < WS)" << std::endl;
- std::cout << " - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl;
- std::cout << "EXIT due to bad user input" << std::endl << std::endl;
- return -1;
- }
-
- unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
- unsigned int window_size = (unsigned int) atoi(argv[2]);
- bool complex_angle = (bool) atoi(argv[3]);
-
- // INITIALIZATION ============================================================================================
- //init random generators
- 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_factor); //odometry noise
- std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise
-
- //init google log
- //google::InitGoogleLogging(argv[0]);
-
- // Ceres initialization
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- // ceres_options.minimizer_progress_to_stdout = false;
- // ceres_options.line_search_direction_type = ceres::LBFGS;
- // ceres_options.max_num_iterations = 100;
- ceres::Problem::Options problem_options;
- problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- CeresManager* ceres_manager = new CeresManager(problem_options);
- std::ofstream log_file, landmark_file; //output file
-
- // Faramotics stuff
- Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
- vector<Cpose3d> devicePoses;
- vector<float> scan1, scan2;
- string modelFileName;
-
- //model and initial view point
- //modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
+ std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
+
+ // USER INPUT ============================================================================================
+ if (argc != 4 || atoi(argv[1]) < 1 || atoi(argv[1]) > 1100 || atoi(argv[2]) < 0 || atoi(argv[3]) < 0 || atoi(argv[3]) > 1)
+ {
+ std::cout << "Please call me with: [./test_ceres_manager NI PRINT ORIENTATION_MODE], where:" << std::endl;
+ std::cout << " - NI is the number of iterations (0 < NI < 1100)" << std::endl;
+ std::cout << " - WS is the window size (0 < WS)" << std::endl;
+ std::cout << " - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl;
+ std::cout << "EXIT due to bad user input" << std::endl << std::endl;
+ return -1;
+ }
+
+ unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
+ unsigned int window_size = (unsigned int) atoi(argv[2]);
+ bool complex_angle = (bool) atoi(argv[3]);
+
+ // INITIALIZATION ============================================================================================
+ //init random generators
+ 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_factor); //odometry noise
+ std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise
+
+ //init google log
+ //google::InitGoogleLogging(argv[0]);
+
+ // Ceres initialization
+ ceres::Solver::Options ceres_options;
+ ceres_options.minimizer_type = ceres::LINE_SEARCH; //ceres::TRUST_REGION;
+ ceres_options.max_line_search_step_contraction = 1e-3;
+ // ceres_options.minimizer_progress_to_stdout = false;
+ // ceres_options.line_search_direction_type = ceres::LBFGS;
+ // ceres_options.max_num_iterations = 100;
+ ceres::Problem::Options problem_options;
+ problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ CeresManager* ceres_manager = new CeresManager(problem_options);
+ std::ofstream log_file, landmark_file; //output file
+
+ // Faramotics stuff
+ Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
+ vector < Cpose3d > devicePoses;
+ vector<float> scan1, scan2;
+ string modelFileName;
+
+ //model and initial view point
+ 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";
- devicePose.setPose(2,8,0.2,0,0,0);
- viewPoint.setPose(devicePose);
- viewPoint.moveForward(10);
- viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/2, viewPoint.rt.pitch()+30.*M_PI/180., viewPoint.rt.roll() );
- viewPoint.moveForward(-15);
- //glut initialization
+ //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
+ devicePose.setPose(2, 8, 0.2, 0, 0, 0);
+ viewPoint.setPose(devicePose);
+ viewPoint.moveForward(10);
+ viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
+ viewPoint.moveForward(-15);
+ //glut initialization
faramotics::initGLUT(argc, argv);
-
- //create a viewer for the 3D model and scan points
- CdynamicSceneRender* myRender = new CdynamicSceneRender(1200,700,90*M_PI/180,90*700.0*M_PI/(1200.0*180.0),0.2,100);
- myRender->loadAssimpModel(modelFileName,true); //with wireframe
- //create scanner and load 3D model
- CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG);//HOKUYO_UTM30LX_180DEG or LEUZE_RS4
- myScanner->loadAssimpModel(modelFileName);
-
- //variables
- 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
- Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
- clock_t t1, t2;
-
- // Wolf manager initialization
- 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
- laser_2_pose << -1.2,0,0,0,0,M_PI; //laser 2
- SensorLaser2D laser_1_sensor(laser_1_pose);
- SensorLaser2D laser_2_sensor(laser_2_pose);
-
- // Initial pose
- pose_odom << 2,8,0;
- ground_truth.head(3) = pose_odom;
- odom_trajectory.head(3) = pose_odom;
-
- WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e6, pose_odom, 0.3, window_size);
-
- //std::cout << "START TRAJECTORY..." << std::endl;
- // START TRAJECTORY ============================================================================================
- for (uint step=1; step < n_execution; step++)
- {
- //get init time
- t2=clock();
-
- // ROBOT MOVEMENT ---------------------------
- //std::cout << "ROBOT MOVEMENT..." << std::endl;
- // moves the device position
- t1=clock();
- motionCampus(step, devicePose, odom_reading(0), odom_reading(2));
- odom_reading(1) = 0;
- devicePoses.push_back(devicePose);
-
-
- // SENSOR DATA ---------------------------
- //std::cout << "SENSOR DATA..." << std::endl;
- // store groundtruth
- 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)*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)) - 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;
-
- // compute GPS
- gps_fix_reading << devicePose.pt(0), devicePose.pt(1);
- gps_fix_reading(0) += distribution_gps(generator);
- gps_fix_reading(1) += distribution_gps(generator);
-
- //compute scans
- scan1.clear();
- scan2.clear();
- // scan 1
- laser1Pose.setPose(devicePose);
- laser1Pose.moveForward(laser_1_pose(0));
- myScanner->computeScan(laser1Pose,scan1);
- // scan 2
- laser2Pose.setPose(devicePose);
- laser2Pose.moveForward(laser_2_pose(0));
- laser2Pose.rt.setEuler( laser2Pose.rt.head()+M_PI, laser2Pose.rt.pitch(), laser2Pose.rt.roll() );
- myScanner->computeScan(laser2Pose,scan2);
-
- mean_times(0) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
-
- // ADD CAPTURES ---------------------------
- //std::cout << "ADD CAPTURES..." << std::endl;
- t1=clock();
- // adding new sensor captures
- wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading));//, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
+
+ //create a viewer for the 3D model and scan points
+ CdynamicSceneRender* myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
+ myRender->loadAssimpModel(modelFileName, true); //with wireframe
+ //create scanner and load 3D model
+ CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
+ myScanner->loadAssimpModel(modelFileName);
+
+ //variables
+ 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
+ Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
+ clock_t t1, t2;
+
+ // Wolf manager initialization
+ Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
+ Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
+ Eigen::Vector3s laser_1_pose, laser_2_pose;
+ laser_1_pose << 1.2, 0, 0; //laser 1
+ laser_2_pose << -1.2, 0, M_PI; //laser 2
+ SensorOdom2D odom_sensor(new StatePoint2D(odom_pose.data()), new StateTheta(&odom_pose(2)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(new StatePoint2D(gps_pose.data()), new StateTheta(&gps_pose(2)), gps_std);
+ SensorLaser2D laser_1_sensor(new StatePoint2D(laser_1_pose.data()), new StateTheta(&laser_1_pose(2)));
+ SensorLaser2D laser_2_sensor(new StatePoint2D(laser_2_pose.data()), new StateTheta(&laser_2_pose(2)));
+
+ // Initial pose
+ pose_odom << 2, 8, 0;
+ ground_truth.head(3) = pose_odom;
+ odom_trajectory.head(3) = pose_odom;
+
+ WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e3, pose_odom, Eigen::Matrix3s::Identity() * 0.01, 0.3, window_size);
+
+ //std::cout << "START TRAJECTORY..." << std::endl;
+ // START TRAJECTORY ============================================================================================
+ for (uint step = 1; step < n_execution; step++)
+ {
+ //get init time
+ t2 = clock();
+
+ // ROBOT MOVEMENT ---------------------------
+ //std::cout << "ROBOT MOVEMENT..." << std::endl;
+ // moves the device position
+ t1 = clock();
+ motionCampus(step, devicePose, odom_reading(0), odom_reading(2));
+ odom_reading(1) = 0;
+ devicePoses.push_back(devicePose);
+
+ // SENSOR DATA ---------------------------
+ //std::cout << "SENSOR DATA..." << std::endl;
+ // store groundtruth
+ 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) * 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)) - 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;
+
+ // compute GPS
+ gps_fix_reading << devicePose.pt(0), devicePose.pt(1);
+ gps_fix_reading(0) += distribution_gps(generator);
+ gps_fix_reading(1) += distribution_gps(generator);
+
+ //compute scans
+ scan1.clear();
+ scan2.clear();
+ // scan 1
+ laser1Pose.setPose(devicePose);
+ laser1Pose.moveForward(laser_1_pose(0));
+ myScanner->computeScan(laser1Pose, scan1);
+ // scan 2
+ laser2Pose.setPose(devicePose);
+ laser2Pose.moveForward(laser_2_pose(0));
+ laser2Pose.rt.setEuler(laser2Pose.rt.head() + M_PI, laser2Pose.rt.pitch(), laser2Pose.rt.roll());
+ myScanner->computeScan(laser2Pose, scan2);
+
+ mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ // ADD CAPTURES ---------------------------
+ //std::cout << "ADD CAPTURES..." << std::endl;
+ t1 = clock();
+ // adding new sensor captures
+ 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));
- wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
+ wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
+ wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
// updating problem
- wolf_manager->update();
- mean_times(1) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
-
- // UPDATING CERES ---------------------------
- //std::cout << "UPDATING CERES..." << std::endl;
- t1=clock();
- // update state units and constraints in ceres
- ceres_manager->update(wolf_manager->getProblemPtr());
- mean_times(2) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
-
- // SOLVE OPTIMIZATION ---------------------------
- //std::cout << "SOLVING..." << std::endl;
- t1=clock();
- ceres::Solver::Summary summary = ceres_manager->solve(ceres_options);
- //std::cout << summary.FullReport() << std::endl;
- mean_times(3) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
- // COMPUTE COVARIANCES ---------------------------
- //std::cout << "COMPUTING COVARIANCES..." << std::endl;
- t1=clock();
- //ceres_manager->computeCovariances(wolf_manager->getProblemPtr());
- mean_times(4) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
- // DRAWING STUFF ---------------------------
- t1=clock();
- // draw detected corners
- std::list<laserscanutils::Corner> corner_list;
- std::vector<double> corner_vector;
- CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
- last_scan.extractCorners(corner_list);
- for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ )
- {
- corner_vector.push_back(corner_it->pt_(0));
- corner_vector.push_back(corner_it->pt_(1));
- }
- myRender->drawCorners(laser1Pose,corner_vector);
-
- // draw landmarks
- std::vector<double> landmark_vector;
- for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ )
- {
- WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
- landmark_vector.push_back(*position_ptr); //x
- landmark_vector.push_back(*(position_ptr+1)); //y
- landmark_vector.push_back(0.2); //z
- }
- myRender->drawLandmarks(landmark_vector);
-
- // draw localization and sensors
- estimated_vehicle_pose.setPose(wolf_manager->getVehiclePose()(0),wolf_manager->getVehiclePose()(1),0.2,wolf_manager->getVehiclePose()(2),0,0);
- estimated_laser_1_pose.setPose(estimated_vehicle_pose);
- estimated_laser_1_pose.moveForward(laser_1_pose(0));
- estimated_laser_2_pose.setPose(estimated_vehicle_pose);
- estimated_laser_2_pose.moveForward(laser_2_pose(0));
- estimated_laser_2_pose.rt.setEuler( estimated_laser_2_pose.rt.head()+M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll() );
- myRender->drawPoseAxisVector({estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose});
-
- //Set view point and render the scene
- //locate visualization view point, somewhere behind the device
+ wolf_manager->update();
+ mean_times(1) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ // UPDATING CERES ---------------------------
+ //std::cout << "UPDATING CERES..." << std::endl;
+ t1 = clock();
+ // update state units and constraints in ceres
+ ceres_manager->update(wolf_manager->getProblemPtr());
+ mean_times(2) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ // SOLVE OPTIMIZATION ---------------------------
+ //std::cout << "SOLVING..." << std::endl;
+ t1 = clock();
+ ceres::Solver::Summary summary = ceres_manager->solve(ceres_options);
+ //std::cout << summary.FullReport() << std::endl;
+ mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ // COMPUTE COVARIANCES ---------------------------
+ //std::cout << "COMPUTING COVARIANCES..." << std::endl;
+ t1 = clock();
+ ceres_manager->computeCovariances(wolf_manager->getProblemPtr());
+ mean_times(4) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ // DRAWING STUFF ---------------------------
+ t1 = clock();
+ // draw detected corners
+ std::list < laserscanutils::Corner > corner_list;
+ std::vector<double> corner_vector;
+ CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
+ last_scan.extractCorners(corner_list);
+ for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
+ {
+ corner_vector.push_back(corner_it->pt_(0));
+ corner_vector.push_back(corner_it->pt_(1));
+ }
+ myRender->drawCorners(laser1Pose, corner_vector);
+
+ // draw landmarks
+ std::vector<double> landmark_vector;
+ for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
+ {
+ WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+ landmark_vector.push_back(*position_ptr); //x
+ landmark_vector.push_back(*(position_ptr + 1)); //y
+ landmark_vector.push_back(0.2); //z
+ }
+ myRender->drawLandmarks(landmark_vector);
+
+ // draw localization and sensors
+ estimated_vehicle_pose.setPose(wolf_manager->getVehiclePose()(0), wolf_manager->getVehiclePose()(1), 0.2, wolf_manager->getVehiclePose()(2), 0, 0);
+ estimated_laser_1_pose.setPose(estimated_vehicle_pose);
+ estimated_laser_1_pose.moveForward(laser_1_pose(0));
+ estimated_laser_2_pose.setPose(estimated_vehicle_pose);
+ estimated_laser_2_pose.moveForward(laser_2_pose(0));
+ estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
+ myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
+
+ //Set view point and render the scene
+ //locate visualization view point, somewhere behind the device
// viewPoint.setPose(devicePose);
// viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
// viewPoint.moveForward(-5);
- myRender->setViewPoint(viewPoint);
- myRender->drawPoseAxis(devicePose);
- myRender->drawScan(laser1Pose,scan1,180.*M_PI/180.,90.*M_PI/180.); //draw scan
- myRender->render();
- mean_times(5) += ((double)clock()-t1)/CLOCKS_PER_SEC;
-
- // TIME MANAGEMENT ---------------------------
- double dt = ((double)clock()-t2)/CLOCKS_PER_SEC;
- mean_times(6) += dt;
- if (dt < 0.1)
- usleep(100000-1e6*dt);
+ myRender->setViewPoint(viewPoint);
+ myRender->drawPoseAxis(devicePose);
+ myRender->drawScan(laser1Pose, scan1, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
+ myRender->render();
+ mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ // TIME MANAGEMENT ---------------------------
+ double dt = ((double) clock() - t2) / CLOCKS_PER_SEC;
+ mean_times(6) += dt;
+ if (dt < 0.1)
+ usleep(100000 - 1e6 * dt);
// std::cout << "\nTree after step..." << std::endl;
// wolf_manager->getProblemPtr()->print();
- }
-
- // DISPLAY RESULTS ============================================================================================
- mean_times /= n_execution;
- std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
- std::cout << " data generation: " << mean_times(0) << std::endl;
- std::cout << " wolf managing: " << mean_times(1) << std::endl;
- std::cout << " ceres managing: " << mean_times(2) << std::endl;
- std::cout << " ceres optimization: " << mean_times(3) << std::endl;
- std::cout << " ceres covariance: " << mean_times(4) << std::endl;
- std::cout << " results drawing: " << mean_times(5) << std::endl;
- std::cout << " loop time: " << mean_times(6) << std::endl;
+ }
+
+ // DISPLAY RESULTS ============================================================================================
+ mean_times /= n_execution;
+ std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
+ std::cout << " data generation: " << mean_times(0) << std::endl;
+ std::cout << " wolf managing: " << mean_times(1) << std::endl;
+ std::cout << " ceres managing: " << mean_times(2) << std::endl;
+ std::cout << " ceres optimization: " << mean_times(3) << std::endl;
+ std::cout << " ceres covariance: " << mean_times(4) << std::endl;
+ std::cout << " results drawing: " << mean_times(5) << std::endl;
+ std::cout << " loop time: " << mean_times(6) << std::endl;
// std::cout << "\nTree before deleting..." << std::endl;
// wolf_manager->getProblemPtr()->print();
- // Draw Final result -------------------------
- std::vector<double> landmark_vector;
- for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ )
- {
- WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
- landmark_vector.push_back(*position_ptr); //x
- landmark_vector.push_back(*(position_ptr+1)); //y
- landmark_vector.push_back(0.2); //z
- }
- myRender->drawLandmarks(landmark_vector);
+ // Draw Final result -------------------------
+ std::vector<double> landmark_vector;
+ for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
+ {
+ WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+ landmark_vector.push_back(*position_ptr); //x
+ landmark_vector.push_back(*(position_ptr + 1)); //y
+ landmark_vector.push_back(0.2); //z
+ }
+ myRender->drawLandmarks(landmark_vector);
// viewPoint.setPose(devicePoses.front());
// viewPoint.moveForward(10);
// viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
// viewPoint.moveForward(-10);
- myRender->setViewPoint(viewPoint);
- myRender->render();
-
- // Print Final result in a file -------------------------
- // Vehicle poses
- int i = 0;
- Eigen::VectorXs state_poses(n_execution * 3);
- for (auto frame_it = wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->begin(); frame_it != wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->end(); frame_it++ )
- {
- if (complex_angle)
- state_poses.segment(i,3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr()+1), atan2(*(*frame_it)->getOPtr()->getPtr(), *((*frame_it)->getOPtr()->getPtr()+1));
- else
- state_poses.segment(i,3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr()+1), *(*frame_it)->getOPtr()->getPtr();
- i+=3;
- }
-
- // Landmarks
- i = 0;
- Eigen::VectorXs landmarks(wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->size()*2);
- for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ )
- {
- Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getPPtr()->getPtr());
- landmarks.segment(i,2) = landmark;
- i+=2;
- }
-
- // Print log files
- std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt"));
- log_file.open(filepath, std::ofstream::out); //open log file
-
- if (log_file.is_open())
- {
- log_file << 0 << std::endl;
- for (unsigned int ii = 0; ii<n_execution; ii++)
- log_file << state_poses.segment(ii*3,3).transpose()
- << "\t" << ground_truth.segment(ii*3,3).transpose()
- << "\t" << (state_poses.segment(ii*3,3)-ground_truth.segment(ii*3,3)).transpose()
- << "\t" << odom_trajectory.segment(ii*3,3).transpose() << std::endl;
- log_file.close(); //close log file
- std::cout << std::endl << "Result file " << filepath << std::endl;
- }
- else
- std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
-
- std::string filepath2 = getenv("HOME") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt") : std::string("/Desktop/landmarks_file_2.txt"));
- landmark_file.open(filepath2, std::ofstream::out); //open log file
-
- if (landmark_file.is_open())
- {
- for (unsigned int ii = 0; ii<landmarks.size(); ii+=2)
- landmark_file << landmarks.segment(ii,2).transpose() << std::endl;
- landmark_file.close(); //close log file
- std::cout << std::endl << "Landmark file " << filepath << std::endl;
- }
- else
- std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl;
-
- std::cout << "Press any key for ending... " << std::endl << std::endl;
- std::getchar();
+ myRender->setViewPoint(viewPoint);
+ myRender->render();
+
+ // Print Final result in a file -------------------------
+ // Vehicle poses
+ int i = 0;
+ Eigen::VectorXs state_poses(n_execution * 3);
+ for (auto frame_it = wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->begin(); frame_it != wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->end(); frame_it++)
+ {
+ if (complex_angle)
+ state_poses.segment(i, 3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr() + 1), atan2(*(*frame_it)->getOPtr()->getPtr(), *((*frame_it)->getOPtr()->getPtr() + 1));
+ else
+ state_poses.segment(i, 3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr() + 1), *(*frame_it)->getOPtr()->getPtr();
+ i += 3;
+ }
+
+ // Landmarks
+ i = 0;
+ Eigen::VectorXs landmarks(wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->size() * 2);
+ for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
+ {
+ Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getPPtr()->getPtr());
+ landmarks.segment(i, 2) = landmark;
+ i += 2;
+ }
+
+ // Print log files
+ std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt"));
+ log_file.open(filepath, std::ofstream::out); //open log file
+
+ if (log_file.is_open())
+ {
+ log_file << 0 << std::endl;
+ for (unsigned int ii = 0; ii < n_execution; ii++)
+ log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" << ground_truth.segment(ii * 3, 3).transpose() << "\t" << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl;
+ log_file.close(); //close log file
+ std::cout << std::endl << "Result file " << filepath << std::endl;
+ }
+ else
+ std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
+
+ std::string filepath2 = getenv("HOME") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt") : std::string("/Desktop/landmarks_file_2.txt"));
+ landmark_file.open(filepath2, std::ofstream::out); //open log file
+
+ if (landmark_file.is_open())
+ {
+ for (unsigned int ii = 0; ii < landmarks.size(); ii += 2)
+ landmark_file << landmarks.segment(ii, 2).transpose() << std::endl;
+ landmark_file.close(); //close log file
+ std::cout << std::endl << "Landmark file " << filepath << std::endl;
+ }
+ else
+ std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl;
+
+ std::cout << "Press any key for ending... " << std::endl << std::endl;
+ std::getchar();
delete myRender;
delete myScanner;
- delete wolf_manager;
- std::cout << "wolf deleted" << std::endl;
- delete ceres_manager;
- std::cout << "ceres_manager deleted" << std::endl;
+ delete wolf_manager;
+ std::cout << "wolf deleted" << std::endl;
+ delete ceres_manager;
+ std::cout << "ceres_manager deleted" << std::endl;
- std::cout << " ========= END ===========" << std::endl << std::endl;
+ std::cout << " ========= END ===========" << std::endl << std::endl;
- //exit
- return 0;
+ //exit
+ return 0;
}
diff --git a/src/frame_base.h b/src/frame_base.h
index c52a5d9e6..7cd5ef3f3 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -45,7 +45,7 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
* \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
*
**/
- FrameBase(const TimeStamp& _ts, StateBase* _p_ptr = nullptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
+ FrameBase(const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
/** \brief Constructor with type, time stamp and state pointer
*
@@ -59,7 +59,7 @@ class FrameBase : public NodeLinked<TrajectoryBase,CaptureBase>
* \param _w_ptr StateBase pointer to the angular velocity (default: nullptr)
*
**/
- FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_ptr = nullptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
+ FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBase* _p_ptr, StateBase* _o_ptr = nullptr, StateBase* _v_ptr = nullptr, StateBase* _w_ptr = nullptr);
/** \brief Destructor
*
diff --git a/src/landmark_base.h b/src/landmark_base.h
index 33d678bb8..3b7d4d15d 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -1,4 +1,3 @@
-
#ifndef LANDMARK_BASE_H_
#define LANDMARK_BASE_H_
@@ -25,76 +24,75 @@ class NodeTerminus;
//
//class LandmarkBase
-class LandmarkBase : public NodeLinked<MapBase,NodeTerminus>
+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;
-
- StateBase* getOPtr() const;
-
- StateBase* getVPtr() const;
-
- StateBase* getWPtr() const;
-
- void setDescriptor(const Eigen::VectorXs& _descriptor);
-
- const Eigen::VectorXs& getDescriptor() const;
-
- //const StateBase* getStatePtr() const;
-
- //const StateBaseList* getStateListPtr() 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;
+
+ StateBase* getVPtr() const;
+
+ StateBase* getWPtr() const;
+
+ void setDescriptor(const Eigen::VectorXs& _descriptor);
+
+ const Eigen::VectorXs& getDescriptor() const;
+
+ //const StateBase* getStatePtr() const;
+
+ //const StateBaseList* getStateListPtr() const;
};
#endif
diff --git a/src/node_linked.h b/src/node_linked.h
index fb07972db..776e3c018 100644
--- a/src/node_linked.h
+++ b/src/node_linked.h
@@ -172,7 +172,6 @@ class NodeLinked : public NodeBase
* TODO: Review if it could return a pointer to a derived class instead of NodeBase JVN: I tried to do so...
*
**/
- //virtual NodeBase* getTop();
virtual WolfProblem* getTop();
/** \brief Prints node information
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index fa72cfed0..221db8e06 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -1,57 +1,41 @@
#include "sensor_base.h"
-SensorBase::SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, const Eigen::VectorXs & _params) :
- NodeBase("SENSOR"),
- type_(_tp),
- sensor_position_vehicle_(_pose.head(3)),
- params_(_params.size())
+SensorBase::SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateOrientation* _o_ptr, const Eigen::VectorXs & _params) :
+ NodeBase("SENSOR"),
+ type_(_tp),
+ p_ptr_(_p_ptr),
+ o_ptr_(_o_ptr),
+ params_(_params.size())
{
- 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());
+ params_ = _params;
}
-SensorBase::SensorBase(const SensorType & _tp, const Eigen::Vector6s & _pose, unsigned int _params_size) :
- NodeBase("SENSOR"),
- type_(_tp),
- sensor_position_vehicle_(_pose.head(3)),
- params_(_params_size)
+SensorBase::SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateOrientation* _o_ptr, unsigned int _params_size) :
+ NodeBase("SENSOR"),
+ type_(_tp),
+ p_ptr_(_p_ptr),
+ o_ptr_(_o_ptr),
+ 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());
+ //
}
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
+StateBase* SensorBase::getPPtr() const
{
- //std::cout << "getSensorPosition: " << sensor_position_vehicle_.transpose() << std::endl;
- return & sensor_position_vehicle_;
+ return p_ptr_;
}
-const Eigen::Matrix3s * SensorBase::getSensorRotation() const
-{
- //std::cout << "getSensorRotation: " << sensor_rotation_vehicle_ << std::endl;
- return & sensor_rotation_vehicle_;
-}
-
-void SensorBase::setSensorPose(const Eigen::Vector6s & _pose)
+StateOrientation* SensorBase::getOPtr() const
{
- 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());
+ return o_ptr_;
}
-
-
-
diff --git a/src/sensor_base.h b/src/sensor_base.h
index e7b1f55c7..0524f1d1c 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -12,30 +12,42 @@
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:
-// 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
-
- 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;
-
- void setSensorPose(const Eigen::Vector6s & _pose);
+ protected:
+ SensorType type_; //indicates sensor type. Enum defined at wolf.h
+ 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, ...
+
+ public:
+ /** \brief Constructor with parameter vector
+ *
+ * Constructor with parameter vector
+ * \param _tp Type of the sensor (types defined at wolf.h)
+ * \param _p_ptr StateBase pointer to the sensor position
+ * \param _o_ptr StateOrientation pointer to the sensor orientation
+ * \param _params Vector containing the sensor parameters
+ *
+ **/
+ SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateOrientation* _o_ptr, const Eigen::VectorXs & _params);
+
+ /** \brief Constructor with parameter size
+ *
+ * Constructor with parameter vector
+ * \param _tp Type of the sensor (types defined at wolf.h)
+ * \param _p_ptr StateBase pointer to the sensor position
+ * \param _o_ptr StateOrientation pointer to the sensor orientation
+ * \param _params_size size of the vector containing the sensor parameters
+ *
+ **/
+ SensorBase(const SensorType & _tp, StateBase* _p_ptr, StateOrientation* _o_ptr, unsigned int _params_size);
+
+ ~SensorBase();
+
+ const SensorType getSensorType() const;
+
+ StateBase* getPPtr() const;
+
+ StateOrientation* getOPtr() const;
};
#endif
diff --git a/src/sensor_gps_fix.cpp b/src/sensor_gps_fix.cpp
index f9a52cd49..b798dbb92 100644
--- a/src/sensor_gps_fix.cpp
+++ b/src/sensor_gps_fix.cpp
@@ -1,7 +1,7 @@
#include "sensor_gps_fix.h"
-SensorGPSFix::SensorGPSFix(const Eigen::Vector6s & _sp, const double& _noise) :
- SensorBase(GPS_FIX, _sp, Eigen::VectorXs::Constant(1,_noise))
+SensorGPSFix::SensorGPSFix(StateBase* _p_ptr, StateOrientation* _o_ptr, const double& _noise) :
+ SensorBase(GPS_FIX, _p_ptr, _o_ptr, Eigen::VectorXs::Constant(1,_noise))
{
//
}
diff --git a/src/sensor_gps_fix.h b/src/sensor_gps_fix.h
index d7fe44724..3b918a0e6 100644
--- a/src/sensor_gps_fix.h
+++ b/src/sensor_gps_fix.h
@@ -11,11 +11,12 @@ class SensorGPSFix : public SensorBase
/** \brief Constructor with arguments
*
* Constructor with arguments
- * \param _sp sensor 3D pose with respect to vehicle base frame
+ * \param _p_ptr StateBase pointer to the sensor position
+ * \param _o_ptr StateOrientation pointer to the sensor orientation
* \param _noise noise standard deviation
*
**/
- SensorGPSFix(const Eigen::Vector6s & _sp, const double& _noise);
+ SensorGPSFix(StateBase* _p_ptr, StateOrientation* _o_ptr, const double& _noise);
/** \brief Destructor
*
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index f6f6c63d2..bb6ff38f3 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -7,8 +7,8 @@
// params_ << _angle_min, _angle_max, _angle_increment, _range_min, _range_max, _range_stdev, _time_increment, _scan_time;
// }
-SensorLaser2D::SensorLaser2D(const Eigen::Vector6s & _sp) :
- SensorBase(LIDAR, _sp, 8)
+SensorLaser2D::SensorLaser2D(StateBase* _p_ptr, StateOrientation* _o_ptr) :
+ SensorBase(LIDAR, _p_ptr, _o_ptr, 8)
{
setDefaultScanParams();
setDefaultCornerAlgParams();
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index 83743a361..cae219d61 100644
--- a/src/sensor_laser_2D.h
+++ b/src/sensor_laser_2D.h
@@ -24,27 +24,12 @@ class SensorLaser2D : public SensorBase
/** \brief Constructor with arguments
*
* Constructor with arguments
- * \param _sp sensor 3D pose with respect to vehicle base frame
- * \param _angle_min start angle of the scan [rad]
- * \param _angle_max end angle of the scan [rad]
- * \param _angle_increment angular distance between measurements [rad]
- * \param _range_min minimum range value [m]
- * \param _range_max maximum range value [m]
- * \param _range_stdev range standard deviation [m]
- * \param _time_increment time between beams [seconds]
- * \param _scan_time time between scans [seconds]
- *
- **/
- //SensorLaser2D(const Eigen::VectorXs & _sp, WolfScalar _angle_min, WolfScalar _angle_max, WolfScalar _angle_increment, WolfScalar _range_min, WolfScalar _range_max, WolfScalar _range_stdev, WolfScalar _time_increment=0, WolfScalar _scan_time=0);
-
- /** \brief Constructor with arguments
- *
- * Constructor with arguments
- * \param _sp sensor 3D pose with respect to vehicle base frame
+ * \param _p_ptr StateBase pointer to the sensor position
+ * \param _o_ptr StateOrientation pointer to the sensor orientation
* \param _params struct with scan parameters. See laser_scan_utils library API for reference
*
**/
- SensorLaser2D(const Eigen::Vector6s & _sp);
+ SensorLaser2D(StateBase* _p_ptr, StateOrientation* _o_ptr);
//SensorLaser2D(const Eigen::VectorXs & _sp, const laserscanutils::ScanParams & _params);
/** \brief Destructor
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index c39546461..309ac30a7 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -1,7 +1,7 @@
#include "sensor_odom_2D.h"
-SensorOdom2D::SensorOdom2D(const Eigen::Vector6s & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) :
- SensorBase(ODOM_2D, _sp, 2)
+SensorOdom2D::SensorOdom2D(StateBase* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) :
+ SensorBase(ODOM_2D, _p_ptr, _o_ptr, 2)
{
params_ << _disp_noise_factor, _rot_noise_factor;
}
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
index 84491a928..4e37a2211 100644
--- a/src/sensor_odom_2D.h
+++ b/src/sensor_odom_2D.h
@@ -11,12 +11,13 @@ class SensorOdom2D : public SensorBase
/** \brief Constructor with arguments
*
* Constructor with arguments
- * \param _sp sensor 3D pose with respect to vehicle base frame
+ * \param _p_ptr StateBase pointer to the sensor position
+ * \param _o_ptr StateOrientation pointer to the sensor orientation
* \param _disp_noise_factor displacement noise factor
* \param _rot_noise_factor rotation noise factor
*
**/
- SensorOdom2D(const Eigen::Vector6s & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor);
+ SensorOdom2D(StateBase* _p_ptr, StateOrientation* _o_ptr, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor);
/** \brief Destructor
*
diff --git a/src/state_base.h b/src/state_base.h
index 84477fcd2..a31dd401f 100644
--- a/src/state_base.h
+++ b/src/state_base.h
@@ -50,6 +50,13 @@ class StateBase : public NodeBase
**/
virtual WolfScalar* getPtr();
+ /** \brief Returns a (mapped) vector of the state unit
+ *
+ * Returns a (mapped) vector of the state unit
+ *
+ **/
+ virtual Eigen::Map<const Eigen::VectorXs> getVector() const = 0;
+
/** \brief Set the pointer of the first element of the state
*
* Set the pointer of the first element of the state
diff --git a/src/state_complex_angle.cpp b/src/state_complex_angle.cpp
index 05a8853c7..b2211a82f 100644
--- a/src/state_complex_angle.cpp
+++ b/src/state_complex_angle.cpp
@@ -39,6 +39,11 @@ Eigen::Matrix3s StateComplexAngle::getRotationMatrix() const
return R;
}
+Eigen::Map<const Eigen::VectorXs> StateComplexAngle::getVector() const
+{
+ return Eigen::Map<const Eigen::VectorXs>(state_ptr_, BLOCK_SIZE);
+}
+
void StateComplexAngle::print(unsigned int _ntabs, std::ostream& _ost) const
{
printTabs(_ntabs);
diff --git a/src/state_complex_angle.h b/src/state_complex_angle.h
index d4aab33c5..ee7fca41c 100644
--- a/src/state_complex_angle.h
+++ b/src/state_complex_angle.h
@@ -15,7 +15,7 @@
class StateComplexAngle : public StateOrientation
{
public:
- static const unsigned int BLOCK_SIZE = 2;
+ static const unsigned int BLOCK_SIZE = 2;
/** \brief Constructor with whole state storage and index where starts the state unit
*
@@ -62,6 +62,13 @@ class StateComplexAngle : public StateOrientation
**/
virtual Eigen::Matrix3s getRotationMatrix() const;
+ /** \brief Returns a (mapped) vector of the state unit
+ *
+ * Returns a (mapped) vector of the state unit
+ *
+ **/
+ virtual Eigen::Map<const Eigen::VectorXs> getVector() const;
+
/** \brief Prints all the elements of the state unit
*
* Prints all the elements of the state unit
diff --git a/src/state_orientation.h b/src/state_orientation.h
index df5ee6427..7a854b86d 100644
--- a/src/state_orientation.h
+++ b/src/state_orientation.h
@@ -1,4 +1,3 @@
-
#ifndef STATE_ORIENTATION_H_
#define STATE_ORIENTATION_H_
@@ -14,37 +13,44 @@
//class StateOrientation
class StateOrientation : public StateBase
{
- public:
-
+ public:
+
/** \brief Constructor with whole state storage and index where starts the state unit
- *
+ *
* Constructor with whole state storage and index where starts the state unit
* \param _st_remote is the whole state vector
* \param _idx is the index of the first element of the state in the whole state vector
- *
+ *
**/
- StateOrientation(Eigen::VectorXs& _st_remote, const unsigned int _idx);
+ StateOrientation(Eigen::VectorXs& _st_remote, const unsigned int _idx);
/** \brief Constructor with scalar pointer
- *
+ *
* Constructor with scalar pointer
* \param _st_ptr is the pointer of the first element of the state unit
- *
+ *
**/
- StateOrientation(WolfScalar* _st_ptr);
-
+ StateOrientation(WolfScalar* _st_ptr);
+
/** \brief Destructor
- *
+ *
* Destructor
- *
+ *
**/
virtual ~StateOrientation();
/** \brief Returns the 3x3 rotation matrix of the orientation
- *
- * Returns the 3x3 rotation matrix of the orientation
- *
- **/
- virtual Eigen::Matrix3s getRotationMatrix() const = 0;
+ *
+ * Returns the 3x3 rotation matrix of the orientation
+ *
+ **/
+ virtual Eigen::Matrix3s getRotationMatrix() const = 0;
+
+ /** \brief Returns a (mapped) vector of the state unit
+ *
+ * Returns a (mapped) vector of the state unit
+ *
+ **/
+ virtual Eigen::Map<const Eigen::VectorXs> getVector() const = 0;
};
#endif
diff --git a/src/state_point.h b/src/state_point.h
index 7f515cc3f..19ce416b7 100644
--- a/src/state_point.h
+++ b/src/state_point.h
@@ -81,14 +81,14 @@ class StatePoint : public StateBase
return BLOCK_SIZE;
}
- /** \brief Returns the point in a vector
+ /** \brief Returns the point in a (mapped) vector
*
- * Returns the point in a vector
+ * Returns the point in a (mapped) vector
*
**/
- const Eigen::Matrix<WolfScalar, BLOCK_SIZE, 1>& getVector() const
+ virtual Eigen::Map<const Eigen::VectorXs> getVector() const
{
- return Eigen::Map<const Eigen::Matrix<WolfScalar, BLOCK_SIZE, 1>>(state_ptr_);
+ return Eigen::Map<const Eigen::VectorXs>(state_ptr_, BLOCK_SIZE);
}
/** \brief Prints all the elements of the state unit
diff --git a/src/state_theta.cpp b/src/state_theta.cpp
index b1e707b3e..ff2b851dc 100644
--- a/src/state_theta.cpp
+++ b/src/state_theta.cpp
@@ -1,50 +1,66 @@
-
#include "state_theta.h"
StateTheta::StateTheta(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
- StateOrientation(_st_remote, _idx)
+ StateOrientation(_st_remote, _idx)
{
- //
+ //
}
StateTheta::StateTheta(WolfScalar* _st_ptr) :
- StateOrientation(_st_ptr)
+ StateOrientation(_st_ptr)
{
- //
+ //
}
StateTheta::~StateTheta()
{
- //
+ //
}
StateType StateTheta::getStateType() const
{
- return ST_THETA;
+ return ST_THETA;
}
unsigned int StateTheta::getStateSize() const
{
- return BLOCK_SIZE;
+ return BLOCK_SIZE;
}
Eigen::Matrix3s StateTheta::getRotationMatrix() const
{
- Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
- R(0,0) = cos(*state_ptr_);
- R(1,1) = cos(*state_ptr_);
- R(0,1) = -sin(*state_ptr_);
- R(1,0) = sin(*state_ptr_);
+ Eigen::Matrix3s R(Eigen::Matrix3s::Identity());
+ R(0, 0) = cos(*state_ptr_);
+ R(1, 1) = cos(*state_ptr_);
+ R(0, 1) = -sin(*state_ptr_);
+ R(1, 0) = sin(*state_ptr_);
+
+ //std::cout << "StateTheta::getRotationMatrix()" << R << std::endl;
+ return R;
+}
+
+void StateTheta::getRotationMatrix(Eigen::Matrix3s& R) const
+{
+ R = Eigen::Matrix3s::Identity();
+ R(0, 0) = cos(*state_ptr_);
+ R(1, 1) = cos(*state_ptr_);
+ R(0, 1) = -sin(*state_ptr_);
+ R(1, 0) = sin(*state_ptr_);
+
+ //std::cout << "StateTheta::getRotationMatrix()" << R << std::endl;
+}
- return R;
+Eigen::Map<const Eigen::VectorXs> StateTheta::getVector() const
+{
+ return Eigen::Map<const Eigen::VectorXs>(state_ptr_, 1);
}
void StateTheta::print(unsigned int _ntabs, std::ostream& _ost) const
{
- printTabs(_ntabs);
- _ost << nodeLabel() << " " << nodeId() << std::endl;
- printTabs(_ntabs);
- for (uint i = 0; i < BLOCK_SIZE; i++)
- _ost << *(this->state_ptr_+i) << " ";
- _ost << std::endl;
+ printTabs(_ntabs);
+ _ost << nodeLabel() << " " << nodeId() << std::endl;
+ printTabs(_ntabs);
+ for (uint i = 0; i < BLOCK_SIZE; i++)
+ _ost << *(this->state_ptr_ + i) << " ";
+ _ost << std::endl;
}
diff --git a/src/state_theta.h b/src/state_theta.h
index 6c76502f2..5de9d1176 100644
--- a/src/state_theta.h
+++ b/src/state_theta.h
@@ -1,4 +1,3 @@
-
#ifndef STATE_THETA_H_
#define STATE_THETA_H_
@@ -14,59 +13,69 @@
//class StateTheta
class StateTheta : public StateOrientation
{
- public:
- static const unsigned int BLOCK_SIZE = 1;
-
+ public:
+ static const unsigned int BLOCK_SIZE = 1;
+
+ public:
+
/** \brief Constructor with whole state storage and index where starts the state unit
- *
+ *
* Constructor with whole state storage and index where starts the state unit
* \param _st_remote is the whole state vector
* \param _idx is the index of the first element of the state in the whole state vector
- *
+ *
**/
- StateTheta(Eigen::VectorXs& _st_remote, const unsigned int _idx);
+ StateTheta(Eigen::VectorXs& _st_remote, const unsigned int _idx);
/** \brief Constructor with scalar pointer
- *
+ *
* Constructor with scalar pointer
* \param _st_ptr is the pointer of the first element of the state unit
- *
+ *
**/
- StateTheta(WolfScalar* _st_ptr);
-
+ StateTheta(WolfScalar* _st_ptr);
+
/** \brief Destructor
- *
+ *
* Destructor
- *
+ *
**/
virtual ~StateTheta();
-
+
/** \brief Returns the parametrization of the state unit
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual StateType getStateType() const;
+ *
+ * Returns the parametrizationType (see wolf.h) of the state unit
+ *
+ **/
+ virtual StateType getStateType() const;
- /** \brief Returns the state unit size
- *
- * Returns the parametrizationType (see wolf.h) of the state unit
- *
- **/
- virtual unsigned int getStateSize() const;
+ /** \brief Returns the state unit size
+ *
+ * Returns the parametrizationType (see wolf.h) of the state unit
+ *
+ **/
+ virtual unsigned int getStateSize() const;
/** \brief Returns the 3x3 rotation matrix of the orientation
- *
- * Returns the 3x3 rotation matrix of the orientation
- *
- **/
- virtual Eigen::Matrix3s getRotationMatrix() const;
+ *
+ * Returns the 3x3 rotation matrix of the orientation
+ *
+ **/
+ virtual Eigen::Matrix3s getRotationMatrix() const;
+ void getRotationMatrix(Eigen::Matrix3s& R) const;
+
+ /** \brief Returns a (mapped) vector of the state unit
+ *
+ * Returns a (mapped) vector of the state unit
+ *
+ **/
+ virtual Eigen::Map<const Eigen::VectorXs> getVector() const;
/** \brief Prints all the elements of the state unit
- *
- * Prints all the elements of the state unit
- *
- **/
- virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
+ *
+ * Prints all the elements of the state unit
+ *
+ **/
+ virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const;
};
#endif
diff --git a/src/wolf.h b/src/wolf.h
index bcef332a0..7f207ddd2 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -21,6 +21,7 @@
//includes from Eigen lib
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
+#include <eigen3/Eigen/Sparse>
/**
* \brief scalar type for the Wolf project
@@ -84,7 +85,7 @@ typedef AngleAxis<WolfScalar> AngleAxiss; ///< Angle-Axis of rea
typedef enum
{
TOP, ///< root node location. This is the one that commands jobs down the tree.
- MID,///< middle nodes. These delegate jobs to lower nodes.
+ MID, ///< middle nodes. These delegate jobs to lower nodes.
BOTTOM ///< lowest level nodes. These are the ones that do not delegate any longer and have to do the job.
} NodeLocation;
@@ -108,13 +109,13 @@ typedef enum
* You may add items to this list as needed. Be concise with names, and document your entries.
*
*/
-typedef enum
+typedef enum
{
CTR_GPS_FIX_2D, ///< marks a 2D GPS Fix constraint.
- CTR_ODOM_2D_COMPLEX_ANGLE, ///< marks a 2D Odometry using complex angles.
- CTR_ODOM_2D_THETA, ///< marks a 2D Odometry using theta angles.
- CTR_CORNER_2D_THETA ///< marks a 2D Odometry using theta angles.
-
+ CTR_ODOM_2D_COMPLEX_ANGLE, ///< marks a 2D Odometry using complex angles.
+ CTR_ODOM_2D_THETA, ///< marks a 2D Odometry using theta angles.
+ CTR_CORNER_2D_THETA ///< marks a 2D Odometry using theta angles.
+
} ConstraintType;
/** \brief Enumeration of all possible state parametrizations
@@ -126,12 +127,12 @@ typedef enum
*/
typedef enum
{
- ST_POINT_1D, ///< A 1D point. No parametrization.
- ST_POINT_2D, ///< A 2D point. No parametrization.
- ST_POINT_3D, ///< A 3D point. No parametrization.
- ST_THETA, ///< A 2D orientation represented by a single angle. No parametrization.
- ST_COMPLEX_ANGLE, ///< A 2D orientation represented by a complex number.
- ST_QUATERNION ///< A 3D orientation represented by a quaternion.
+ ST_POINT_1D, ///< A 1D point. No parametrization.
+ ST_POINT_2D, ///< A 2D point. No parametrization.
+ ST_POINT_3D, ///< A 3D point. No parametrization.
+ ST_THETA, ///< A 2D orientation represented by a single angle. No parametrization.
+ ST_COMPLEX_ANGLE, ///< A 2D orientation represented by a complex number.
+ ST_QUATERNION ///< A 3D orientation represented by a quaternion.
} StateType;
/** \brief Enumeration of all possible state status
@@ -143,9 +144,9 @@ typedef enum
*/
typedef enum
{
- ST_ESTIMATED, ///< State in estimation (default)
- ST_FIXED, ///< State fixed, estimated enough or fixed infrastructure.
- ST_REMOVED ///< Removed state. TODO: is it useful?
+ ST_ESTIMATED, ///< State in estimation (default)
+ ST_FIXED, ///< State fixed, estimated enough or fixed infrastructure.
+ ST_REMOVED ///< Removed state. TODO: is it useful?
} StateStatus;
/** \brief Enumeration of all possible sensor types
@@ -157,13 +158,13 @@ typedef enum
*/
typedef enum
{
- ODOM_2D, ///< Odometry measurement from encoders: displacement and rotation.
- IMU, ///< Inertial measurement unit with 3 acceleros, 3 gyros
- CAMERA, ///< Regular pinhole camera
- GPS_FIX, ///< GPS fix calculated from a GPS receiver
- GPS_RAW, ///< GPS pseudo ranges, doppler and satellite ephemerides
- LIDAR, ///< Laser Range Finder, 2D
- RADAR, ///< Radar
+ ODOM_2D, ///< Odometry measurement from encoders: displacement and rotation.
+ IMU, ///< Inertial measurement unit with 3 acceleros, 3 gyros
+ CAMERA, ///< Regular pinhole camera
+ GPS_FIX, ///< GPS fix calculated from a GPS receiver
+ GPS_RAW, ///< GPS pseudo ranges, doppler and satellite ephemerides
+ LIDAR, ///< Laser Range Finder, 2D
+ RADAR, ///< Radar
ABSOLUTE_POSE ///< Full absolute pose (XYZ+quaternion)
} SensorType;
@@ -176,18 +177,18 @@ typedef enum
*/
typedef enum
{
- LANDMARK_POINT, ///< A point landmark, either 3D or 2D
- LANDMARK_CORNER, ///< A corner landmark (2D)
- LANDMARK_CONTAINER ///< A container landmark
+ LANDMARK_POINT, ///< A point landmark, either 3D or 2D
+ LANDMARK_CORNER, ///< A corner landmark (2D)
+ LANDMARK_CONTAINER ///< A container landmark
} LandmarkType;
typedef enum
{
- LANDMARK_CANDIDATE, ///< A landmark, just created. Association with it allowed, but not yet stablish an actual constraint for the solver
- LANDMARK_ESTIMATED, ///< A landmark being estimated. Association with it allowed, stablishing actual constraints for the solver where both vehicle and landmark states are being estimated
- LANDMARK_FIXED, ///< A landmark estimated. Association with it allowed, stablishing actual constraints for the solver, but its value remains static, no longer optimized
- LANDMARK_OUT_OF_VIEW, ///< A landmark out of the field of view. Association with it is not allowed, so does not pose constraints for the solver
- LANDMARK_OLD ///< An old landmark. Association with it not allowed, but old constraints can still be taken into account by the solver.
+ LANDMARK_CANDIDATE, ///< A landmark, just created. Association with it allowed, but not yet stablish an actual constraint for the solver
+ LANDMARK_ESTIMATED, ///< A landmark being estimated. Association with it allowed, stablishing actual constraints for the solver where both vehicle and landmark states are being estimated
+ LANDMARK_FIXED, ///< A landmark estimated. Association with it allowed, stablishing actual constraints for the solver, but its value remains static, no longer optimized
+ LANDMARK_OUT_OF_VIEW, ///< A landmark out of the field of view. Association with it is not allowed, so does not pose constraints for the solver
+ LANDMARK_OLD ///< An old landmark. Association with it not allowed, but old constraints can still be taken into account by the solver.
} LandmarkStatus;
/** \brief Pending status of a node
@@ -199,9 +200,9 @@ typedef enum
*/
typedef enum
{
- NOT_PENDING, ///< A point landmark, either 3D or 2D
- ADD_PENDING, ///< A corner landmark (2D)
- UPDATE_PENDING ///< A container landmark
+ NOT_PENDING, ///< A point landmark, either 3D or 2D
+ ADD_PENDING, ///< A corner landmark (2D)
+ UPDATE_PENDING ///< A container landmark
} PendingStatus;
/////////////////////////////////////////////////////////////////////////
@@ -210,7 +211,6 @@ typedef enum
// - forwards for pointers
//class VehicleBase;
-
class NodeTerminus;
class WolfProblem;
class MapBase;
@@ -230,10 +230,9 @@ class SensorBase;
class SensorLaser2D;
class TransSensor;
class StateBase;
-template <unsigned int SIZE> class StatePoint;
+template<unsigned int SIZE> class StatePoint;
class PinHole;
-
// - Vehicle
// typedef std::shared_ptr<VehicleBase> VehicleShPtr;
// typedef VehicleBase* VehiclePtr;
@@ -304,8 +303,6 @@ typedef StatePoint<3> StatePoint3D;
// - Pin hole
-
-
///** \brief Enumeration of all possible feature types
// *
// * Enumeration of all possible feature types.
@@ -326,5 +323,4 @@ typedef StatePoint<3> StatePoint3D;
// LIDAR_RAY ///< A single laser ray
//} FeatureType;
-
#endif /* WOLF_H_ */
diff --git a/src/wolf_manager.h b/src/wolf_manager.h
index 8e5e30f86..218c6969b 100644
--- a/src/wolf_manager.h
+++ b/src/wolf_manager.h
@@ -41,158 +41,171 @@
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_;
- CaptureRelative* second_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),
- last_capture_relative_(nullptr),
- second_last_capture_relative_(nullptr)
- {
- createFrame(_init_frame, TimeStamp(0));
- 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)
- 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)
- {
- new_captures_.push(_capture);
- //std::cout << "added new capture: " << _capture->nodeId() << "stamp: " << _capture->getTimeStamp().get() << 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_)
+ 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_;
+ CaptureRelative* second_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 Eigen::MatrixXs& _init_frame_cov, 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),
+ last_capture_relative_(nullptr),
+ second_last_capture_relative_(nullptr)
{
- // 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));
-
- // 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());
-
- // 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 second_last_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back();
-
- // 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 = 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;
- 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_++;
- }
+ assert( ((!_complex_angle && _init_frame.size() == 3 && _init_frame_cov.cols() == 3 && _init_frame_cov.rows() == 3) ||
+ (_complex_angle && _init_frame.size() == 4 && _init_frame_cov.cols() == 4 && _init_frame_cov.rows() == 4))
+ && "Wrong init_frame state vector or covariance matrix size");
+
+
+ // Set initial covariance with a fake ODOM 2D capture to a fix frame
+ createFrame(_init_frame, TimeStamp(0));
+ problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix();
+ last_capture_relative_->integrateCapture((CaptureRelative*)(new CaptureOdom2D(TimeStamp(0),
+ nullptr,
+ Eigen::Vector3s::Zero(),
+ _init_frame_cov)));
+ createFrame(_init_frame, TimeStamp(0));
+ second_last_capture_relative_->processCapture();
+ }
+
+ 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
- first_window_frame_ = problem_->getTrajectoryPtr()->getFrameListPtr()->begin();
- }
+ 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)
+ 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)
+ {
+ new_captures_.push(_capture);
+ //std::cout << "added new capture: " << _capture->nodeId() << "stamp: " << _capture->getTimeStamp().get() << std::endl;
}
- else
+
+ void update()
{
- // ADD CAPTURE TO THE LAST FRAME (or substitute the same sensor previous capture)
- //std::cout << "adding not odometry capture..." << std::endl;
- bool same_sensor_capture_found = false;
- 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())
+ while (!new_captures_.empty())
{
- //std::cout << "removing previous capture" << std::endl;
- //problem_->getTrajectoryPtr()->getFrameListPtr()->back()->removeCapture(capture_it);
- same_sensor_capture_found = true;
- //std::cout << "removed!" << std::endl;
- break;
+ // EXTRACT NEW CAPTURE
+ CaptureBase* new_capture = new_captures_.front();
+ new_captures_.pop();
+
+ // ODOMETRY SENSOR
+ if (new_capture->getSensorPtr() == sensor_prior_)
+ {
+ // UPDATE LAST STATE FROM SECOND LAST (optimized) TODO: see if it is necessary
+ 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));
+
+ // 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());
+
+ // 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 second_last_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back();
+
+ // 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 = 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;
+ 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 " << new_capture->nodeId() << std::endl;
+ bool same_sensor_capture_found = false;
+ 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);
+ same_sensor_capture_found = true;
+ //std::cout << "removed!" << std::endl;
+ break;
+ }
+ }
+ if (!same_sensor_capture_found)
+ problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new_capture);
+ }
}
- }
- if (!same_sensor_capture_found)
- problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new_capture);
}
- }
- }
-
- 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();
- }
-
- WolfProblem* getProblemPtr()
- {
- return problem_;
- }
-
- void setWindowSize(const unsigned int& _size)
- {
- window_size_ = _size;
- }
-
- void setNewFrameElapsedTime( const WolfScalar& _dt)
- {
- new_frame_elapsed_time_ = _dt;
- }
+
+ Eigen::VectorXs getVehiclePose()
+ {
+ if (second_last_capture_relative_ != nullptr)
+ problem_->getTrajectoryPtr()->getFrameListPtr()->back()->setState(second_last_capture_relative_->computePrior());
+ return last_capture_relative_->computePrior();
+ }
+
+ WolfProblem* getProblemPtr()
+ {
+ return problem_;
+ }
+
+ void setWindowSize(const unsigned int& _size)
+ {
+ window_size_ = _size;
+ }
+
+ void setNewFrameElapsedTime(const WolfScalar& _dt)
+ {
+ new_frame_elapsed_time_ = _dt;
+ }
};
diff --git a/src/wolf_problem.cpp b/src/wolf_problem.cpp
index c0fa4baac..4a8c0b3c9 100644
--- a/src/wolf_problem.cpp
+++ b/src/wolf_problem.cpp
@@ -2,7 +2,8 @@
WolfProblem::WolfProblem(unsigned int _size) :
NodeBase("WOLF_PROBLEM"), //
- state_(_size),
+ state_(_size),
+ covariance_(_size,_size),
state_idx_last_(0),
location_(TOP),
map_ptr_(new MapBase),
@@ -18,6 +19,7 @@ WolfProblem::WolfProblem(unsigned int _size) :
WolfProblem::WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr, unsigned int _size) :
NodeBase("WOLF_PROBLEM"), //
state_(_size),
+ covariance_(_size,_size),
state_idx_last_(0),
location_(TOP),
map_ptr_(_map_ptr==nullptr ? new MapBase : _map_ptr),
@@ -65,6 +67,66 @@ bool WolfProblem::addState(StateBase* _new_state_ptr, const Eigen::VectorXs& _ne
return reallocated_;
}
+void WolfProblem::addCovarianceBlock(StateBase* _state1, StateBase* _state2, const Eigen::MatrixXs& _cov)
+{
+ assert(_state1 != nullptr && _state1->getPtr() != nullptr && _state1->getPtr() < state_.data() + state_idx_last_ && _state1->getPtr() > state_.data());
+ assert(_state2 != nullptr && _state2->getPtr() != nullptr && _state2->getPtr() < state_.data() + state_idx_last_ && _state2->getPtr() > state_.data());
+
+ // Guarantee that we are updating the top triangular matrix (in cross covariance case)
+ bool flip = _state1->getPtr() > _state2->getPtr();
+ StateBase* stateA = (flip ? _state2 : _state1);
+ StateBase* stateB = (flip ? _state1 : _state2);
+ unsigned int row = (stateA->getPtr() - state_.data());
+ unsigned int col = (stateB->getPtr() - state_.data());
+ unsigned int block_rows = stateA->getStateSize();
+ unsigned int block_cols = stateB->getStateSize();
+
+ assert( block_rows == (flip ? _cov.cols() : _cov.rows()) && block_cols == (flip ? _cov.rows() : _cov.cols()) && "Bad covariance size in WolfProblem::addCovarianceBlock");
+
+ // STORE COVARIANCE
+ for (unsigned int i = 0; i < block_rows; i++)
+ for (unsigned int j = 0; j < block_cols; j++)
+ covariance_.coeffRef(i+row,j+col) = (flip ? _cov(j,i) : _cov(i,j));
+}
+
+void WolfProblem::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::MatrixXs& _cov_block) const
+{
+ assert(_state1 != nullptr && _state1->getPtr() != nullptr && _state1->getPtr() < state_.data() + state_idx_last_ && _state1->getPtr() > state_.data());
+ assert(_state2 != nullptr && _state2->getPtr() != nullptr && _state2->getPtr() < state_.data() + state_idx_last_ && _state2->getPtr() > state_.data());
+
+ // Guarantee that we are getting the top triangular matrix (in cross covariance case)
+ bool flip = _state1->getPtr() > _state2->getPtr();
+ StateBase* stateA = (flip ? _state2 : _state1);
+ StateBase* stateB = (flip ? _state1 : _state2);
+ unsigned int row = (stateA->getPtr() - state_.data());
+ unsigned int col = (stateB->getPtr() - state_.data());
+ unsigned int block_rows = stateA->getStateSize();
+ unsigned int block_cols = stateB->getStateSize();
+
+ assert(_cov_block.rows() == (flip ? block_cols : block_rows) && _cov_block.cols() == (flip ? block_rows : block_cols) && "Bad _cov_block matrix sizes");
+
+ _cov_block = (flip ? Eigen::MatrixXs(covariance_.block(row, col, block_rows, block_cols)) : Eigen::MatrixXs(covariance_.block(row, col, block_rows, block_cols)).transpose() );
+}
+
+void WolfProblem::getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::Map<Eigen::MatrixXs>& _cov_block) const
+{
+ assert(_state1 != nullptr && _state1->getPtr() != nullptr && _state1->getPtr() < state_.data() + state_idx_last_ && _state1->getPtr() > state_.data());
+ assert(_state2 != nullptr && _state2->getPtr() != nullptr && _state2->getPtr() < state_.data() + state_idx_last_ && _state2->getPtr() > state_.data());
+
+ // Guarantee that we are getting the top triangular matrix (in cross covariance case)
+ bool flip = _state1->getPtr() > _state2->getPtr();
+ StateBase* stateA = (flip ? _state2 : _state1);
+ StateBase* stateB = (flip ? _state1 : _state2);
+ unsigned int row = (stateA->getPtr() - state_.data());
+ unsigned int col = (stateB->getPtr() - state_.data());
+ unsigned int block_rows = stateA->getStateSize();
+ unsigned int block_cols = stateB->getStateSize();
+
+ assert(_cov_block.rows() == (flip ? block_cols : block_rows) && _cov_block.cols() == (flip ? block_rows : block_cols) && "Bad _cov_block matrix sizes");
+
+ _cov_block = (flip ? Eigen::MatrixXs(covariance_.block(row, col, block_rows, block_cols)).transpose() : Eigen::MatrixXs(covariance_.block(row, col, block_rows, block_cols)) );
+}
+
void WolfProblem::removeState(StateBase* _state_ptr)
{
// TODO: Reordering? Mandatory for filtering...
diff --git a/src/wolf_problem.h b/src/wolf_problem.h
index 2a7066c99..62c5cfd7c 100644
--- a/src/wolf_problem.h
+++ b/src/wolf_problem.h
@@ -20,7 +20,8 @@
class WolfProblem: public NodeBase
{
protected:
- Eigen::VectorXs state_;
+ Eigen::VectorXs state_;
+ Eigen::SparseMatrix<WolfScalar> covariance_;
unsigned int state_idx_last_;
NodeLocation location_;// TODO: should it be in node_base?
MapBase* map_ptr_;
@@ -37,14 +38,14 @@ class WolfProblem: public NodeBase
* Constructor from state size
*
*/
- WolfProblem(unsigned int _size=1e6);
+ WolfProblem(unsigned int _size=1e3);
/** \brief Constructor from map and trajectory shared pointers
*
* Constructor from map and trajectory shared pointers
*
*/
- WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr=nullptr, unsigned int _size=1e6);
+ WolfProblem(TrajectoryBase* _trajectory_ptr, MapBase* _map_ptr=nullptr, unsigned int _size=1e3);
/** \brief Default destructor
*
@@ -60,6 +61,21 @@ class WolfProblem: public NodeBase
*/
bool addState(StateBase* _new_state, const Eigen::VectorXs& _new_state_values);
+ /** \brief Adds a new covariance block
+ *
+ * Adds a new covariance block
+ *
+ */
+ void addCovarianceBlock(StateBase* _state1, StateBase* _state2, const Eigen::MatrixXs& _cov);
+
+ /** \brief Gets a covariance block
+ *
+ * Gets a covariance block
+ *
+ */
+ void getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::MatrixXs& _cov_block) const;
+ void getCovarianceBlock(StateBase* _state1, StateBase* _state2, Eigen::Map<Eigen::MatrixXs>& _cov_block) const;
+
/** \brief Removes a new state unit of the state
*
* Removes a new state unit of the state
--
GitLab