├── CMakeLists.txt
├── README.md
├── error_term.h
├── main.cc
└── test.g2o


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(unity-lans-gz)
 3 | 
 4 | set(CMAKE_CXX_STANDARD 14)
 5 | set(CMAKE_BUILD_TYPE Release)
 6 | set(CMAKE_EXPORT_COMPILE_COMMANDS on)
 7 | 
 8 | find_package(Eigen3 REQUIRED)
 9 | 
10 | find_package(Sophus REQUIRED)
11 | include_directories(${Sophus_INCLUDE_DIRS})
12 | 
13 | find_package(Ceres REQUIRED)
14 | include_directories(${CERES_INCLUDE_DIRS})
15 | 
16 | add_executable(a main.cc)
17 | target_link_libraries(a
18 |   ${CERES_LIBRARIES} ${Sophus_LIBRARIES})
19 | 
20 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # switchable_constraints
2 | Mini program for testing the algorithm in {Switchable constraints for robust pose graph SLAM}
3 | 


--------------------------------------------------------------------------------
/error_term.h:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by alter on 17/10/2019.
  3 | //
  4 | 
  5 | #ifndef UNITY_LANS_GZ__ERROR_TERM_H_
  6 | #define UNITY_LANS_GZ__ERROR_TERM_H_
  7 | 
  8 | #include <sophus/se3.hpp>
  9 | #include <ceres/ceres.h>
 10 | 
 11 | class LocalParameterizationSE3 : public ceres::LocalParameterization {
 12 |  public:
 13 |   virtual ~LocalParameterizationSE3() {}
 14 | 
 15 |   // SE3 plus operation for Ceres
 16 |   //
 17 |   //  T * exp(x)
 18 |   //
 19 |   virtual bool Plus(double const *T_raw, double const *delta_raw,
 20 |                     double *T_plus_delta_raw) const {
 21 |     Eigen::Map<Sophus::SE3d const> const T(T_raw);
 22 |     Eigen::Map<Sophus::Vector6d const> const delta(delta_raw);
 23 |     Eigen::Map<Sophus::SE3d> T_plus_delta(T_plus_delta_raw);
 24 |     T_plus_delta = T * Sophus::SE3d::exp(delta);
 25 |     return true;
 26 |   }
 27 | 
 28 |   // Jacobian of SE3 plus operation for Ceres
 29 |   //
 30 |   // Dx T * exp(x)  with  x=0
 31 |   //
 32 |   virtual bool ComputeJacobian(double const *T_raw,
 33 |                                double *jacobian_raw) const {
 34 |     Eigen::Map<Sophus::SE3d const> T(T_raw);
 35 |     Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian(
 36 |         jacobian_raw);
 37 |     jacobian = T.Dx_this_mul_exp_x_at_0();
 38 |     return true;
 39 |   }
 40 | 
 41 |   virtual int GlobalSize() const { return Sophus::SE3d::num_parameters; }
 42 | 
 43 |   virtual int LocalSize() const { return Sophus::SE3d::DoF; }
 44 | };
 45 | 
 46 | struct OdometryFunctor {
 47 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 48 |   OdometryFunctor(Sophus::SE3d t_ij) : t_ij(t_ij) {}
 49 | 
 50 |   template<typename T>
 51 |   bool operator()(const T *t_i_ptr, const T *t_j_ptr, T *residual_ptr) const {
 52 |     Eigen::Map<const Sophus::SE3<T>> t_i(t_i_ptr);
 53 |     Eigen::Map<const Sophus::SE3<T>> t_j(t_j_ptr);
 54 |     Eigen::Map<Eigen::Matrix<T, 6, 1>> residual(residual_ptr);
 55 | 
 56 |     residual = (t_i * t_ij.template cast<T>() * t_j.inverse()).log();
 57 |     return true;
 58 |   }
 59 | 
 60 |   static ceres::CostFunction *Creat(const Sophus::SE3d t_ij) {
 61 |     return (new ceres::AutoDiffCostFunction<OdometryFunctor, 6, 7, 7>(
 62 |         new OdometryFunctor(t_ij)));
 63 |   }
 64 | 
 65 |   Sophus::SE3d t_ij;
 66 | };
 67 | 
 68 | struct LoopClosureFunctor {
 69 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 70 |   LoopClosureFunctor(Sophus::SE3d t_mn) : t_mn(t_mn) {}
 71 | 
 72 |   template<typename T>
 73 |   bool operator()(const T *t_m_ptr,
 74 |                   const T *t_n_ptr,
 75 |                   const T *s_ptr,
 76 |                   T *residual_ptr) const {
 77 |     Eigen::Map<const Sophus::SE3<T>> t_m(t_m_ptr);
 78 |     Eigen::Map<const Sophus::SE3<T>> t_n(t_n_ptr);
 79 |     Eigen::Map<Eigen::Matrix<T, 6, 1>> residual(residual_ptr);
 80 | 
 81 |     residual =
 82 |         *s_ptr * (t_m * t_mn.template cast<T>() * t_n.inverse()).log();
 83 |     return true;
 84 |   }
 85 | 
 86 |   static ceres::CostFunction *Creat(const Sophus::SE3d t_mn) {
 87 |     return (new ceres::AutoDiffCostFunction<LoopClosureFunctor, 6, 7, 7, 1>(
 88 |         new LoopClosureFunctor(t_mn)));
 89 |   }
 90 | 
 91 |   Sophus::SE3d t_mn;
 92 | };
 93 | 
 94 | struct PriorFunctor {
 95 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 96 |   PriorFunctor(double gamma) : gamma(gamma) {}
 97 | 
 98 |   template<typename T>
 99 |   bool operator()(const T *s_ptr, T *residual_ptr) const {
100 |     residual_ptr[0] = T(gamma) - *s_ptr;
101 |     return true;
102 |   }
103 | 
104 |   static ceres::CostFunction *Create(const double gamma) {
105 |     return (new ceres::AutoDiffCostFunction<PriorFunctor,
106 |                                             1,
107 |                                             1>(new PriorFunctor(gamma)));
108 |   }
109 | 
110 |   double gamma;
111 | };
112 | 
113 | #endif //UNITY_LANS_GZ__ERROR_TERM_H_
114 | 


--------------------------------------------------------------------------------
/main.cc:
--------------------------------------------------------------------------------
  1 | #include <vector>
  2 | #include <fstream>
  3 | #include <Eigen/Core>
  4 | #include "error_term.h"
  5 | 
  6 | Sophus::SE3d ReadVertex(std::ifstream *fin) {
  7 |   double x, y, z, qx, qy, qz, qw;
  8 |   *fin >> x >> y >> z >> qx >> qy >> qz >> qw;
  9 |   Sophus::SE3d
 10 |       pose(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(x, y, z));
 11 |   return pose;
 12 | }
 13 | 
 14 | Sophus::SE3d ReadEdge(std::ifstream *fin) {
 15 |   double x, y, z, qx, qy, qz, qw;
 16 |   *fin >> x >> y >> z >> qx >> qy >> qz >> qw;
 17 |   Sophus::SE3d
 18 |       pose(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(x, y, z));
 19 | 
 20 |   double information;
 21 |   for (int i = 0; i < 21; i++)
 22 |     *fin >> information;
 23 |   return pose;
 24 | }
 25 | 
 26 | int main(int argc, char **argv) {
 27 |   typedef Eigen::aligned_allocator<Sophus::SE3d> sophus_allocator;
 28 |   std::vector<Sophus::SE3d, sophus_allocator> vertices;
 29 |   std::vector<std::pair<std::pair<int, int>, Sophus::SE3d>, sophus_allocator>
 30 |       edges;
 31 | 
 32 |   std::ifstream fin("../test.g2o");
 33 |   std::string data_type;
 34 |   while (fin.good()) {
 35 |     fin >> data_type;
 36 |     if (data_type == "VERTEX_SE3:QUAT") {
 37 |       int id;
 38 |       fin >> id;
 39 |       vertices.emplace_back(ReadVertex(&fin));
 40 |     } else if (data_type == "EDGE_SE3:QUAT") {
 41 |       int i, j;
 42 |       fin >> i >> j;
 43 |       edges.emplace_back(std::pair<int, int>(i, j), ReadEdge(&fin));
 44 |     }
 45 | 
 46 |     fin >> std::ws;
 47 |   }
 48 | 
 49 |   std::vector<double> v_s;
 50 |   std::vector<double> v_gamma;
 51 | 
 52 |   ceres::Problem problem;
 53 | 
 54 |   for (auto e: edges) {
 55 |     auto ij = e.first;
 56 |     auto i = ij.first;
 57 |     auto j = ij.second;
 58 |     auto &pose_i = vertices.at(i);
 59 |     auto &pose_j = vertices.at(j);
 60 | 
 61 |     auto edge = e.second;
 62 |     if (i + 1 == j) {
 63 |       //odometry
 64 |       ceres::CostFunction *cost_function = OdometryFunctor::Creat(edge);
 65 |       problem.AddResidualBlock(cost_function,
 66 |                                NULL, pose_i.data(), pose_j.data());
 67 |     } else {
 68 |       //loop closure
 69 |       double s = 1;
 70 |       v_s.emplace_back(s);
 71 |       ceres::CostFunction *cost_function = LoopClosureFunctor::Creat(edge);
 72 |       problem.AddResidualBlock(cost_function,
 73 |                                NULL, pose_i.data(), pose_j.data(), &v_s.back());
 74 | 
 75 |       double gamma = 1;
 76 |       v_gamma.emplace_back(gamma);
 77 |       ceres::CostFunction
 78 |           *cost_function1 = PriorFunctor::Create(v_gamma.back());
 79 |       problem.AddResidualBlock(cost_function1,NULL,&v_s.back());
 80 | 
 81 |       //no robust
 82 | //      problem.SetParameterBlockConstant(&v_s.back());
 83 | 
 84 |       problem.SetParameterLowerBound(&v_s.back(),0,0);
 85 |       problem.SetParameterUpperBound(&v_s.back(),0,1);
 86 |     }
 87 |   }
 88 |   for (auto &i: vertices) {
 89 |     problem.SetParameterization(i.data(), new LocalParameterizationSE3);
 90 |   }
 91 | 
 92 |   problem.SetParameterBlockConstant(vertices.at(0).data());
 93 | 
 94 |   ceres::Solver::Options options;
 95 |   options.minimizer_progress_to_stdout = true;
 96 |   ceres::Solver::Summary summary;
 97 |   ceres::Solve(options, &problem, &summary);
 98 |   std::cout << summary.FullReport() << std::endl;
 99 | 
100 |   for (auto i: vertices) {
101 |     std::cout << i.matrix() << "\n" << std::endl;
102 |   }
103 | 
104 |   for (auto s: v_s) {
105 |     std::cout << s << std::endl;
106 |   }
107 | 
108 |   return 0;
109 | }
110 | 


--------------------------------------------------------------------------------
/test.g2o:
--------------------------------------------------------------------------------
 1 | VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
 2 | VERTEX_SE3:QUAT 1 1 0 0 0 0 0 1
 3 | VERTEX_SE3:QUAT 2 1 1 0 0 0 0 1
 4 | VERTEX_SE3:QUAT 3 0 1 0 0 0 0 1
 5 | VERTEX_SE3:QUAT 4 0 0 0 0 0 0 1
 6 | EDGE_SE3:QUAT 0 1 1 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
 7 | EDGE_SE3:QUAT 1 2 0 1 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
 8 | EDGE_SE3:QUAT 2 3 -1 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
 9 | EDGE_SE3:QUAT 3 4 0 -1 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
10 | EDGE_SE3:QUAT 4 0 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
11 | EDGE_SE3:QUAT 1 3 -0.5 0.5 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
12 | 


--------------------------------------------------------------------------------