diff --git a/include/laser/processor/processor_odom_icp_3d.h b/include/laser/processor/processor_odom_icp_3d.h
index 06da63839a3d7deb90a4d3b5ce50808f3c50b360..655b72090286104dbd03dfa6337fcb555830a1cf 100644
--- a/include/laser/processor/processor_odom_icp_3d.h
+++ b/include/laser/processor/processor_odom_icp_3d.h
@@ -5,10 +5,9 @@
/**************************
* WOLF includes *
**************************/
-// #include "laser/processor/params_icp.h" //vsainz: parameters not specified yet
-#include "laser/sensor/sensor_laser_3d.h"
+
#include "laser/capture/capture_laser_3d.h"
-#include "laser/utils/laser3d_tools.h"
+#include "laser/sensor/sensor_laser_3d.h"
#include "core/common/wolf.h"
#include "core/processor/processor_tracker.h"
@@ -19,11 +18,7 @@
**************************/
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
-// #include <pcl/io/pcd_io.h>
-// #include <pcl/filters/voxel_grid.h>
-// #include <pcl/registration/icp.h>
-// #include <pcl/registration/icp_nl.h>
-// #include <pcl/registration/transforms.h>
+#include <pcl/registration/icp_nl.h>
namespace wolf
{
@@ -38,6 +33,8 @@ struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMo
{
bool pcl_downsample;
+ int vote_elapsed_time;
+
int icp_max_iterations;
double icp_transformation_translation_epsilon;
double icp_transformation_rotation_epsilon;
@@ -55,6 +52,8 @@ struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMo
pcl_downsample = _server.getParam<bool>(prefix + _unique_name + "/pcl_downsample");
+ vote_elapsed_time = _server.getParam<int>(prefix + _unique_name + "/vote_elapsed_time");
+
icp_max_iterations = _server.getParam<int>(prefix + _unique_name + "/icp_max_iterations");
icp_transformation_translation_epsilon = _server.getParam<double>(prefix + _unique_name + "/icp_transformation_translation_epsilon");
icp_transformation_rotation_epsilon = _server.getParam<double>(prefix + _unique_name + "/icp_transformation_rotation_epsilon");
@@ -87,9 +86,11 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
CaptureLaser3dPtr origin_laser_, last_laser_, incoming_laser_;
Eigen::Isometry3d T_origin_last_, T_last_incoming_, T_origin_incoming_, T_robot_sensor_, T_sensor_robot_;
-
pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver_;
+ Eigen::Matrix6d transform_cov_;
+
+
public:
ParamsProcessorOdomIcp3dPtr params_odom_icp_;
@@ -104,7 +105,6 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
VectorComposite getState(const StateStructure& _structure = "") const override;
TimeStamp getTimeStamp() const override;
VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
- void setProblem(ProblemPtr _problem_ptr) override;
protected:
// API required by ProcessorTracker
diff --git a/include/laser/sensor/sensor_laser_3d.h b/include/laser/sensor/sensor_laser_3d.h
index 1a16c56076731001e0e410b8887a52c3cec3d795..9f047946ee5ddaa52d40f61ca4d2e620e8c55411 100644
--- a/include/laser/sensor/sensor_laser_3d.h
+++ b/include/laser/sensor/sensor_laser_3d.h
@@ -19,17 +19,20 @@ struct ParamsSensorLaser3d : public ParamsSensorBase
{
}
~ParamsSensorLaser3d() override = default;
+};
- WOLF_PTR_TYPEDEFS(SensorLaser3d);
+WOLF_PTR_TYPEDEFS(SensorLaser3d);
- class SensorLaser3d : public SensorBase
- {
- public:
- SensorLaser3d(StateBlockPtr p_ptr, StateBlockPtr _o_ptr);
- ~SensorLaser3d();
- };
-
- WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 0);
+class SensorLaser3d : public SensorBase
+{
+ public:
+ SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr);
+ SensorLaser3d(const Eigen::VectorXd& _extrinsics, const std::shared_ptr<ParamsSensorLaser3d> _params);
+ ~SensorLaser3d();
+ ParamsSensorLaser3dPtr params_laser3d_;
+};
+
+WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 7);
} // namespace laser
} // namespace wolf
diff --git a/include/laser/utils/laser3d_tools.h b/include/laser/utils/laser3d_tools.h
index 5c82a908f0b51c75a31ca297f04187ab5172a8dc..c98305ea8c372850babf878c9ffe8f43afcdaa8c 100644
--- a/include/laser/utils/laser3d_tools.h
+++ b/include/laser/utils/laser3d_tools.h
@@ -1,5 +1,3 @@
-
-
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
@@ -11,7 +9,6 @@
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
-
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
typedef boost::shared_ptr<PointCloud> PointCloudBoostPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudBoostConstPtr;
@@ -20,9 +17,6 @@ namespace wolf
{
namespace laser
{
-
-// convenient typedefs
-
// _cloud_ref: first PointCloud
// _cloud_otherref: first PointCloud
inline void pairAlign(const PointCloudBoostConstPtr _cloud_ref,
diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp
index d2372ed77d0eb34bdebce23915bdb566df61f4e2..641ecc9cacca8f8f646970096a4d4a3117beb451 100644
--- a/src/processor/processor_odom_icp_3d.cpp
+++ b/src/processor/processor_odom_icp_3d.cpp
@@ -1,4 +1,6 @@
#include "laser/processor/processor_odom_icp_3d.h"
+#include "laser/utils/laser3d_tools.h"
+#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
namespace wolf
{
@@ -17,7 +19,12 @@ ProcessorOdomIcp3d::ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params)
registration_solver_.setTransformationRotationEpsilon(_params->icp_transformation_rotation_epsilon);
registration_solver_.setTransformationEpsilon(_params->icp_transformation_translation_epsilon);
registration_solver_.setMaxCorrespondenceDistance(_params->icp_max_correspondence_distance);
-}
+
+ params_odom_icp_ = _params;
+ transform_cov_.setIdentity();
+ }
+
+ProcessorOdomIcp3d::~ProcessorOdomIcp3d() {}
void ProcessorOdomIcp3d::configure(SensorBasePtr _sensor)
{
@@ -34,5 +41,128 @@ void ProcessorOdomIcp3d::preProcess()
incoming_laser_ = std::static_pointer_cast<CaptureLaser3d>(incoming_ptr_);
}
+/** \brief Tracker function
+ * \return The number of successful tracks.
+ *
+ * This is the tracker function to be implemented in derived classes.
+ * It operates on the \b incoming capture pointed by incoming_ptr_.
+ *
+ * This should do one of the following, depending on the design of the tracker -- see use_landmarks_:
+ * - Track Features against other Features in the \b origin Capture. Tips:
+ * - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
+ * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in \b
+ * last.
+ * - If required, correct the drift by re-comparing against the Features in origin.
+ * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
+ * - Track Features against Landmarks in the Map. Tips:
+ * - The links to the Landmarks are in the Features' Factors in last.
+ * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
+ *
+ * The function must generate the necessary Features in the \b incoming Capture,
+ * of the correct type, derived from FeatureBase.
+ *
+ * It must also generate the factors, of the correct type, derived from FactorBase
+ * (through FactorAnalytic or FactorSparse).
+ */
+unsigned int ProcessorOdomIcp3d::processKnown()
+{
+ pairAlign(origin_laser_->getPointCloud(),
+ incoming_laser_->getPointCloud(),
+ T_origin_last_,
+ T_origin_incoming_,
+ registration_solver_);
+ return 0;
+};
+
+/**\brief Process new Features or Landmarks
+ *
+ */
+unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
+{
+ pairAlign(last_laser_->getPointCloud(),
+ incoming_laser_->getPointCloud(),
+ Eigen::Isometry3d::Identity(),
+ T_last_incoming_,
+ registration_solver_);
+ return 0;
+};
+
+/** \brief Vote for KeyFrame generation
+ *
+ * If a KeyFrame criterion is validated, this function returns true,
+ * meaning that it wants to create a KeyFrame at the \b last Capture.
+ *
+ * WARNING! This function only votes! It does not create KeyFrames!
+ */
+bool ProcessorOdomIcp3d::voteForKeyFrame() const
+{
+ if (incoming_laser_->getTimeStamp() - origin_laser_->getTimeStamp() > params_odom_icp_->vote_elapsed_time)
+ {
+ return true;
+ }
+ return false;
+};
+
+/** \brief Advance the incoming Capture to become the last.
+ *
+ * Call this when the tracking and keyframe policy work is done and
+ * we need to get ready to accept a new incoming Capture.
+ */
+void ProcessorOdomIcp3d::advanceDerived(){
+ T_origin_last_ = T_origin_incoming_;
+ last_laser_ = incoming_laser_;
+ incoming_laser_ = nullptr;
+};
+
+/** \brief Reset the tracker using the \b last Capture as the new \b origin.
+ */
+void ProcessorOdomIcp3d::resetDerived(){
+ T_origin_last_ = T_last_incoming_;
+ origin_laser_ = last_laser_;
+ last_laser_ = incoming_laser_;
+ incoming_laser_ = nullptr;
+};
+
+// Queries to the processor:
+TimeStamp ProcessorOdomIcp3d::getTimeStamp() const
+{
+ return last_laser_->getTimeStamp();
+}
+
+VectorComposite ProcessorOdomIcp3d::getState(const StateStructure& _structure) const
+{
+ VectorComposite state_origin = origin_laser_->getFrame()->getState("PO");
+ Eigen::Isometry3d T_world_origin_robot = Eigen::Translation3d(state_origin.at('P')) * Eigen::Quaterniond(state_origin.at('O').data());
+ Eigen::Isometry3d T_world_last_robot = T_world_origin_robot * T_robot_sensor_ * T_origin_last_ * T_sensor_robot_;
+ VectorComposite state_last;
+ state_last.at('P') = T_world_last_robot.translation();
+ state_last.at('O') = Eigen::Quaterniond(T_world_last_robot.rotation()).coeffs();
+ return state_last;
+}
+
+VectorComposite ProcessorOdomIcp3d::getState(const TimeStamp& _ts, const StateStructure& _structure) const
+{
+ // TODO get the appropriate capture
+ return getState(_structure);
+}
+
+/**\brief Creates and adds factors from last_ to origin_
+ *
+ */
+void ProcessorOdomIcp3d::establishFactors()
+{
+// emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_
+ Eigen::Vector7d measurement_of_motion ;
+ measurement_of_motion.head(3) = T_origin_last_.translation();
+ measurement_of_motion.tail(4) = Eigen::Quaterniond( T_origin_last_.rotation() ).coeffs();
+ auto feature = FeatureBase::emplace<FeatureBase>(last_laser_, "FeatureBase", measurement_of_motion, transform_cov_ );
+
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(feature, feature, origin_laser_->getFrame(), shared_from_this(), false , TOP_MOTION);
+
+// emplace a factor of type FactorRelativePose3dWithExtrinsics under the feature above
+// TODO
+
+};
+
} // namespace laser
} // namespace wolf
\ No newline at end of file
diff --git a/src/sensor/sensor_laser_3d.cpp b/src/sensor/sensor_laser_3d.cpp
index e70bcd0b4785d421c9b9b4677a3ff6c25ce255bb..dd71f5a18531710e1ee210d8bd8bada7ffba6919 100644
--- a/src/sensor/sensor_laser_3d.cpp
+++ b/src/sensor/sensor_laser_3d.cpp
@@ -5,9 +5,21 @@ namespace wolf
namespace laser
{
-SensorLaser3d::SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
-SensorBase("SensorLaser3d", _p_ptr, _o_ptr, nullptr, 0) {}
-// vsainz: Parameters should be set somewhere, not yet!!
+SensorLaser3d::SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr)
+ : SensorBase("SensorLaser3d", _p_ptr, _o_ptr, nullptr, 0)
+{
+}
+
+SensorLaser3d::SensorLaser3d(const Eigen::VectorXd& _extrinsics, const std::shared_ptr<ParamsSensorLaser3d> _params);
+SensorBase("SensorLaser3d",
+ std::make_shared<StatePoint3d>(_extrinsics.head(3), true),
+ std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
+ nullptr,
+ 0),
+ params_laser3d_(_params)
+{
+ //
+}
SensorLaser3d::~SensorLaser3d() {}