Skip to content
Snippets Groups Projects
Commit 171c1259 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

compiling

parent 8ebaf8d1
No related branches found
No related tags found
No related merge requests found
...@@ -619,17 +619,21 @@ IF (laser_scan_utils_FOUND) ...@@ -619,17 +619,21 @@ IF (laser_scan_utils_FOUND)
include/base/capture/capture_laser_2D.h include/base/capture/capture_laser_2D.h
) )
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
include/base/processor/processor_polyline.h
include/base/processor/processor_tracker_feature_corner.h include/base/processor/processor_tracker_feature_corner.h
include/base/processor/processor_tracker_landmark_corner.h include/base/processor/processor_tracker_landmark_corner.h
) )
SET(HDRS_SENSOR ${HDRS_SENSOR} SET(HDRS_SENSOR ${HDRS_SENSOR}
include/base/sensor/sensor_laser_2D.h include/base/sensor/sensor_laser_2D.h
) )
SET(SRCS ${SRCS} SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
src/sensor/sensor_laser_2D.cpp src/processor/processor_polyline.cpp
src/processor/processor_tracker_feature_corner.cpp src/processor/processor_tracker_feature_corner.cpp
src/processor/processor_tracker_landmark_corner.cpp src/processor/processor_tracker_landmark_corner.cpp
) )
SET(SRCS_SENSOR ${SRCS_SENSOR}
src/sensor/sensor_laser_2D.cpp
)
ENDIF(laser_scan_utils_FOUND) ENDIF(laser_scan_utils_FOUND)
IF (raw_gps_utils_FOUND) IF (raw_gps_utils_FOUND)
......
...@@ -212,7 +212,7 @@ class LandmarkPolyline2D : public LandmarkBase ...@@ -212,7 +212,7 @@ class LandmarkPolyline2D : public LandmarkBase
YAML::Node saveToYaml() const; YAML::Node saveToYaml() const;
static void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _dist_tol); static void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _sq_dist_tol);
}; };
inline StateBlockPtr& LandmarkPolyline2D::firstStateBlock() inline StateBlockPtr& LandmarkPolyline2D::firstStateBlock()
......
...@@ -31,22 +31,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline); ...@@ -31,22 +31,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline);
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline); WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline);
typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DList; typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DList;
typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DList; typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DList;
typedef std::map<Scalar,FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DScalarMap;
typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScalarMap;
struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeature struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeature
{ {
laserscanutils::LineFinderIterativeParams line_finder_params; laserscanutils::LineFinderIterativeParams line_finder_params;
Scalar match_alignment_position_sq_norm_th; Scalar match_alignment_position_sq_norm_th;
Scalar match_pose_sq_norm_th; Scalar match_landmark_pose_sq_norm_th;
Scalar class_position_error_th; Scalar match_feature_pose_sq_norm_th;
unsigned int new_features_th; Scalar class_position_error_th;
Scalar loop_time_th; unsigned int new_features_th;
std::vector<PolylineRectangularClass> polyline_classes; Scalar loop_time_th;
std::vector<PolylineRectangularClass> polyline_classes;
// These values below are constant and defined within the class -- provide a setter or accept them at construction time if you need to configure them
//Scalar aperture_error_th_;// = 20.0 * M_PI / 180.; //20 degrees
//Scalar angular_error_th_;// = 10.0 * M_PI / 180.; //10 degrees;
//Scalar position_error_th_;// = 1;
//Scalar min_features_ratio_th_;// = 0.5;
}; };
class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
...@@ -57,6 +54,8 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature ...@@ -57,6 +54,8 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
FeatureBaseList all_features_incoming_, all_features_last_; FeatureBaseList all_features_incoming_, all_features_last_;
LandmarkMatchPolyline2DMap landmark_match_map_; LandmarkMatchPolyline2DMap landmark_match_map_;
LandmarkPolyline2DList modified_lmks_;
std::list<LandmarkPolyline2DList> merge_candidates_list_;
Eigen::Matrix2s R_sensor_world_last_, R_world_sensor_last_; Eigen::Matrix2s R_sensor_world_last_, R_world_sensor_last_;
Eigen::Vector2s t_sensor_world_last_, t_world_sensor_last_; Eigen::Vector2s t_sensor_world_last_, t_world_sensor_last_;
...@@ -130,24 +129,28 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature ...@@ -130,24 +129,28 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr){ return nullptr; }; FeatureBasePtr _feature_other_ptr){ return nullptr; };
/** \brief Create a new constraint and link it to the wolf tree
* \param _feature_ptr pointer to the feature
* \param _landmark_ptr pointer to the landmark.
*
* This function creates a constraint of the appropriate type for the derived processor.
*/
virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr,
LandmarkBasePtr _landmark_ptr) const;
/** \brief Establish constraints between features in Captures \b last and \b origin /** \brief Establish constraints between features in Captures \b last and \b origin
*/ */
virtual void establishConstraints(); virtual void establishConstraints();
void emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
LandmarkPolyline2DPtr _polyline_landmark,
const int& _ftr_point_id,
const int& _lmk_point_id,
const int& _lmk_prev_point_id);
void emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
LandmarkPolyline2DPtr _polyline_landmark,
const int& _ftr_point_id,
const int& _lmk_point_id);
/** \brief create a landmark from a feature /** \brief create a landmark from a feature
* *
*/ */
virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr); virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
bool modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr);
/** \brief advance pointers /** \brief advance pointers
* *
*/ */
...@@ -169,17 +172,19 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature ...@@ -169,17 +172,19 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
virtual void postProcess() override; virtual void postProcess() override;
FeatureMatchPolyline2DList tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const; FeatureMatchPolyline2DList tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const;
void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming, const Eigen::Matrix3s& _T_last_incoming_prior) const;
// FeatureMatchPolyline2DPtr bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A, // FeatureMatchPolyline2DPtr bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A,
// const FeatureMatchPolyline2DPtr& _match_B) const; // const FeatureMatchPolyline2DPtr& _match_B) const;
LandmarkMatchPolyline2DList tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const; LandmarkMatchPolyline2DList tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& _T_feature_landmark_prior) const;
bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& T_landmark_feature_prior); bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr);
void computeTransformations(); void computeTransformations();
void expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const; //void expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const;
public: public:
......
...@@ -967,7 +967,7 @@ YAML::Node LandmarkPolyline2D::saveToYaml() const ...@@ -967,7 +967,7 @@ YAML::Node LandmarkPolyline2D::saveToYaml() const
return node; return node;
} }
void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _dist_tol) void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _sq_dist_tol)
{ {
std::cout << "LandmarkPolyline2D::tryMergeLandmarks\n"; std::cout << "LandmarkPolyline2D::tryMergeLandmarks\n";
assert(_lmk_list.size() >= 2); assert(_lmk_list.size() >= 2);
...@@ -993,7 +993,7 @@ void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _dist_to ...@@ -993,7 +993,7 @@ void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _dist_to
Eigen::MatrixXs points_1 = lmk_1->computePointsGlobal(); Eigen::MatrixXs points_1 = lmk_1->computePointsGlobal();
Eigen::MatrixXs points_2 = lmk_2->computePointsGlobal(); Eigen::MatrixXs points_2 = lmk_2->computePointsGlobal();
MatchPolyline2DPtr match = computeBestSqDist(points_1, lmk_1->isFirstDefined(), lmk_1->isLastDefined(), lmk_1->isClosed(), MatchPolyline2DPtr match = computeBestSqDist(points_1, lmk_1->isFirstDefined(), lmk_1->isLastDefined(), lmk_1->isClosed(),
points_2, lmk_2->isFirstDefined(), lmk_2->isLastDefined(), lmk_2->isClosed(), _dist_tol*_dist_tol); points_2, lmk_2->isFirstDefined(), lmk_2->isLastDefined(), lmk_2->isClosed(), _sq_dist_tol);
// match // match
if ( match ) if ( match )
......
This diff is collapsed.
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment