zbw/yiliao2026
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

改了一下ekf的源码，让odom不再完全相信轮式里程计；手贱删了个.git里的文件导致文件全部重新存，看不了diff了

Browse Source
This commit is contained in:
Orange
2026-06-13 19:54:44 +08:00
parent 51ea5bd52e
commit 5efbc1e68e
278 changed files with 0 additions and 23991 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
src/ground_slam/include/camera.h
View File

@@ -1,59 +0,0 @@
#ifndef CAMERA_H_
#define CAMERA_H_
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
class Camera{
public:
Camera();
Camera(const std::string& camera_file);
Camera& operator=(const Camera& camera);
void UndistortImage(cv::Mat& image, cv::Mat& undistort_image);
void GetNewCameraMatrix(cv::Mat& camera_matrix);
double GetImageHeight();
double GetImageWidth();
double GetHeight();
bool HeightIsAccurate();
void GetExtrinsics(Eigen::Matrix3d& extrinsics);
double GetLengthOfPixel();
Eigen::Vector3d ConvertPrincipalToCenter(const Eigen::Vector3d& image_plane_pose);
Eigen::Vector3d ConvertCenterToPrincipal(const Eigen::Vector3d& image_center_pose);
// Image plane: pixel plane, principal point on the image is the origin
// Camera: normalized plane,
// Robot: robot body coordinate system.
bool ConvertImagePlanePoseToCamera(Eigen::Vector3d& image_plane_pose, Eigen::Vector3d& camera_pose);
bool ConvertCameraPoseToImagePlane(Eigen::Vector3d& image_plane_pose, Eigen::Vector3d& camera_pose);
bool ConvertCameraPoseToRobot(Eigen::Vector3d& camera_pose, Eigen::Vector3d& robot_pose);
bool ConvertRobotPoseToCamera(Eigen::Vector3d& camera_pose, Eigen::Vector3d& robot_pose);
bool ConvertImagePlanePoseToRobot(Eigen::Vector3d& image_plane_pose, Eigen::Vector3d& robot_pose);
bool ConvertRobotPoseToImagePlane(Eigen::Vector3d& image_plane_pose, Eigen::Vector3d& robot_pose);
private:
int _image_height;
int _image_width;
double _scale;
double _new_scale;
int _new_width;
int _new_height;
double _height;
bool _accurate_height;
cv::Mat _K;
cv::Mat _new_K;
cv::Mat _D;
cv::Mat _map1;
cv::Mat _map2;
Eigen::Matrix3d _extrinsics;
};
typedef std::shared_ptr<Camera> CameraPtr;
#endif // CAMERA_H_

252
src/ground_slam/include/circ_shift.h
View File

@@ -1,252 +0,0 @@
// circ_shift.h
// https://stackoverflow.com/questions/46077242/eigen-modifyable-custom-expression/46301503#46301503
// this file implements circShift, fftshift, and ifftshift for Eigen vectors/matrices.
//
#pragma once
#include <Eigen/Core>
template <bool B> using bool_constant = std::integral_constant<bool, B>;
namespace helper
{
namespace detail
{
template <typename T>
constexpr std::true_type is_matrix(Eigen::MatrixBase<T>);
std::false_type constexpr is_matrix(...);
template <typename T>
constexpr std::true_type is_array(Eigen::ArrayBase<T>);
std::false_type constexpr is_array(...);
}
template <typename T>
struct is_matrix : decltype(detail::is_matrix(std::declval<std::remove_cv_t<T>>()))
{
};
template <typename T>
struct is_array : decltype(detail::is_array(std::declval<std::remove_cv_t<T>>()))
{
};
template <typename T>
using is_matrix_or_array = bool_constant<is_array<T>::value || is_matrix<T>::value>;
/*
* Index something if it's not an scalar
*/
template <typename T, typename std::enable_if<is_matrix_or_array<T>::value, int>::type = 0>
auto index_if_necessary(T&& thing, Eigen::Index idx)
{
return thing(idx);
}
/*
* Overload for scalar.
*/
template <typename T, typename std::enable_if<std::is_scalar<std::decay_t<T>>::value, int>::type = 0>
auto index_if_necessary(T&& thing, Eigen::Index)
{
return thing;
}
}
namespace Eigen
{
template <typename XprType, typename RowIndices, typename ColIndices>
class CircShiftedView;
namespace internal
{
template <typename XprType, typename RowIndices, typename ColIndices>
struct traits<CircShiftedView<XprType, RowIndices, ColIndices>>
: traits<XprType>
{
enum
{
RowsAtCompileTime = traits<XprType>::RowsAtCompileTime,
ColsAtCompileTime = traits<XprType>::ColsAtCompileTime,
MaxRowsAtCompileTime = (RowsAtCompileTime != Dynamic
? int(RowsAtCompileTime)
: int(traits<XprType>::MaxRowsAtCompileTime)),
MaxColsAtCompileTime = (ColsAtCompileTime != Dynamic
? int(ColsAtCompileTime)
: int(traits<XprType>::MaxColsAtCompileTime)),
XprTypeIsRowMajor = (int(traits<XprType>::Flags) & RowMajorBit) != 0,
IsRowMajor = ((MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1) ? 1
: (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) ? 0
: XprTypeIsRowMajor),
FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
Flags = (traits<XprType>::Flags & HereditaryBits) | FlagsLvalueBit | FlagsRowMajorBit
};
};
}
template <typename XprType, typename RowShift, typename ColShift, typename StorageKind>
class CircShiftedViewImpl;
template <typename XprType, typename RowShift, typename ColShift>
class CircShiftedView : public CircShiftedViewImpl<XprType, RowShift, ColShift,
typename internal::traits<XprType>::StorageKind>
{
public:
typedef typename CircShiftedViewImpl<XprType, RowShift, ColShift,
typename internal::traits<XprType>::StorageKind>::Base Base;
EIGEN_GENERIC_PUBLIC_INTERFACE(CircShiftedView)
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CircShiftedView)
typedef typename internal::ref_selector<XprType>::non_const_type MatrixTypeNested;
typedef typename internal::remove_all<XprType>::type NestedExpression;
template <typename T0, typename T1>
CircShiftedView(XprType& xpr, const T0& rowShift, const T1& colShift)
: m_xpr(xpr), m_rowShift(rowShift), m_colShift(colShift)
{
for (auto c = 0; c < xpr.cols(); ++c)
assert(std::abs(helper::index_if_necessary(m_rowShift, c)) < m_xpr.rows()); // row shift must be within +- rows()-1
for (auto r = 0; r < xpr.rows(); ++r)
assert(std::abs(helper::index_if_necessary(m_colShift, r)) < m_xpr.cols()); // col shift must be within +- cols()-1
}
/** \returns number of rows */
Index rows() const { return m_xpr.rows(); }
/** \returns number of columns */
Index cols() const { return m_xpr.cols(); }
/** \returns the nested expression */
const typename internal::remove_all<XprType>::type&
nestedExpression() const { return m_xpr; }
/** \returns the nested expression */
typename internal::remove_reference<XprType>::type&
nestedExpression() { return m_xpr.const_cast_derived(); }
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Index getRowIdx(Index row, Index col) const
{
Index R = m_xpr.rows();
assert(row >= 0 && row < R && col >= 0 && col < m_xpr.cols());
Index r = row - helper::index_if_necessary(m_rowShift, col);
if (r >= R)
return r - R;
if (r < 0)
return r + R;
return r;
}
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Index getColIdx(Index row, Index col) const
{
Index C = m_xpr.cols();
assert(row >= 0 && row < m_xpr.rows() && col >= 0 && col < C);
Index c = col - helper::index_if_necessary(m_colShift, row);
if (c >= C)
return c - C;
if (c < 0)
return c + C;
return c;
}
protected:
MatrixTypeNested m_xpr;
RowShift m_rowShift;
ColShift m_colShift;
};
// Generic API dispatcher
template <typename XprType, typename RowIndices, typename ColIndices, typename StorageKind>
class CircShiftedViewImpl
: public internal::generic_xpr_base<CircShiftedView<XprType, RowIndices, ColIndices>>::type
{
public:
typedef typename internal::generic_xpr_base<CircShiftedView<XprType, RowIndices, ColIndices>>::type Base;
};
namespace internal
{
template <typename ArgType, typename RowIndices, typename ColIndices>
struct unary_evaluator<CircShiftedView<ArgType, RowIndices, ColIndices>, IndexBased>
: evaluator_base<CircShiftedView<ArgType, RowIndices, ColIndices>>
{
typedef CircShiftedView<ArgType, RowIndices, ColIndices> XprType;
enum
{
CoeffReadCost = (evaluator<ArgType>::CoeffReadCost
+ NumTraits<Index>::AddCost /* for comparison */
+ NumTraits<Index>::AddCost) /* for addition */,
Flags = (evaluator<ArgType>::Flags & HereditaryBits),
Alignment = 0
};
EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_xpr(xpr)
{
EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
}
typedef typename XprType::Scalar Scalar;
typedef typename XprType::CoeffReturnType CoeffReturnType;
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
CoeffReturnType coeff(Index row, Index col) const
{
return m_argImpl.coeff(m_xpr.getRowIdx(row, col), m_xpr.getColIdx(row, col));
}
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
CoeffReturnType coeff(Index idx) const
{
if (m_xpr.cols() == 1)
return m_argImpl.coeff(m_xpr.getRowIdx(idx, 1), 1);
if (m_xpr.rows() == 1)
return m_argImpl.coeff(1, m_xpr.getColIdx(1, idx));
assert(m_xpr.cols() == 1 || m_xpr.rows() == 1);
// default no-assert case - assume col vector
return m_argImpl.coeff(m_xpr.getRowIdx(idx, 1), 1);
}
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Scalar& coeffRef(Index row, Index col)
{
assert(row >= 0 && row < m_xpr.rows() && col >= 0 && col < m_xpr.cols());
return m_argImpl.coeffRef(m_xpr.getRowIdx(row, col), m_xpr.getColIdx(row, col));
}
protected:
evaluator<ArgType> m_argImpl;
const XprType& m_xpr;
};
} // end namespace internal
} // end namespace Eigen
template <typename XprType, typename RowShift, typename ColShift>
auto circShift(Eigen::DenseBase<XprType>& x, RowShift r, ColShift c)
{
return Eigen::CircShiftedView<XprType, RowShift, ColShift>(x.derived(), r, c);
}
template <typename XprType>
auto fftshift(Eigen::DenseBase<XprType>& x)
{
Eigen::Index rs = x.rows() / 2;
Eigen::Index cs = x.cols() / 2;
return Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index>(x.derived(), rs, cs);
}
template <typename XprType>
auto ifftshift(Eigen::DenseBase<XprType>& x)
{
Eigen::Index rs = (x.rows() + 1) / 2;
Eigen::Index cs = (x.cols() + 1) / 2;
return Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index>(x.derived(), rs, cs);
}

36
src/ground_slam/include/correlation_flow.h
View File

@@ -1,36 +0,0 @@
#ifndef CORRELATION_FLOW_H
#define CORRELATION_FLOW_H
#include <fftw3.h>
#include "utils.h"
#include "read_configs.h"
class CorrelationFlow{
public:
CorrelationFlow(CFConfig& cf_config, double &image_height, double &image_width);
void ComputeIntermedium(const Eigen::ArrayXXf&, Eigen::ArrayXXcf&, Eigen::ArrayXXcf&);
Eigen::Vector3d ComputePose(const Eigen::ArrayXXcf&, const Eigen::ArrayXXf&, const Eigen::ArrayXXcf&, const Eigen::ArrayXXcf&, Eigen::Vector3d&, bool);
private:
CFConfig cfg;
Eigen::ArrayXXcf target_fft;
Eigen::ArrayXXcf target_rotation_fft;
Eigen::ArrayXXcf FFT(const Eigen::ArrayXXf&);
Eigen::ArrayXXf IFFT(const Eigen::ArrayXXcf&);
Eigen::ArrayXXcf GetTargetFFT(int, int);
inline Eigen::ArrayXXf RemoveZeroComponent(const Eigen::ArrayXXf&);
inline Eigen::ArrayXXcf gaussian_kernel(const Eigen::ArrayXXcf&, int, int);
inline Eigen::ArrayXXcf gaussian_kernel(const Eigen::ArrayXXcf&, const Eigen::ArrayXXcf&, int, int);
inline Eigen::ArrayXXcf polynomial_kernel(const Eigen::ArrayXXcf&, int, int);
inline Eigen::ArrayXXcf polynomial_kernel(const Eigen::ArrayXXcf&, const Eigen::ArrayXXcf&, int, int);
float EstimateTrans(const Eigen::ArrayXXcf&, const Eigen::ArrayXXcf&, const Eigen::ArrayXXcf&, int, int, Eigen::Vector2d&);
inline Eigen::ArrayXXf polar(const Eigen::ArrayXXf&);
inline float GetInfo(const Eigen::ArrayXXf&, float);
// void PrintArrayToFile (const Eigen::ArrayXXf target);
};
typedef std::shared_ptr<CorrelationFlow> CorrelationFlowPtr;
#endif // CORRELATION_FLOW_H

30
src/ground_slam/include/dataset.h
View File

@@ -1,30 +0,0 @@
#ifndef DATASET_H_
#define DATASET_H_
#include <vector>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "utils.h"
class Dataset{
public:
Dataset(const std::string& dataroot, const std::string& image_dir_name);
size_t GetDatasetLength();
bool GetImage(cv::Mat& image, size_t idx);
double GetTimestamp(size_t idx);
private:
std::string _dataroot;
std::string _image_dir;
std::string _image_name_file_path;
std::vector<std::string> _image_names;
std::string _time_file_path;
std::vector<double> _timestamps;
};
#endif // DATASET_H_

31
src/ground_slam/include/edge.h
View File

@@ -1,31 +0,0 @@
#ifndef EDGE_H_
#define EDGE_H_
#include <string>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "utils.h"
struct Edge{
enum Type {
Odom = 0,
KCC = 1,
Loop = 2,
Others = 3,
};
int _edge_id;
Type _type;
int _from;
int _to;
Eigen::Vector3d _T;
Eigen::Matrix3d _information;
Edge();
Edge(int edge_id, Type type, int from, int to, Eigen::Vector3d& T, Eigen::Matrix3d& information);
};
typedef std::shared_ptr<Edge> EdgePtr;
#endif // EDGE_H_

45
src/ground_slam/include/frame.h
View File

@@ -1,45 +0,0 @@
#ifndef FRAME_H_
#define FRAME_H_
#include <string>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "utils.h"
class Frame{
public:
Frame();
Frame(int frame_id);
Frame(int frame_id, double timestamp, Eigen::ArrayXXf&, Eigen::ArrayXXcf&, Eigen::ArrayXXcf&);
Frame& operator=(const Frame& other);
void SetFrameId(int frame_id);
int GetFrameId();
double GetTimestamp();
Eigen::ArrayXXf GetFrame();
void SetFFTResult(Eigen::ArrayXXcf& fft_result);
void SetFFTResult(Eigen::ArrayXXcf& fft_result, Eigen::ArrayXXcf& depth_fft_result);
void GetFFTResult(Eigen::ArrayXXcf& fft_result);
void GetFFTResult(Eigen::ArrayXXcf& fft_result, Eigen::ArrayXXcf& depth_fft_result);
void SetPose(Eigen::Vector3d& pose);
void GetPose(Eigen::Vector3d& pose);
void AddEdge(int edge_id);
void GetEdgeIds(std::vector<int>& edge_ids);
void SaveToDisk(const std::string root_dir);
private:
int _frame_id;
double _timestamp;
Eigen::ArrayXXf _frame;
Eigen::ArrayXXcf _fft_result;
Eigen::ArrayXXcf _fft_polar;
Eigen::ArrayXXcf _depth_fft_result;
Eigen::Vector3d _pose;
std::vector<int> _edge_ids;
};
typedef std::shared_ptr<Frame> FramePtr;
#endif // FRAME_H_

43
src/ground_slam/include/loop_closure.h
View File

@@ -1,43 +0,0 @@
#ifndef LOOP_CLOSURE_H_
#define LOOP_CLOSURE_H_
#include "read_configs.h"
#include "correlation_flow.h"
#include "map.h"
struct LoopClosureResult{
bool found;
Eigen::Vector3d response;
FramePtr current_frame;
FramePtr loop_frame;
Eigen::Vector3d relative_pose;
LoopClosureResult(): found(false), response(-1.0, -1.0, -1.0) {}
LoopClosureResult(bool _found, Eigen::Vector3d _resopnse): found(_found), response(_resopnse) {}
LoopClosureResult& operator=(const LoopClosureResult& other){
found = other.found;
response = other.response;
current_frame = other.current_frame;
loop_frame = other.loop_frame;
relative_pose = other.relative_pose;
return *this;
}
};
class LoopClosure{
public:
LoopClosure(LoopClosureConfig& loop_closure_config, CorrelationFlowPtr correlation_flow, MapPtr map);
LoopClosureResult FindLoopClosure(Eigen::ArrayXXf& image, FramePtr& current_frame);
LoopClosureResult FindLoopClosure(Eigen::ArrayXXf& image, FramePtr& current_frame, Eigen::Vector3d& prior_pose);
LoopClosureResult FindLoopClosure(Eigen::ArrayXXf& image, FramePtr& current_frame, std::vector<FramePtr>& frames);
private:
LoopClosureConfig _loop_thr;
CorrelationFlowPtr _correlation_flow;
MapPtr _map;
};
typedef std::shared_ptr<LoopClosure> LoopClosurePtr;
#endif // LOOP_CLOSURE_H_

81
src/ground_slam/include/map.h
View File

@@ -1,81 +0,0 @@
#ifndef MAP_H_
#define MAP_H_
#include <vector>
#include <map>
#include <unordered_set>
#include <unordered_map>
#include "read_configs.h"
#include "camera.h"
#include "frame.h"
#include "edge.h"
#include "utils.h"
struct GridLocation{
GridLocation() : x(0), y(0) {}
GridLocation(int x_, int y_) : x(x_), y(y_) {}
GridLocation& operator=(const GridLocation& other){
x = other.x;
y = other.y;
return *this;
}
friend std::ostream & operator<<(std::ostream &out, GridLocation &loc){
out << "(" << loc.x << ", " << loc.y << ")";
return out;
}
int x;
int y;
};
struct GridLocationHash{
std::size_t operator() (const GridLocation& loc) const{
return std::hash<int>()(loc.x) ^ std::hash<int>()(loc.y);
}
};
struct GridLocationEqual{
bool operator()(const GridLocation& l1, const GridLocation& l2) const{
return l1.x == l2.x && l1.y == l2.y;
}
};
typedef std::unordered_set<FramePtr> FrameSet;
typedef std::unordered_map<GridLocation, FrameSet, GridLocationHash, GridLocationEqual> GridMap;
class Map{
public:
Map();
Map(MapConfig& map_config);
void AddFrame(FramePtr& frame);
void SetFrameDistance(FramePtr& frame, double distance);
void AddEdge(EdgePtr& edge);
int GetAllFrames(std::vector<FramePtr>& frames);
double GetFrameDistance(FramePtr& frame);
int GetAllEdges(std::vector<EdgePtr>& edges);
void UpdatePoses(AlignedMap<int, Eigen::Vector3d> frame_poses);
GridLocation ComputeGridLocation(double x, double y);
GridLocation ComputeGridLocation(Eigen::Vector3d pose);
int GetFramesInGrids(std::vector<FramePtr>& frames, std::vector<GridLocation>& grid_locations);
FramePtr GetBaseframe();
private:
CameraPtr _camera;
std::map<int, FramePtr> _frames;
std::map<FramePtr, double> _frame_distanses;
std::map<int, EdgePtr> _edges;
double _grid_scale;
GridMap _grid_map;
FramePtr _baseframe;
};
typedef std::shared_ptr<Map> MapPtr;
#endif // MAP_H_

83
src/ground_slam/include/map_builder.h
View File

@@ -1,83 +0,0 @@
#ifndef MAP_BUILDER_H_
#define MAP_BUILDER_H_
#include "read_configs.h"
#include "camera.h"
#include "frame.h"
#include "correlation_flow.h"
#include "map.h"
#include "loop_closure.h"
#include "map_stitcher.h"
class MapBuilder{
public:
MapBuilder(Configs& configs);
bool AddNewInput(cv::Mat& image, double timestamp = -1);
void ComputeFFTResult(cv::Mat& image);
void ConstructFrame(double timestamp);
void SetCurrentFramePose();
bool Initialize();
void UpdateIntermedium();
void UpdateCurrentPose();
bool Tracking(Eigen::Vector3d& response);
void AddCFEdge();
void AddCFEdgeToMap(Eigen::Vector3d& relative_pose, int from, int to,
int edge_id, Edge::Type edge_type, Eigen::Matrix3d& info);
Eigen::Vector2d ComputeRelativeDA();
void SetFrameDistance();
bool FindLoopClosure();
void AddLoopEdges();
bool OptimizeMap();
void CheckAndOptimize();
void UpdateValueAfterLoop();
// for visualization
bool GetCFPose(Eigen::Vector3d& pose);
bool GetFramePoses(Aligned<std::vector, Eigen::Vector3d>& poses, std::vector<double>& timestamps);
bool GetOccupancyMapOrigin(
Eigen::Vector3d& pixel_origin, Eigen::Matrix<double, 7, 1>& real_origin); // [qw, qx, qy, qz, x, y, z]
double GetMapResolution();
OccupancyData& GetMapData();
private:
bool _init;
int _frame_id;
int _edge_id;
bool _last_lost;
// tmp
FramePtr _last_frame;
FramePtr _current_frame;
// image plane pose
Eigen::Vector3d _last_cf_pose;
Eigen::Vector3d _current_cf_pose;
// real scale pose
Eigen::Vector3d _last_cf_real_pose;
Eigen::Vector3d _current_cf_real_pose;
// robot pose
Eigen::Vector3d _last_pose;
Eigen::Vector3d _current_pose;
// distance
double _distance;
// intermedium rsults
Eigen::ArrayXXf _image_array;
Eigen::ArrayXXcf _last_fft_result;
Eigen::ArrayXXcf _last_fft_polar;
Eigen::ArrayXXcf _fft_result;
Eigen::ArrayXXcf _fft_polar;
std::vector<LoopClosureResult> _loop_matches;
Configs _configs;
CameraPtr _camera;
CorrelationFlowPtr _correlation_flow;
KeyframeSelectionConfig _kfs_config;
MapPtr _map;
LoopClosurePtr _loop_closure;
MapStitcherPtr _map_stitcher;
};
#endif // MAP_BUILDER_H

42
src/ground_slam/include/map_stitcher.h
View File

@@ -1,42 +0,0 @@
#ifndef MAP_STITCHER_H_
#define MAP_STITCHER_H_
#include "optimization_2d/pose_graph_2d_error_term.h"
#include "camera.h"
#include "frame.h"
#include "map.h"
#include "read_configs.h"
struct Cell{
int size;
Eigen::ArrayXXi data;
Eigen::ArrayXXi weight;
Cell(): size(0) {}
Cell(int _size): size(_size), data(Eigen::ArrayXXi::Zero(_size, _size)), weight(Eigen::ArrayXXi::Zero(_size, _size)) {}
};
typedef std::unordered_map<GridLocation, Cell, GridLocationHash, GridLocationEqual> OccupancyData;
class MapStitcher{
public:
MapStitcher(MapStitcherConfig config, CameraPtr camera);
void InsertFrame(FramePtr frame, cv::Mat& image);
// input is the posion in one axis, output is the cell and the position in cell
Eigen::Vector2i ComputeCellPosition(int x);
void AddImageToOccupancy(FramePtr frame);
void RecomputeOccupancy();
OccupancyData& GetOccupancyData();
private:
int _cell_size;
bool _to_stitch;
CameraPtr _camera;
std::unordered_map<FramePtr, Eigen::MatrixXi> _raw_images;
OccupancyData _occupancy_data;
};
typedef std::shared_ptr<MapStitcher> MapStitcherPtr;
#endif // MAP_STITCHER_H_

66
src/ground_slam/include/optimization_2d/angle_local_parameterization.h
View File

@@ -1,66 +0,0 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: vitus@google.com (Michael Vitus)
#ifndef OPTIMIZATION_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
#define OPTIMIZATION_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
#include <ceres/local_parameterization.h>
#include "normalize_angle.h"
namespace ceres {
namespace optimization_2d {
// Defines a local parameterization for updating the angle to be constrained in
// [-pi to pi).
class AngleLocalParameterization {
public:
template <typename T>
bool operator()(const T* theta_radians,
const T* delta_theta_radians,
T* theta_radians_plus_delta) const {
*theta_radians_plus_delta =
NormalizeAngle(*theta_radians + *delta_theta_radians);
return true;
}
static ceres::LocalParameterization* Create() {
return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
1,
1>);
}
};
} // namespace optimization_2d
} // namespace ceres
#endif // OPTIMIZATION_2D_ANGLE_LOCAL_PARAMETERIZATION_H_

52
src/ground_slam/include/optimization_2d/normalize_angle.h
View File

@@ -1,52 +0,0 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: vitus@google.com (Michael Vitus)
#ifndef OPTIMIZATION_2D_NORMALIZE_ANGLE_H_
#define OPTIMIZATION_2D_NORMALIZE_ANGLE_H_
#include <cmath>
#include <ceres/ceres.h>
namespace ceres {
namespace optimization_2d {
// Normalizes the angle in radians between [-pi and pi).
template <typename T>
inline T NormalizeAngle(const T& angle_radians) {
// Use ceres::floor because it is specialized for double and Jet types.
T two_pi(2.0 * M_PI);
return angle_radians -
two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
}
} // namespace optimization_2d
} // namespace ceres
#endif // OPTIMIZATION_2D_NORMALIZE_ANGLE_H_

33
src/ground_slam/include/optimization_2d/pose_graph_2d.h
View File

@@ -1,33 +0,0 @@
#ifndef OPTIMIZATION_2D_POSE_GRAPH_2D_H_
#define OPTIMIZATION_2D_POSE_GRAPH_2D_H_
#include <fstream>
#include <iostream>
#include <map>
#include <string>
#include <vector>
#include <ceres/ceres.h>
#include "angle_local_parameterization.h"
#include "pose_graph_2d_error_term.h"
#include "types.h"
namespace ceres {
namespace optimization_2d {
void BuildOptimizationProblem(const std::vector<Constraint2d>& constraints,
std::map<int, Pose2d>* poses,
ceres::Problem* problem);
void BuildOptimizationProblemWithScale(const std::vector<Constraint2d>& constraints,
std::vector<ScaleData>& scale_data,
const std::vector<int>& scale_data_idx,
std::map<int, Pose2d>* poses,
ceres::Problem* problem);
bool SolveOptimizationProblem(ceres::Problem* problem);
} // namespace optimization_2d
} // namespace ceres
#endif // OPTIMIZATION_2D_POSE_GRAPH_2D_H_

183
src/ground_slam/include/optimization_2d/pose_graph_2d_error_term.h
View File

@@ -1,183 +0,0 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: vitus@google.com (Michael Vitus)
//
// Cost function for a 2D pose graph formulation.
#ifndef OPTIMIZATION_2D_POSE_GRAPH_2D_ERROR_TERM_H_
#define OPTIMIZATION_2D_POSE_GRAPH_2D_ERROR_TERM_H_
#include <Eigen/Core>
#include "optimization_2d/normalize_angle.h"
namespace ceres {
namespace optimization_2d {
template <typename T>
Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) {
const T cos_yaw = ceres::cos(yaw_radians);
const T sin_yaw = ceres::sin(yaw_radians);
Eigen::Matrix<T, 2, 2> rotation;
rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
return rotation;
}
// Computes the error term for two poses that have a relative pose measurement
// between them. Let the hat variables be the measurement.
//
// residual = information^{1/2} * [ r_a^T * (p_b - p_a) - \hat{p_ab} ]
// [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
//
// where r_a is the rotation matrix that rotates a vector represented in frame A
// into the global frame, and Normalize(*) ensures the angles are in the range
// [-pi, pi).
class PoseGraph2dErrorTerm {
public:
PoseGraph2dErrorTerm(double x_ab,
double y_ab,
double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information)
: p_ab_(x_ab, y_ab),
yaw_ab_radians_(yaw_ab_radians),
sqrt_information_(sqrt_information) {}
template <typename T>
bool operator()(const T* const x_a,
const T* const y_a,
const T* const yaw_a,
const T* const x_b,
const T* const y_b,
const T* const yaw_b,
T* residuals_ptr) const {
const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
residuals_map.template head<2>() =
RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
residuals_map(2) = NormalizeAngle(
(*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
// Scale the residuals by the square root information matrix to account for
// the measurement uncertainty.
residuals_map = sqrt_information_.template cast<T>() * residuals_map;
return true;
}
static ceres::CostFunction* Create(double x_ab,
double y_ab,
double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information) {
return (new ceres::
AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
new PoseGraph2dErrorTerm(
x_ab, y_ab, yaw_ab_radians, sqrt_information)));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// The position of B relative to A in the A frame.
const Eigen::Vector2d p_ab_;
// The orientation of frame B relative to frame A.
const double yaw_ab_radians_;
// The inverse square root of the measurement covariance matrix.
const Eigen::Matrix3d sqrt_information_;
};
class PoseGraph2dErrorTermWithScale {
public:
PoseGraph2dErrorTermWithScale(double x_ab,
double y_ab,
double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information)
: p_ab_(x_ab, y_ab),
yaw_ab_radians_(yaw_ab_radians),
sqrt_information_(sqrt_information) {}
template <typename T>
bool operator()(const T* const x_a,
const T* const y_a,
const T* const yaw_a,
const T* const x_b,
const T* const y_b,
const T* const yaw_b,
const T* const scale,
T* residuals_ptr) const {
const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
Eigen::Matrix<T, 2, 2> scale_matrix = Eigen::Matrix<T, 2, 2>::Identity();
scale_matrix *= (*scale);
residuals_map.template head<2>() =
RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - scale_matrix * p_ab_.cast<T>();
residuals_map(2) = NormalizeAngle(
(*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
// Scale the residuals by the square root information matrix to account for
// the measurement uncertainty.
residuals_map = sqrt_information_.template cast<T>() * residuals_map;
return true;
}
static ceres::CostFunction* Create(double x_ab,
double y_ab,
double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information) {
return (new ceres::
AutoDiffCostFunction<PoseGraph2dErrorTermWithScale, 3, 1, 1, 1, 1, 1, 1, 1>(
new PoseGraph2dErrorTermWithScale(
x_ab, y_ab, yaw_ab_radians, sqrt_information)));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// The position of B relative to A in the A frame.
const Eigen::Vector2d p_ab_;
// The orientation of frame B relative to frame A.
const double yaw_ab_radians_;
// The inverse square root of the measurement covariance matrix.
const Eigen::Matrix3d sqrt_information_;
};
} // namespace optimization_2d
} // namespace ceres
#endif // OPTIMIZATION_2D_POSE_GRAPH_2D_ERROR_TERM_H_

116
src/ground_slam/include/optimization_2d/types.h
View File

@@ -1,116 +0,0 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: vitus@google.com (Michael Vitus)
//
// Defines the types used in the 2D pose graph SLAM formulation. Each vertex of
// the graph has a unique integer ID with a position and orientation. There are
// delta transformation constraints between two vertices.
#ifndef OPTIMIZATION_2D_TYPES_H_
#define OPTIMIZATION_2D_TYPES_H_
#include <fstream>
#include <Eigen/Core>
#include "optimization_2d/normalize_angle.h"
namespace ceres {
namespace optimization_2d {
struct ScaleData{
double scale;
bool fixed;
ScaleData() {}
ScaleData(double _scale, bool _fixed): scale(_scale), fixed(_fixed) {}
};
inline std::istream& operator>>(std::istream& input, ScaleData& scale_data) {
input >> scale_data.scale >> scale_data.fixed;
// Normalize the angle between -pi to pi.
return input;
}
// The state for each vertex in the pose graph.
struct Pose2d {
double x;
double y;
double yaw_radians;
// The name of the data type in the g2o file format.
static std::string name() { return "VERTEX_SE2"; }
};
inline std::istream& operator>>(std::istream& input, Pose2d& pose) {
input >> pose.x >> pose.y >> pose.yaw_radians;
// Normalize the angle between -pi to pi.
pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
return input;
}
// The constraint between two vertices in the pose graph. The constraint is the
// transformation from vertex id_begin to vertex id_end.
struct Constraint2d {
int id_begin;
int id_end;
double x;
double y;
double yaw_radians;
// The inverse of the covariance matrix for the measurement. The order of the
// entries are x, y, and yaw.
Eigen::Matrix3d information;
// The name of the data type in the g2o file format.
static std::string name() { return "EDGE_SE2"; }
};
inline std::istream& operator>>(std::istream& input, Constraint2d& constraint) {
input >> constraint.id_begin >> constraint.id_end >> constraint.x >>
constraint.y >> constraint.yaw_radians >> constraint.information(0, 0) >>
constraint.information(0, 1) >> constraint.information(0, 2) >>
constraint.information(1, 1) >> constraint.information(1, 2) >>
constraint.information(2, 2);
// Set the lower triangular part of the information matrix.
constraint.information(1, 0) = constraint.information(0, 1);
constraint.information(2, 0) = constraint.information(0, 2);
constraint.information(2, 1) = constraint.information(1, 2);
// Normalize the angle between -pi to pi.
constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians);
return input;
}
} // namespace optimization_2d
} // namespace ceres
#endif // OPTIMIZATION_2D_TYPES_H_

137
src/ground_slam/include/read_configs.h
View File

@@ -1,137 +0,0 @@
#ifndef READ_CONFIGS_H_
#define READ_CONFIGS_H_
#include <iostream>
#include <yaml-cpp/yaml.h>
#include "utils.h"
struct DatasetConfig{
std::string dataroot;
std::string image_dir_name;
std::string camera_file;
};
struct CFConfig{
int width;
int height;
float lambda;
int kernel;
float sigma;
float offset;
int power;
int rotation_divisor;
int rotation_channel;
};
struct KeyframeSelectionConfig{
double max_distance;
double max_angle;
double lower_response_thr;
double upper_response_thr;
};
struct MapConfig{
double grid_scale;
};
struct LoopClosureConfig{
bool to_find_loop;
double position_response_thr;
double angle_response_thr;
int frame_gap_thr;
double distance_thr;
};
struct MapStitcherConfig{
bool stitch_map;
int cell_size;
};
struct VisualizationConfig{
std::string frame_id;
std::string kcc_pose_topic;
std::string frame_pose_topic;
std::string map_topic;
std::string image_topic;
};
struct SavingConfig{
std::string saving_root;
bool save_pose;
};
struct Configs{
DatasetConfig dataset_config;
CFConfig cf_config;
KeyframeSelectionConfig keyframe_selection_config;
MapConfig map_config;
LoopClosureConfig loop_closure_config;
MapStitcherConfig map_stitcher_config;
VisualizationConfig visualization_config;
SavingConfig saving_config;
Configs(const std::string& config_file){
if(!FileExists(config_file)){
std::cout << "config file: " << config_file << " doesn't exist" << std::endl;
return;
}
YAML::Node file_node = YAML::LoadFile(config_file);
YAML::Node dataset_node = file_node["dataset"];
dataset_config.dataroot = dataset_node["dataroot"].as<std::string>();
dataset_config.image_dir_name = dataset_node["image_dir_name"].as<std::string>();
dataset_config.camera_file = dataset_node["camera_config"].as<std::string>();
YAML::Node cf_node = file_node["correlation_flow"];
cf_config.width = cf_node["width"].as<int>();
cf_config.height = cf_node["height"].as<int>();
cf_config.lambda = cf_node["lambda"].as<float>();
cf_config.rotation_divisor = cf_node["rotation_divisor"].as<int>();
cf_config.rotation_channel = cf_node["rotation_channel"].as<int>();
cf_config.kernel = cf_node["kernel"].as<int>();
cf_config.offset = cf_node["polynomial"]["offset"].as<float>();
cf_config.power = cf_node["polynomial"]["power"].as<int>();
cf_config.sigma = cf_node["gaussian"]["sigma"].as<float>();
YAML::Node kfs_node = file_node["keyframe_selection"];
keyframe_selection_config.max_distance = kfs_node["max_distance"].as<double>();
keyframe_selection_config.max_angle = kfs_node["max_angle"].as<double>();
keyframe_selection_config.lower_response_thr = kfs_node["lower_response_thr"].as<double>();
keyframe_selection_config.upper_response_thr = kfs_node["upper_response_thr"].as<double>();
YAML::Node map_node = file_node["map"];
map_config.grid_scale = map_node["grid_scale"].as<double>();
YAML::Node loop_closure_node = file_node["loop_closure"];
loop_closure_config.to_find_loop =
loop_closure_node["to_find_loop"].as<bool>();
loop_closure_config.position_response_thr =
loop_closure_node["position_response_thr"].as<double>();
loop_closure_config.angle_response_thr =
loop_closure_node["angle_response_thr"].as<double>();
loop_closure_config.frame_gap_thr =
loop_closure_node["frame_gap_thr"].as<int>();
loop_closure_config.distance_thr =
loop_closure_node["distance_thr"].as<double>();
YAML::Node map_stitcher_node = file_node["map_sticther"];
map_stitcher_config.stitch_map = map_stitcher_node["stitch_map"].as<bool>();
map_stitcher_config.cell_size = map_stitcher_node["cell_size"].as<int>();
YAML::Node visualization_node = file_node["visualization"];
visualization_config.frame_id = visualization_node["frame_id"].as<std::string>();
visualization_config.kcc_pose_topic = visualization_node["topic"]["kcc_pose"].as<std::string>();
visualization_config.frame_pose_topic = visualization_node["topic"]["frame_pose"].as<std::string>();
visualization_config.map_topic = visualization_node["topic"]["map"].as<std::string>();
visualization_config.image_topic = visualization_node["topic"]["image"].as<std::string>();
YAML::Node saving_node = file_node["saving"];
saving_config.saving_root = saving_node["saving_root"].as<std::string>();
saving_config.save_pose = saving_node["save_pose"].as<bool>();
}
};
#endif // READ_CONFIGS_H_

32
src/ground_slam/include/thread_publisher.h
View File

@@ -1,32 +0,0 @@
#ifndef THREAD_PUBLISHER_H_
#define THREAD_PUBLISHER_H_
#include <string>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <vector>
#include <queue>
#include <functional>
template <typename T>
class ThreadPublisher {
public:
ThreadPublisher();
~ThreadPublisher();
void Register(std::function<void(const std::shared_ptr<const T>&)> cb);
void Start();
void Publish(const std::shared_ptr<const T> msg);
void Process();
void ShutDown();
private:
std::mutex msg_mutex_;
std::condition_variable msg_cond_;
std::queue<std::shared_ptr<const T>> msgs_;
std::thread publish_thread_;
std::vector<std::function<void(const std::shared_ptr<const T>&)>> callbacks_;
bool shutdown_requested_;
};
#endif // THREAD_PUBLISHER_H_

27
src/ground_slam/include/timer.h
View File

@@ -1,27 +0,0 @@
#ifndef _TIMER_H_
#define _TIMER_H_
#include <sys/time.h>
#include <stdio.h>
typedef struct{
struct timeval start;
struct timeval stop;
}Timer;
void startTimer(Timer *pTimer);
void stopTimer(Timer *pTimer);
double getElapsedTime(Timer *pTimer);
void writeTimeToFile(double arrTime[], int nCount, int nFrameNo, char *filename);
#define INITIALIZE_TIMER Timer stTimer; double arrTime[100]
#define START_TIMER startTimer(&stTimer)
#define STOP_TIMER(text) stopTimer(&stTimer); printf(text); printf(": %.1f\n",getElapsedTime(&stTimer))
#define END_TIMER(nIndex) stopTimer(&stTimer); arrTime[nIndex] = getElapsedTime(&stTimer)
#define ACC_TIMER(nIndex) stopTimer(&stTimer); arrTime[nIndex] += getElapsedTime(&stTimer)
#define WRITE_TIME_FILE(nCount, nFrameNo, filename) writeTimeToFile(arrTime, nCount, nFrameNo, (char *)filename)
#endif//_TIMER_H_

82
src/ground_slam/include/utils.h
View File

@@ -1,82 +0,0 @@
#ifndef UTILS_H_
#define UTILS_H_
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <sys/types.h>
#include <sys/stat.h>
#include <functional>
#include <map>
#include <limits.h>
#include <memory>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
// Eigen type
template <template <typename, typename> class Container, typename Type>
using Aligned = Container<Type, Eigen::aligned_allocator<Type>>;
template <typename KeyType, typename ValueType>
using AlignedMap =
std::map<KeyType, ValueType, std::less<KeyType>,
Eigen::aligned_allocator<std::pair<const KeyType, ValueType>>>;
template <typename KeyType, typename ValueType>
using AlignedUnorderedMap = std::unordered_map<
KeyType, ValueType, std::hash<KeyType>, std::equal_to<KeyType>,
Eigen::aligned_allocator<std::pair<const KeyType, ValueType>>>;
template <typename KeyType, typename ValueType>
using AlignedUnorderedMultimap = std::unordered_multimap<
KeyType, ValueType, std::hash<KeyType>, std::equal_to<KeyType>,
Eigen::aligned_allocator<std::pair<const KeyType, ValueType>>>;
template <typename Type>
using AlignedUnorderedSet =
std::unordered_set<Type, std::hash<Type>, std::equal_to<Type>,
Eigen::aligned_allocator<Type>>;
// files
bool FileExists(const std::string& file);
bool PathExists(const std::string& path);
void ConcatenateFolderAndFileName(
const std::string& folder, const std::string& file_name,
std::string* path);
std::string ConcatenateFolderAndFileName(
const std::string& folder, const std::string& file_name);
void MakeDir(const std::string& path);
void ReadTxt(const std::string& file_path,
std::vector<std::vector<std::string> >& lines, std::string seq);
void WriteTxt(const std::string file_path,
std::vector<std::vector<std::string> >& lines, std::string seq);
// correlation flow
void ConvertMatToNormalizedArray(cv::Mat& image, Eigen::ArrayXXf& array);
Eigen::ArrayXXf ConvertMatToArray(const cv::Mat&);
cv::Mat ConvertArrayToMat(const Eigen::ArrayXXf&);
Eigen::ArrayXXf RotateArray(const Eigen::ArrayXXf&, float);
Eigen::ArrayXXf WarpArray(const Eigen::ArrayXXf&, float tx, float ty, float degree);
double NormalizeDegree(double angle_degree);
// Eigen pose
Eigen::Vector3d ComputeRelativePose(Eigen::Vector3d& pose1, Eigen::Vector3d& pose2);
Eigen::Vector3d ComputeAbsolutePose(Eigen::Vector3d& pose1, Eigen::Vector3d& relative_pose);
void ShowArray(const Eigen::ArrayXXf&, std::string window="debug", int waitKey=1);
#endif // UTILS_H_

65
src/ground_slam/include/visualization.h
View File

@@ -1,65 +0,0 @@
#ifndef VISUALIZATION_H_
#define VISUALIZATION_H_
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <functional>
#include <map>
#include <memory>
#include <Eigen/Core>
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <cv_bridge/cv_bridge.h>
#include "read_configs.h"
#include "map_stitcher.h"
#include "map_builder.h"
class Visualizer {
public:
enum class TrajectoryType {
Frame = 0,
KCC = 1,
Odom = 2,
};
Visualizer(rclcpp::Node::SharedPtr node, VisualizationConfig& config);
void AddNewPoseToPath(
Eigen::Vector3d& pose, double time_double,
nav_msgs::msg::Path& path, std::string& frame_id);
void UpdateOdomPose(Eigen::Vector3d& pose, double time_double);
void UpdateKccPose(Eigen::Vector3d& pose, double time_double);
void UpdateFramePose(Aligned<std::vector, Eigen::Vector3d>& frame_poses,
std::vector<double>& timestamps);
void ConvertMapToOccupancyMsgs(OccupancyData& map,
nav_msgs::msg::OccupancyGrid& msgs);
void UpdateMap(MapBuilder& map_builder);
void PublishImage(cv::Mat& image, double time_double);
void GetTrajectoryTxt(std::vector<std::vector<std::string>>& lines,
TrajectoryType trajectory_type);
private:
rclcpp::Node::SharedPtr node_;
std::string frame_id_;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr kcc_pose_pub_;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr frame_pose_pub_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
nav_msgs::msg::Path odom_pose_msgs_;
nav_msgs::msg::Path kcc_pose_msgs_;
nav_msgs::msg::Path frame_pose_msgs_;
nav_msgs::msg::OccupancyGrid occupancy_map_msgs_;
};
#endif // VISUALIZATION_H_