diff --git a/CMakeLists.txt b/CMakeLists.txt
index ecd68cab22364f4622c690bf8c121c1ac2eed53f..99e01fd2aedd98002570dd9a448c7e31a14d9737 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -221,14 +221,14 @@ SET(SRCS_YAML
#OPTIONALS
if(csm_FOUND)
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/laser/processor/processor_closeloop_icp.h
+ include/laser/processor/processor_loop_closure_icp.h
include/laser/processor/processor_odom_icp.h
)
SET(HDRS_FEATURE ${HDRS_FEATURE}
include/laser/feature/feature_icp_align.h
)
SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- src/processor/processor_closeloop_icp.cpp
+ src/processor/processor_loop_closure_icp.cpp
src/processor/processor_odom_icp.cpp
)
SET(SRCS_FEATURE ${SRCS_FEATURE}
diff --git a/include/laser/factor/factor_container.h b/include/laser/factor/factor_container.h
index 03c9ea6732fc337e44039acaedda88dd9b863560..0c3f5e29accee12a85f3945de1f36523e23a0881 100644
--- a/include/laser/factor/factor_container.h
+++ b/include/laser/factor/factor_container.h
@@ -24,6 +24,7 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
const unsigned int _corner,
bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorContainer,3,2,1,2,1>("FactorContainer",
+ TOP_LMK,
nullptr,
nullptr,
nullptr,
@@ -43,25 +44,20 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
std::cout << "new factor container: corner idx = " << corner_ << std::endl;
}
- virtual ~FactorContainer() = default;
+ virtual ~FactorContainer() = default;
- virtual std::string getTopology() const override
- {
- return std::string("LMK");
- }
+ LandmarkContainerPtr getLandmark()
+ {
+ return lmk_ptr_.lock();
+ }
- LandmarkContainerPtr getLandmark()
- {
- return lmk_ptr_.lock();
- }
-
- template <typename T>
- bool operator()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP, const T* const _landmarkO, T* _residuals) const
- {
- // Mapping
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkP);
- Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
- Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(_residuals);
+ template <typename T>
+ bool operator()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP, const T* const _landmarkO, T* _residuals) const
+ {
+ // Mapping
+ Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkP);
+ Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
+ Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(_residuals);
//std::cout << "getSensorPosition: " << std::endl;
//std::cout << getCapture()->getSensor()->getSensorPosition()->head(2).transpose() << std::endl;
@@ -69,7 +65,7 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
//std::cout << getCapture()->getSensor()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
//std::cout << "atan2: " << atan2(getCapture()->getSensor()->getSensorRotation()->transpose()(0,1),getCapture()->getSensor()->getSensorRotation()->transpose()(0,0)) << std::endl;
- // sensor transformation
+ // sensor transformation
Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
// robot information
@@ -128,8 +124,8 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
// std::cout << "\n\t" << _residuals[i] << " ";
//std::cout << std::endl;
- return true;
- }
+ return true;
+ }
};
} // namespace wolf
diff --git a/include/laser/factor/factor_point_2d.h b/include/laser/factor/factor_point_2d.h
index f2fd4e5c0790f33df685b5b12d032108dad68786..0e4188d5e69d29bbf619cc4b14fb6d8fff25bb13 100644
--- a/include/laser/factor/factor_point_2d.h
+++ b/include/laser/factor/factor_point_2d.h
@@ -31,6 +31,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPoint2d,2,2,1,2,1,2>("FactorPoint2d",
+ TOP_LMK,
_ftr_ptr,
nullptr, nullptr, nullptr, _lmk_ptr,
_processor_ptr,
@@ -62,11 +63,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
~FactorPoint2d() override = default;
- std::string getTopology() const override
- {
- return std::string("LMK");
- }
-
/**
* @brief getLandmarkPtr
* @return
diff --git a/include/laser/factor/factor_point_to_line_2d.h b/include/laser/factor/factor_point_to_line_2d.h
index 01d389a1ce2158ff8b63fe0d987a90bab89deca7..80db7c795b47e5733bbbe620376c8d7716311516 100644
--- a/include/laser/factor/factor_point_to_line_2d.h
+++ b/include/laser/factor/factor_point_to_line_2d.h
@@ -30,6 +30,7 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,2,2>("FactorPointToLine2d",
+ TOP_LMK,
_ftr_ptr,
nullptr,
nullptr,
@@ -69,11 +70,6 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
~FactorPointToLine2d() override = default;
- std::string getTopology() const override
- {
- return std::string("GEOM");
- }
-
LandmarkPolyline2dPtr getLandmark()
{
return std::static_pointer_cast<LandmarkPolyline2d>( getLandmarkOther() );
diff --git a/include/laser/processor/processor_closeloop_icp.h b/include/laser/processor/processor_loop_closure_icp.h
similarity index 91%
rename from include/laser/processor/processor_closeloop_icp.h
rename to include/laser/processor/processor_loop_closure_icp.h
index 03c873441b3399d662ae1c1c1c9f25231933d826..8c0ab2d910f734e44864222b76461c7a6ed635ff 100644
--- a/include/laser/processor/processor_closeloop_icp.h
+++ b/include/laser/processor/processor_loop_closure_icp.h
@@ -1,15 +1,14 @@
-#include "core/common/wolf.h"
-#include <iostream>
-#include <fstream>
+#ifndef SRC_PROCESSOR_LOOP_CLOSURE_ICP_H_
+#define SRC_PROCESSOR_LOOP_CLOSURE_ICP_H_
+
/**************************
* WOLF includes *
**************************/
+#include "core/common/wolf.h"
#include <core/processor/processor_base.h>
#include <laser/capture/capture_laser_2d.h>
-#include <core/factor/factor_base.h>
-#include <core/factor/factor_loopclosure_2d.h>
-#include <core/feature/feature_base.h>
+#include <core/factor/factor_relative_pose_2d_with_extrinsics.h>
#include <laser/feature/feature_icp_align.h>
/**************************
@@ -17,23 +16,13 @@
**************************/
#include <laser_scan_utils/laser_scan_utils.h>
#include <laser_scan_utils/icp.h>
-#include <map>
-
-/**************************
- * STD includes *
- **************************/
-// #include <fstream>
-// #include <iomanip>
-// #include <iostream>
-// #include <queue>
namespace wolf
{
-WOLF_PTR_TYPEDEFS(ProcessorCloseloopIcp);
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorCloseloopIcp);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureIcp);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureIcp);
-
-struct ParamsProcessorCloseloopIcp : public ParamsProcessorBase
+struct ParamsProcessorLoopClosureIcp : public ParamsProcessorBase
{
int recent_key_frames_ignored;
int key_frames_to_wait;
@@ -85,8 +74,8 @@ struct ParamsProcessorCloseloopIcp : public ParamsProcessorBase
double hsm_linear_xc_max_npeaks;
double hsm_linear_xc_peaks_min_distance;
- ParamsProcessorCloseloopIcp() = default;
- ParamsProcessorCloseloopIcp(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorLoopClosureIcp() = default;
+ ParamsProcessorLoopClosureIcp(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorBase(_unique_name, _server)
{
recent_key_frames_ignored = _server.getParam<int> (prefix + _unique_name + "/recent_key_frames_ignored");
@@ -160,7 +149,7 @@ struct ParamsProcessorCloseloopIcp : public ParamsProcessorBase
};
-class ProcessorCloseloopIcp : public ProcessorBase
+class ProcessorLoopClosureIcp : public ProcessorBase
{
protected:
// Useful sensor stuff
@@ -181,9 +170,9 @@ class ProcessorCloseloopIcp : public ProcessorBase
std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
public:
laserscanutils::icpParams icp_params_;
- ProcessorCloseloopIcp(ParamsProcessorCloseloopIcpPtr _params);
+ ProcessorLoopClosureIcp(ParamsProcessorLoopClosureIcpPtr _params);
- WOLF_PROCESSOR_CREATE(ProcessorCloseloopIcp, ParamsProcessorCloseloopIcp);
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureIcp, ParamsProcessorLoopClosureIcp);
void configure(SensorBasePtr _sensor) override;
protected:
@@ -216,3 +205,5 @@ class ProcessorCloseloopIcp : public ProcessorBase
};
}
+
+#endif // SRC_PROCESSOR_LOOP_CLOSURE_ICP_H_
diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
index ac5c30a5c27a1407c58d0a9f1689a657173bfbb4..d98b1bba63c697c7fd6869657cb1fdedf9bdfb0e 100644
--- a/include/laser/processor/processor_odom_icp.h
+++ b/include/laser/processor/processor_odom_icp.h
@@ -1,12 +1,18 @@
#ifndef SRC_PROCESSOR_ODOM_ICP_H_
#define SRC_PROCESSOR_ODOM_ICP_H_
-
+/**************************
+ * WOLF includes *
+ **************************/
+#include "core/common/wolf.h"
+#include "core/processor/processor_tracker.h"
#include "laser/capture/capture_laser_2d.h"
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "laser/feature/feature_icp_align.h"
-#include <core/processor/processor_tracker.h>
-
+/**************************
+ * ICP includes *
+ **************************/
#include <laser_scan_utils/laser_scan_utils.h>
#include <laser_scan_utils/icp.h>
@@ -190,19 +196,12 @@ class ProcessorOdomIcp : public ProcessorTracker, public IsMotion
FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser);
FactorBasePtr emplaceFactor(FeatureBasePtr _feature);
- inline bool voteForKeyFrameDistance() const;
- inline bool voteForKeyFrameAngle() const;
- inline bool voteForKeyFrameTime() const;
- inline bool voteForKeyFrameMatchQuality() const;
-
-
-
-
+ bool voteForKeyFrameDistance() const;
+ bool voteForKeyFrameAngle() const;
+ bool voteForKeyFrameTime() const;
+ bool voteForKeyFrameMatchQuality() const;
};
-
-
-
}
#endif // SRC_PROCESSOR_ODOM_ICP_H_
diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_loop_closure_icp.cpp
similarity index 85%
rename from src/processor/processor_closeloop_icp.cpp
rename to src/processor/processor_loop_closure_icp.cpp
index 2b5d24a8e87a6bb61416719b9892ea1c665d4f44..ef2acf5f3690dd40bf7e98925b8edcf21d2b0c5a 100644
--- a/src/processor/processor_closeloop_icp.cpp
+++ b/src/processor/processor_loop_closure_icp.cpp
@@ -1,16 +1,13 @@
-#include "laser/processor/processor_closeloop_icp.h"
-#include "core/common/wolf.h"
+#include "laser/processor/processor_loop_closure_icp.h"
#include "laser/sensor/sensor_laser_2d.h"
-#include <algorithm>
-#include <iterator>
using namespace wolf;
using namespace laserscanutils;
namespace wolf{
// Constructor
-ProcessorCloseloopIcp::ProcessorCloseloopIcp(ParamsProcessorCloseloopIcpPtr _params) :
- ProcessorBase("ProcessorCloseloopIcp", 2, _params)
+ProcessorLoopClosureIcp::ProcessorLoopClosureIcp(ParamsProcessorLoopClosureIcpPtr _params) :
+ ProcessorBase("ProcessorLoopClosureIcp", 2, _params)
{
recent_key_frames_ignored_ = _params->recent_key_frames_ignored;
key_frames_to_wait_ = _params->key_frames_to_wait;
@@ -55,17 +52,17 @@ ProcessorCloseloopIcp::ProcessorCloseloopIcp(ParamsProcessorCloseloopIcpPtr _par
icp_tools_ptr_ = std::make_shared<ICP>();
}
-void ProcessorCloseloopIcp::configure(SensorBasePtr _sensor)
+void ProcessorLoopClosureIcp::configure(SensorBasePtr _sensor)
{
sensor_laser_ = std::static_pointer_cast<SensorLaser2d>(_sensor);
laser_scan_params_ = sensor_laser_->getScanParams();
}
-void ProcessorCloseloopIcp::processCapture(CaptureBasePtr _capture_ptr)
+void ProcessorLoopClosureIcp::processCapture(CaptureBasePtr _capture_ptr)
{
}
-void ProcessorCloseloopIcp::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+void ProcessorLoopClosureIcp::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
{
// TODO: iterate all stored KF
// packs buffer_pack_kf_.selectFirstPackBefore(_keyframe_ptr->getTimeStamp(),_time_tolerance);
@@ -112,32 +109,32 @@ void ProcessorCloseloopIcp::processKeyFrame(FrameBasePtr _keyframe_ptr, const do
}
}
-bool ProcessorCloseloopIcp::triggerInCapture(CaptureBasePtr) const
+bool ProcessorLoopClosureIcp::triggerInCapture(CaptureBasePtr) const
{
return false;
}
-bool ProcessorCloseloopIcp::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const
+bool ProcessorLoopClosureIcp::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const
{
return true;
}
-bool ProcessorCloseloopIcp::storeKeyFrame(FrameBasePtr)
+bool ProcessorLoopClosureIcp::storeKeyFrame(FrameBasePtr)
{
return false;
}
-bool ProcessorCloseloopIcp::storeCapture(CaptureBasePtr)
+bool ProcessorLoopClosureIcp::storeCapture(CaptureBasePtr)
{
return true;
}
-bool ProcessorCloseloopIcp::voteForKeyFrame() const
+bool ProcessorLoopClosureIcp::voteForKeyFrame() const
{
return false;
}
-FrameBasePtrList ProcessorCloseloopIcp::selectCandidates(FrameBasePtr _keyframe_ptr, const double &_time_tolerance)
+FrameBasePtrList ProcessorLoopClosureIcp::selectCandidates(FrameBasePtr _keyframe_ptr, const double &_time_tolerance)
{
FrameBasePtrList candidates;
int key_frames_counter = 0;
@@ -155,7 +152,7 @@ FrameBasePtrList ProcessorCloseloopIcp::selectCandidates(FrameBasePtr _keyframe_
WOLF_DEBUG("%%%%%%%%%%%%%%%%%% CANDIDATES SIZE ", candidates.size());
return candidates;
}
-std::map<double, CapturesAligned> ProcessorCloseloopIcp::evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList &_keyframe_candidates)
+std::map<double, CapturesAligned> ProcessorLoopClosureIcp::evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList &_keyframe_candidates)
{
std::map<double, CapturesAligned> captures_candidates;
CaptureLaser2dPtr capture_current = std::static_pointer_cast<wolf::CaptureLaser2d>(_keyframe_own->getCaptureOf(this->getSensor()));
@@ -236,19 +233,25 @@ std::map<double, CapturesAligned> ProcessorCloseloopIcp::evaluateCandidates(Fram
}
return captures_candidates;
}
-CapturesAligned ProcessorCloseloopIcp::bestCandidate(std::map<double, CapturesAligned> &_capture_candidates)
+CapturesAligned ProcessorLoopClosureIcp::bestCandidate(std::map<double, CapturesAligned> &_capture_candidates)
{
return _capture_candidates.begin()->second;
}
-FactorBasePtr ProcessorCloseloopIcp::emplaceFeatureAndFactor(CapturesAligned &_captures_aligned)
+FactorBasePtr ProcessorLoopClosureIcp::emplaceFeatureAndFactor(CapturesAligned &_captures_aligned)
{
- auto ftr = FeatureBase::emplace<FeatureICPAlign>(_captures_aligned.capture_own, _captures_aligned.align_result);
- return FactorBase::emplace<FactorLoopclosure2d>(ftr, ftr, _captures_aligned.capture_other->getFrame(), shared_from_this(), params_->apply_loss_function);
+ auto ftr = FeatureBase::emplace<FeatureICPAlign>(_captures_aligned.capture_own,
+ _captures_aligned.align_result);
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(ftr,
+ ftr,
+ _captures_aligned.capture_other->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function,
+ TOP_LOOP);
}
}
// Register in the FactorySensor
#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR(ProcessorCloseloopIcp)
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorCloseloopIcp)
+WOLF_REGISTER_PROCESSOR(ProcessorLoopClosureIcp)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLoopClosureIcp)
} /* namespace wolf */
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index d27a141e1a6008cd24ee94c17cf60f4bb4d7a39c..bb8a3346523cca4149658d1997d1cf76c65fd5e3 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -1,11 +1,9 @@
#include "laser/processor/processor_odom_icp.h"
-#include "core/factor/factor_odom_2d.h"
#include "laser/math/laser_tools.h"
-#include <core/common/wolf.h>
-
-using namespace wolf;
using namespace laserscanutils;
+namespace wolf
+{
ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
ProcessorTracker("ProcessorOdomIcp", "PO", 2, _params),
@@ -269,11 +267,12 @@ FeatureBasePtr ProcessorOdomIcp::emplaceFeature(CaptureBasePtr _capture_laser)
FactorBasePtr ProcessorOdomIcp::emplaceFactor(FeatureBasePtr _feature)
{
- return FactorBase::emplace<FactorOdom2d>(_feature,
- _feature,
- origin_ptr_->getFrame(),
- shared_from_this(),
- params_->apply_loss_function);
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature,
+ _feature,
+ origin_ptr_->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function,
+ TOP_MOTION);
}
VectorComposite ProcessorOdomIcp::getState( const StateStructure& _structure ) const
@@ -346,6 +345,9 @@ void ProcessorOdomIcp::setProblem(ProblemPtr _problem_ptr)
addToProblem(_problem_ptr, std::dynamic_pointer_cast<IsMotion>(shared_from_this()));
}
+} // namespace wolf
+
+
// Register in the FactorySensor
#include "core/processor/factory_processor.h"
namespace wolf {