diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index abd78202957f9aa05b9d98c7f68ddd0938bbe0ba..8582571f4b82b89b174b62e9c134457adebcaa53 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -37,6 +37,8 @@ SET(HDRS
node_linked.h
sensor_base.h
sensor_laser_2D.h
+ sensor_odom_2D.h
+ sensor_gps_fix.h
state_base.h
state_point.h
state_complex_angle.h
@@ -61,6 +63,8 @@ SET(SRCS
node_terminus.cpp
sensor_base.cpp
sensor_laser_2D.cpp
+ sensor_odom_2D.cpp
+ sensor_gps_fix.cpp
state_base.cpp
state_complex_angle.cpp
time_stamp.cpp
@@ -70,6 +74,8 @@ SET(SRCS
IF (Ceres_FOUND)
SET(SRCS ${SRCS} ceres_wrapper/complex_angle_parametrization.cpp)
SET(HDRS ${HDRS} ceres_wrapper/complex_angle_parametrization.h)
+ SET(SRCS ${SRCS} ceres_wrapper/ceres_manager.cpp)
+ SET(HDRS ${HDRS} ceres_wrapper/ceres_manager.h)
ENDIF(Ceres_FOUND)
# create the shared library
diff --git a/src/capture_gps_fix.cpp b/src/capture_gps_fix.cpp
index c04dd61f98a67ad75fe849fe6691a73786061635..4ce7a1a02385f1679ce0b505bbefdd4c9a78cb81 100644
--- a/src/capture_gps_fix.cpp
+++ b/src/capture_gps_fix.cpp
@@ -25,8 +25,7 @@ CaptureGPSFix::~CaptureGPSFix()
void CaptureGPSFix::processCapture()
{
- std::cout << "... processing GPS fix capture" << std::endl;
- FeatureBaseShPtr new_feature = FeatureBaseShPtr(new FeatureGPSFix(CaptureBasePtr(this),data_));
+ FeatureBaseShPtr new_feature = FeatureBaseShPtr(new FeatureGPSFix(CaptureBasePtr(this),data_,data_covariance_));
addFeature(new_feature);
}
diff --git a/src/capture_odom_2D.cpp b/src/capture_odom_2D.cpp
index ab514511f36fd504b44dbcbbcf8a8df1012c7e4b..9a2450bce5cd9608bbebbeb4e84180cc3c53b179 100644
--- a/src/capture_odom_2D.cpp
+++ b/src/capture_odom_2D.cpp
@@ -25,8 +25,7 @@ CaptureOdom2D::~CaptureOdom2D()
inline void CaptureOdom2D::processCapture()
{
- std::cout << "... processing odom 2D capture" << std::endl;
- FeatureBaseShPtr new_feature(new FeatureOdom2D(CaptureBasePtr(this),this->data_));
+ FeatureBaseShPtr new_feature(new FeatureOdom2D(CaptureBasePtr(this),data_,data_covariance_));
addFeature(new_feature);
}
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ab15a89ee470ae8fa3a5797910389b125dade3b
--- /dev/null
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -0,0 +1,177 @@
+#include "ceres_manager.h"
+
+CeresManager::CeresManager(ceres::Problem* _ceres_problem) :
+ ceres_problem_(_ceres_problem)
+{
+}
+
+CeresManager::~CeresManager()
+{
+// std::vector<double*> state_units;
+// ceres_problem_->GetParameterBlocks(&state_units);
+//
+// for (uint i = 0; i< state_units.size(); i++)
+// removeStateUnit(state_units.at(i));
+//
+// std::cout << "all state units removed! \n";
+// std::cout << "residual blocks: " << ceres_problem_->NumResidualBlocks() << "\n";
+// std::cout << "parameter blocks: " << ceres_problem_->NumParameterBlocks() << "\n";
+}
+
+ceres::Solver::Summary CeresManager::solve(const ceres::Solver::Options& _ceres_options)
+{
+ // create summary
+ ceres::Solver::Summary ceres_summary_;
+
+ // run Ceres Solver
+ ceres::Solve(_ceres_options, ceres_problem_, &ceres_summary_);
+
+ //display results
+ return ceres_summary_;
+}
+
+void CeresManager::addCorrespondences(std::list<CorrespondenceBasePtr>& _new_correspondences)
+{
+ //std::cout << _new_correspondences.size() << " new correspondences\n";
+ while (!_new_correspondences.empty())
+ {
+ addCorrespondence(_new_correspondences.front());
+ _new_correspondences.pop_front();
+ }
+}
+
+void CeresManager::removeCorrespondences()
+{
+ for (uint i = 0; i<correspondence_list_.size(); i++)
+ {
+ ceres_problem_->RemoveResidualBlock(correspondence_list_.at(i).first);
+ }
+ correspondence_list_.clear();
+}
+
+void CeresManager::addCorrespondence(const CorrespondenceBasePtr& _corr_ptr)
+{
+ ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getStateBlockPtrVector());
+ correspondence_list_.push_back(std::pair<ceres::ResidualBlockId, CorrespondenceBasePtr>(blockIdx,_corr_ptr));
+}
+
+void CeresManager::addStateUnits(std::list<StateBasePtr>& _new_state_units)
+{
+ while (!_new_state_units.empty())
+ {
+ addStateUnit(_new_state_units.front());
+ _new_state_units.pop_front();
+ }
+}
+
+void CeresManager::removeStateUnit(WolfScalar* _st_ptr)
+{
+ ceres_problem_->RemoveParameterBlock(_st_ptr);
+}
+
+void CeresManager::addStateUnit(const StateBasePtr& _st_ptr)
+{
+ //std::cout << "Adding a State Unit to wolf_problem... " << std::endl;
+ //_st_ptr->print();
+
+ switch (_st_ptr->getStateType())
+ {
+ case ST_COMPLEX_ANGLE:
+ {
+ //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
+ //ceres_problem_->SetParameterization(_st_ptr->getPtr(), new ComplexAngleParameterization);
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
+ break;
+ }
+// case PARAM_QUATERNION:
+// {
+// std::cout << "Adding Quaternion Local Parametrization to the List... " << std::endl;
+// ceres_problem_->SetParameterization(_st_ptr->getPtr(), new EigenQuaternionParameterization);
+// ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateQuaternion*)_st_ptr.get())->BLOCK_SIZE, new QuaternionParameterization);
+// break;
+// }
+ case ST_POINT_1D:
+ case ST_THETA:
+ {
+ //std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ case ST_POINT_2D:
+ {
+ //std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint2D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ case ST_POINT_3D:
+ {
+ //std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint3D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ default:
+ std::cout << "Unknown Local Parametrization type!" << std::endl;
+ }
+}
+
+ceres::CostFunction* CeresManager::createCostFunction(const CorrespondenceBasePtr& _corrPtr)
+{
+ switch (_corrPtr->getCorrespondenceType())
+ {
+ case CORR_GPS_FIX_2D:
+ {
+ CorrespondenceGPS2D* specific_ptr = (CorrespondenceGPS2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<CorrespondenceGPS2D,
+ specific_ptr->measurementSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case CORR_ODOM_2D_COMPLEX_ANGLE:
+ {
+ CorrespondenceOdom2DComplexAngle* specific_ptr = (CorrespondenceOdom2DComplexAngle*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<CorrespondenceOdom2DComplexAngle,
+ specific_ptr->measurementSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case CORR_ODOM_2D_THETA:
+ {
+ CorrespondenceOdom2DTheta* specific_ptr = (CorrespondenceOdom2DTheta*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<CorrespondenceOdom2DTheta,
+ specific_ptr->measurementSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ default:
+ std::cout << "Unknown correspondence type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
+
+ return nullptr;
+ }
+}
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..ab74a15de494b2c342ea6c42a893bb3a9aa13b96
--- /dev/null
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -0,0 +1,54 @@
+#ifndef CERES_MANAGER_H_
+#define CERES_MANAGER_H_
+
+//Ceres includes
+#include "ceres/jet.h"
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+//wof includes
+#include "wolf.h"
+#include "state_base.h"
+#include "state_point.h"
+#include "state_complex_angle.h"
+#include "correspondence_sparse.h"
+#include "correspondence_gps_2D.h"
+#include "correspondence_odom_2D_theta.h"
+#include "correspondence_odom_2D_complex_angle.h"
+
+// ceres wrapper includes
+#include "ceres_wrapper/complex_angle_parametrization.h"
+
+/** \brief Ceres manager for WOLF
+ *
+ */
+
+class CeresManager
+{
+ protected:
+ std::vector<std::pair<ceres::ResidualBlockId, CorrespondenceBasePtr>> correspondence_list_;
+ ceres::Problem* ceres_problem_;
+
+ public:
+ CeresManager(ceres::Problem* _ceres_problem);
+
+ ~CeresManager();
+
+ ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
+
+ void addCorrespondences(std::list<CorrespondenceBasePtr>& _new_correspondences);
+
+ void removeCorrespondences();
+
+ void addCorrespondence(const CorrespondenceBasePtr& _corr_ptr);
+
+ void addStateUnits(std::list<StateBasePtr>& _new_state_units);
+
+ void removeStateUnit(WolfScalar* _st_ptr);
+
+ void addStateUnit(const StateBasePtr& _st_ptr);
+
+ ceres::CostFunction* createCostFunction(const CorrespondenceBasePtr& _corrPtr);
+};
+
+#endif
diff --git a/src/correspondence_sparse.h b/src/correspondence_sparse.h
index 78c7035caab388e7e5de412ae241d191394fe2c8..a4351fe598f6daacc40843234d0aaa08a7f118bc 100644
--- a/src/correspondence_sparse.h
+++ b/src/correspondence_sparse.h
@@ -125,15 +125,16 @@ class CorrespondenceSparse: public CorrespondenceBase
{
return state_block_ptr_vector_;
}
-
+
+ // Ja és a correspondence_base...
/** \brief Returns a pointer to the mesaurement associated to this correspondence
- *
+ *
* Returns a pointer to the mesaurement associated to this correspondence.
* Measurement is owned by upper-level feature
**/
- const Eigen::VectorXs * getMeasurement() const
- {
- return upperNode().getMeasurement();
- }
+// const Eigen::VectorXs * getMeasurement() const
+// {
+// return upperNode().getMeasurement();
+// }
};
#endif
diff --git a/src/examples/test_ceres_manager_tree.cpp b/src/examples/test_ceres_manager_tree.cpp
index baf132007684fc88c6445899c5e036f479604bc7..dc44a0315810423c10001a359535c48d273bf0fc 100644
--- a/src/examples/test_ceres_manager_tree.cpp
+++ b/src/examples/test_ceres_manager_tree.cpp
@@ -13,13 +13,14 @@
#include <eigen3/Eigen/Geometry>
//Ceres includes
-#include "ceres/jet.h"
#include "ceres/ceres.h"
#include "glog/logging.h"
//Wolf includes
#include "wolf.h"
#include "sensor_base.h"
+#include "sensor_odom_2D.h"
+#include "sensor_gps_fix.h"
#include "frame_base.h"
#include "state_point.h"
#include "state_complex_angle.h"
@@ -33,8 +34,8 @@
#include "correspondence_odom_2D_theta.h"
#include "correspondence_odom_2D_complex_angle.h"
-// ceres wrapper includes
-#include "ceres_wrapper/complex_angle_parametrization.h"
+// ceres wrapper include
+#include "ceres_wrapper/ceres_manager.h"
/**
* This test implements an optimization using CERES of a vehicle trajectory using odometry and GPS simulated data.
@@ -69,12 +70,11 @@ class WolfManager
else
init_frame << 0, 0, 0;
createFrame(init_frame, 0);
-
- std::cout << "first frame created\n";
}
virtual ~WolfManager()
{
+ delete trajectory_;
// std::cout << "Destroying WolfManager...\n";
// std::cout << "Clearing correspondences_...\n";
// correspondences_.clear();
@@ -165,10 +165,6 @@ class WolfManager
new_correspondences.push_back((*correspondence_list_iter).get());
}
}
-
- // PRINT TREE
- std::cout << "TREE AFTER ADDING A CAPTURE\n\n";
- trajectory_->print();
}
}
@@ -179,9 +175,6 @@ class WolfManager
std::list<StateBasePtr> getStateList()
{
-
- std::cout << "getStateList...\n";
-
std::list<StateBasePtr> st_list;
for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++)
@@ -210,197 +203,16 @@ class WolfManager
}
return corr_list;
}
-};
-
-class CeresManager
-{
- protected:
-
- std::vector<std::pair<ceres::ResidualBlockId, CorrespondenceBasePtr>> correspondence_list_;
- ceres::Problem* ceres_problem_;
-
- public:
- CeresManager(ceres::Problem* _ceres_problem) :
- ceres_problem_(_ceres_problem)
- {
- }
-
- ~CeresManager()
- {
-// std::vector<double*> state_units;
-// ceres_problem_->GetParameterBlocks(&state_units);
-//
-// for (uint i = 0; i< state_units.size(); i++)
-// removeStateUnit(state_units.at(i));
-//
-// std::cout << "all state units removed! \n";
- std::cout << "residuals: " << ceres_problem_->NumResiduals() << "\n";
- std::cout << "parameters: " << ceres_problem_->NumParameters() << "\n";
- }
-
- ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options)
- {
- // create summary
- ceres::Solver::Summary ceres_summary_;
-
- // run Ceres Solver
- ceres::Solve(_ceres_options, ceres_problem_, &ceres_summary_);
- //display results
- return ceres_summary_;
- }
-
- void addCorrespondences(std::list<CorrespondenceBasePtr>& _new_correspondences)
- {
- //std::cout << _new_correspondences.size() << " new correspondences\n";
- while (!_new_correspondences.empty())
- {
- addCorrespondence(_new_correspondences.front());
- _new_correspondences.pop_front();
- }
- }
-
- void removeCorrespondences()
- {
- for (uint i = 0; i<correspondence_list_.size(); i++)
- {
- ceres_problem_->RemoveResidualBlock(correspondence_list_.at(i).first);
- }
- correspondence_list_.clear();
- std::cout << ceres_problem_->NumResidualBlocks() << " residual blocks \n";
- }
-
- void addCorrespondence(const CorrespondenceBasePtr& _corr_ptr)
- {
- ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getStateBlockPtrVector());
- correspondence_list_.push_back(std::pair<ceres::ResidualBlockId, CorrespondenceBasePtr>(blockIdx,_corr_ptr));
- }
-
- void addStateUnits(std::list<StateBasePtr>& _new_state_units)
- {
- while (!_new_state_units.empty())
- {
- addStateUnit(_new_state_units.front());
- _new_state_units.pop_front();
- }
- }
-
- void removeStateUnit(WolfScalar* _st_ptr)
- {
- ceres_problem_->RemoveParameterBlock(_st_ptr);
- }
-
- void addStateUnit(const StateBasePtr& _st_ptr)
- {
- //std::cout << "Adding a State Unit to wolf_problem... " << std::endl;
- //_st_ptr->print();
-
- switch (_st_ptr->getStateType())
- {
- case ST_COMPLEX_ANGLE:
- {
- //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- //ceres_problem_->SetParameterization(_st_ptr->getPtr(), new ComplexAngleParameterization);
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
- break;
- }
-// case PARAM_QUATERNION:
-// {
-// std::cout << "Adding Quaternion Local Parametrization to the List... " << std::endl;
-// ceres_problem_->SetParameterization(_st_ptr->getPtr(), new EigenQuaternionParameterization);
-// ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateQuaternion*)_st_ptr.get())->BLOCK_SIZE, new QuaternionParameterization);
-// break;
-// }
- case ST_POINT_1D:
- case ST_THETA:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_2D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint2D*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- case ST_POINT_3D:
- {
- //std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint3D*)_st_ptr)->BLOCK_SIZE, nullptr);
- break;
- }
- default:
- std::cout << "Unknown Local Parametrization type!" << std::endl;
- }
- }
-
- ceres::CostFunction* createCostFunction(const CorrespondenceBasePtr& _corrPtr)
- {
- switch (_corrPtr->getCorrespondenceType())
- {
- case CORR_GPS_FIX_2D:
- {
- CorrespondenceGPS2D* specific_ptr = (CorrespondenceGPS2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<CorrespondenceGPS2D,
- specific_ptr->measurementSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case CORR_ODOM_2D_COMPLEX_ANGLE:
- {
- CorrespondenceOdom2DComplexAngle* specific_ptr = (CorrespondenceOdom2DComplexAngle*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<CorrespondenceOdom2DComplexAngle,
- specific_ptr->measurementSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- case CORR_ODOM_2D_THETA:
- {
- CorrespondenceOdom2DTheta* specific_ptr = (CorrespondenceOdom2DTheta*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<CorrespondenceOdom2DTheta,
- specific_ptr->measurementSize,
- specific_ptr->block0Size,
- specific_ptr->block1Size,
- specific_ptr->block2Size,
- specific_ptr->block3Size,
- specific_ptr->block4Size,
- specific_ptr->block5Size,
- specific_ptr->block6Size,
- specific_ptr->block7Size,
- specific_ptr->block8Size,
- specific_ptr->block9Size>(specific_ptr);
- break;
- }
- default:
- std::cout << "Unknown correspondence type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
-
- return nullptr;
- }
- }
+ void printTree()
+ {
+ trajectory_->print();
+ }
};
int main(int argc, char** argv)
{
- std::cout << " ========= 2D Robot with odometry and GPS ===========\n\n";
+ std::cout << "\n ========= 2D Robot with odometry and GPS ===========\n";
// USER INPUT ============================================================================================
if (argc!=3 || atoi(argv[1])<1 || atoi(argv[2]) < 0 || atoi(argv[2]) > 1)
@@ -422,9 +234,11 @@ int main(int argc, char** argv)
// INITIALIZATION ============================================================================================
//init random generators
+ WolfScalar odom_std= 0.01;
+ WolfScalar gps_std= 1;
std::default_random_engine generator(1);
- std::normal_distribution<WolfScalar> distribution_odom(0.001,0.01); //odometry noise
- std::normal_distribution<WolfScalar> distribution_gps(0.0,1); //GPS noise
+ std::normal_distribution<WolfScalar> distribution_odom(0.001, odom_std); //odometry noise
+ std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise
//init google log
google::InitGoogleLogging(argv[0]);
@@ -453,9 +267,9 @@ int main(int argc, char** argv)
std::list<CorrespondenceBasePtr> new_correspondences; // new correspondences in wolf that must be added to ceres
// Wolf manager initialization
- SensorBasePtr odom_sensor = SensorBasePtr(new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(3,1),0));
- SensorBasePtr gps_sensor = SensorBasePtr(new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(3,1),0));
- WolfManager* wolf_manager = new WolfManager(odom_sensor, n_execution * (complex_angle ? 4 : 3), complex_angle);
+ SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(3,1), odom_std, odom_std);
+ SensorGPSFix gps_sensor(Eigen::MatrixXs::Zero(3,1), gps_std);
+ WolfManager* wolf_manager = new WolfManager(&odom_sensor, n_execution * (complex_angle ? 4 : 3), complex_angle);
// Initial pose
pose_true << 0,0,0;
@@ -491,26 +305,26 @@ int main(int argc, char** argv)
pose_odom(2) = pose_odom(2) + odom_readings(ii*2+1);
odom_trajectory.segment(ii*3,3) << pose_odom;
}
- std::cout << "sensor data created!\n";
// START TRAJECTORY ============================================================================================
new_state_units = wolf_manager->getStateList(); // First pose to be added in ceres
for (uint step=1; step < n_execution; step++)
{
- std::cout << "adding new sensor captures...\n";
// adding new sensor captures
- wolf_manager->addCapture(CaptureBaseShPtr(new CaptureOdom2D(TimeStamp(step*0.01), odom_sensor, odom_readings.segment(step*2,2))));
- wolf_manager->addCapture(CaptureBaseShPtr(new CaptureGPSFix(TimeStamp(step*0.01), gps_sensor, gps_fix_readings.segment(step*3,3))));
- std::cout << "updating problem...\n";
+ wolf_manager->addCapture(CaptureBaseShPtr(new CaptureOdom2D(TimeStamp(step*0.01), &odom_sensor, odom_readings.segment(step*2,2), odom_std * MatrixXs::Identity(2,2))));
+ wolf_manager->addCapture(CaptureBaseShPtr(new CaptureGPSFix(TimeStamp(step*0.01), &gps_sensor, gps_fix_readings.segment(step*3,3), gps_std * MatrixXs::Identity(3,3))));
+
// updating problem
wolf_manager->update(new_state_units, new_correspondences);
- std::cout << "sadding new state units and correspondences to ceres...\n";
+
// adding new state units and correspondences to ceres
ceres_manager->addStateUnits(new_state_units);
ceres_manager->addCorrespondences(new_correspondences);
}
- std::cout << "solving...\n";
+ //std::cout << "Resulting tree:\n";
+ //wolf_manager->printTree();
+
// SOLVE OPTIMIZATION ============================================================================================
ceres::Solver::Summary summary = ceres_manager->solve(ceres_options);
t2=clock();
@@ -550,13 +364,10 @@ int main(int argc, char** argv)
std::cout << std::endl << "Failed to write the file " << filepath << std::endl;
std::cout << " ========= END ===========" << std::endl << std::endl;
- //ceres_manager->removeCorrespondences();
- delete wolf_manager;
- std::cout << "everything deleted!\n";
+
delete ceres_manager;
- std::cout << "...deleted!\n";
delete ceres_problem;
- std::cout << "amost... deleted!\n";
+ delete wolf_manager;
//exit
return 0;
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index cd3380d6b7bda878de748a2cd09b8aea132693ef..201e265df9bf73f3f5548b5a05302d2eaceab075 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -7,12 +7,12 @@ FeatureBase::FeatureBase(const CaptureBasePtr& _capt_ptr, unsigned int _dim_meas
//
}
-FeatureBase::FeatureBase(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement) :
- NodeLinked(MID, "FEATURE", _capt_ptr),
- measurement_(_measurement)
-{
- //
-}
+//FeatureBase::FeatureBase(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement) :
+// NodeLinked(MID, "FEATURE", _capt_ptr),
+// measurement_(_measurement)
+//{
+// //
+//}
FeatureBase::FeatureBase(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
NodeLinked(MID, "FEATURE", _capt_ptr),
@@ -82,4 +82,6 @@ void FeatureBase::printSelf(unsigned int _ntabs, std::ostream & _ost) const
NodeLinked::printSelf(_ntabs, _ost);
printTabs(_ntabs);
_ost << "\tMeasurement: ( " << measurement_.transpose() << " )" << std::endl;
+ printTabs(_ntabs);
+ _ost << "\tMeasurement covariance: ( " << measurement_covariance_ << " )" << std::endl;
}
diff --git a/src/feature_base.h b/src/feature_base.h
index 9dd15aecfebeec77b20dfc416cc5c35fd1cf5fa8..912e1b56429c12fbe96c8e21abfa9bf402a0a5a0 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -35,7 +35,8 @@ class FeatureBase : public NodeLinked<CaptureBase,CorrespondenceBase>
* \param _measurement the measurement
*
*/
- FeatureBase(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement);
+ // measurement ha d'anar amb covariance, si cal, posem Identity com a default...
+ //FeatureBase(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement);
/** \brief Constructor from capture pointer and measure
*
diff --git a/src/feature_gps_fix.cpp b/src/feature_gps_fix.cpp
index 0efd143734b3fd6eb85157d8f04599c065f9359a..999138773963f50f055b04b67885ff2272d8e2e7 100644
--- a/src/feature_gps_fix.cpp
+++ b/src/feature_gps_fix.cpp
@@ -6,11 +6,11 @@ FeatureGPSFix::FeatureGPSFix(const CaptureBasePtr& _capt_ptr, unsigned int _dim_
//
}
-FeatureGPSFix::FeatureGPSFix(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement) :
- FeatureBase(_capt_ptr, _measurement)
-{
- //
-}
+//FeatureGPSFix::FeatureGPSFix(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement) :
+// FeatureBase(_capt_ptr, _measurement)
+//{
+// //
+//}
FeatureGPSFix::FeatureGPSFix(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
FeatureBase(_capt_ptr, _measurement, _meas_covariance)
diff --git a/src/feature_gps_fix.h b/src/feature_gps_fix.h
index 5263d4a4f1b6255a8197a3b28ae2836b93dd8245..19b99b794c2c52ded24c2f9facbe2573131e8ee1 100644
--- a/src/feature_gps_fix.h
+++ b/src/feature_gps_fix.h
@@ -25,7 +25,7 @@ class FeatureGPSFix : public FeatureBase
* \param _measurement the measurement
*
*/
- FeatureGPSFix(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement);
+ //FeatureGPSFix(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement);
/** \brief Constructor from capture pointer and measure
*
diff --git a/src/feature_odom_2D.cpp b/src/feature_odom_2D.cpp
index 93ae31814e9cd3215792f258affbf2396f56784b..b68c08386a53533d669972c2e0cbe53734d2f10b 100644
--- a/src/feature_odom_2D.cpp
+++ b/src/feature_odom_2D.cpp
@@ -6,11 +6,11 @@ FeatureOdom2D::FeatureOdom2D(const CaptureBasePtr& _capt_ptr, unsigned int _dim_
//
}
-FeatureOdom2D::FeatureOdom2D(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement) :
- FeatureBase(_capt_ptr, _measurement)
-{
- //
-}
+//FeatureOdom2D::FeatureOdom2D(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement) :
+// FeatureBase(_capt_ptr, _measurement)
+//{
+// //
+//}
FeatureOdom2D::FeatureOdom2D(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
FeatureBase(_capt_ptr, _measurement, _meas_covariance)
diff --git a/src/feature_odom_2D.h b/src/feature_odom_2D.h
index d6d66321b5195603a0c4faf41ea9bc6e55ee02ce..d958216d98bc27ee6675364926db40b237e3bef8 100644
--- a/src/feature_odom_2D.h
+++ b/src/feature_odom_2D.h
@@ -26,7 +26,7 @@ class FeatureOdom2D : public FeatureBase
* \param _measurement the measurement
*
*/
- FeatureOdom2D(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement);
+ //FeatureOdom2D(const CaptureBasePtr& _capt_ptr, const Eigen::VectorXs& _measurement);
/** \brief Constructor from capture pointer and measure
*
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 711785b364701317e1f4faac14d3a22f86f10f9d..1f878f6715dd6c1f359fd3b4819d8ef631d10b10 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -11,7 +11,6 @@ FrameBase::FrameBase(const TrajectoryBasePtr & _traj_ptr, const TimeStamp& _ts,
w_ptr_(_w_ptr)
{
//
- std::cout << "frame created\n";
}
FrameBase::FrameBase(const TrajectoryBasePtr & _traj_ptr, const FrameType & _tp, const TimeStamp& _ts, const StateBaseShPtr& _p_ptr, const StateBaseShPtr& _o_ptr, const StateBaseShPtr& _v_ptr, const StateBaseShPtr& _w_ptr) :
@@ -24,7 +23,6 @@ FrameBase::FrameBase(const TrajectoryBasePtr & _traj_ptr, const FrameType & _tp,
w_ptr_(_w_ptr)
{
//
- std::cout << "frame created\n";
}
FrameBase::~FrameBase()
diff --git a/src/sensor_gps_fix.cpp b/src/sensor_gps_fix.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f12a2cd2933c3e183f2d0d8d688fa7e59abf4867
--- /dev/null
+++ b/src/sensor_gps_fix.cpp
@@ -0,0 +1,17 @@
+#include "sensor_gps_fix.h"
+
+SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _sp, const double& _noise) :
+ SensorBase(GPS_FIX, _sp, Eigen::VectorXs::Constant(1,_noise))
+{
+ //
+}
+
+SensorGPSFix::~SensorGPSFix()
+{
+ //
+}
+
+WolfScalar SensorGPSFix::getNoise() const
+{
+ return params_(0);
+}
diff --git a/src/sensor_gps_fix.h b/src/sensor_gps_fix.h
new file mode 100644
index 0000000000000000000000000000000000000000..65a3ae03846d0340d0c8407179346b1153db6cca
--- /dev/null
+++ b/src/sensor_gps_fix.h
@@ -0,0 +1,35 @@
+
+#ifndef SENSOR_GPS_FIX_H_
+#define SENSOR_GPS_FIX_H_
+
+//wolf includes
+#include "sensor_base.h"
+
+class SensorGPSFix : public SensorBase
+{
+ public:
+ /** \brief Constructor with arguments
+ *
+ * Constructor with arguments
+ * \param _sp sensor 3D pose with respect to vehicle base frame
+ * \param _noise noise standard deviation
+ *
+ **/
+ SensorGPSFix(const Eigen::VectorXs & _sp, const double& _noise);
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ virtual ~SensorGPSFix();
+
+ /** \brief Returns noise standard deviation
+ *
+ * Returns noise standard deviation
+ *
+ **/
+ double getNoise() const;
+
+};
+#endif
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index 7915f973da543dff2cdcd31f8a427504320fce11..e0c09ed0fcec798069e197aa4fb4859be75e6e79 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -1,6 +1,6 @@
#include "sensor_laser_2D.h"
-SensorLaser2D::SensorLaser2D(const Eigen::VectorXs & _sp, unsigned int _nrays, double _apert, double _rmin, double _rmax) :
+SensorLaser2D::SensorLaser2D(const Eigen::VectorXs & _sp, unsigned int _nrays, WolfScalar _apert, WolfScalar _rmin, WolfScalar _rmax) :
SensorBase(LIDAR, _sp, 4)
// SensorBase(LIDAR, _sp,{(WolfScalar)(_nrays), _apert, _rmin, _rmax})
{
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index d9a2f626f77420c5311972ba71072109a23c4de1..0a9fd1b9189cc17a535b03ca2be27391391390fd 100644
--- a/src/sensor_laser_2D.h
+++ b/src/sensor_laser_2D.h
@@ -25,7 +25,7 @@ class SensorLaser2D : public SensorBase
* \param _rmax maximum range [m]
*
**/
- SensorLaser2D(const Eigen::VectorXs & _sp, unsigned int _nrays, double _apert, double _rmin, double _rmax);
+ SensorLaser2D(const Eigen::VectorXs & _sp, unsigned int _nrays, WolfScalar _apert, WolfScalar _rmin, WolfScalar _rmax);
/** \brief Destructor
*
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5bf66af15b572d34313918e926db2994b5f5b211
--- /dev/null
+++ b/src/sensor_odom_2D.cpp
@@ -0,0 +1,22 @@
+#include "sensor_odom_2D.h"
+
+SensorOdom2D::SensorOdom2D(const Eigen::VectorXs & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor) :
+ SensorBase(ODOM_2D, _sp, 2)
+{
+ params_ << _disp_noise_factor, _rot_noise_factor;
+}
+
+SensorOdom2D::~SensorOdom2D()
+{
+ //
+}
+
+WolfScalar SensorOdom2D::getDisplacementNoiseFactor() const
+{
+ return params_(0);
+}
+
+WolfScalar SensorOdom2D::getRotationNoiseFactor() const
+{
+ return params_(1);
+}
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
new file mode 100644
index 0000000000000000000000000000000000000000..c81628ba3e4604c6a56a82427b36458371206dd2
--- /dev/null
+++ b/src/sensor_odom_2D.h
@@ -0,0 +1,43 @@
+
+#ifndef SENSOR_ODOM_2D_H_
+#define SENSOR_ODOM_2D_H_
+
+//wolf includes
+#include "sensor_base.h"
+
+class SensorOdom2D : public SensorBase
+{
+ public:
+ /** \brief Constructor with arguments
+ *
+ * Constructor with arguments
+ * \param _sp sensor 3D pose with respect to vehicle base frame
+ * \param _disp_noise_factor displacement noise factor
+ * \param _rot_noise_factor rotation noise factor
+ *
+ **/
+ SensorOdom2D(const Eigen::VectorXs & _sp, const WolfScalar& _disp_noise_factor, const WolfScalar& _rot_noise_factor);
+
+ /** \brief Destructor
+ *
+ * Destructor
+ *
+ **/
+ virtual ~SensorOdom2D();
+
+ /** \brief Returns displacement noise factor
+ *
+ * Returns displacement noise factor
+ *
+ **/
+ double getDisplacementNoiseFactor() const;
+
+ /** \brief Returns rotation noise factor
+ *
+ * Returns rotation noise factor
+ *
+ **/
+ double getRotationNoiseFactor() const;
+
+};
+#endif
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 85ba42e621b2dbe119574a55cd8e4aab11f35771..0b81337c754053975c378f2c850ff86458e97b09 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -13,7 +13,6 @@ TrajectoryBase::~TrajectoryBase()
void TrajectoryBase::addFrame(FrameBaseShPtr& _frame_ptr)
{
- std::cout << "adding frame\n";
addDownNode(_frame_ptr);
}