mapper compile ok

common/math/fitting.h

@@ -48,7 +48,7 @@ Eigen::Matrix<double, 6, 1> FitLine3D(const pcl::PointCloud<PT> &cloud,
  * b^2 + c^2 = 1)
  */
 template <typename PT>
-Eigen::Vector3d FitPlane(const pcl::PointCloud<PT> &cloud,
+Eigen::Vector4d FitPlane(const pcl::PointCloud<PT> &cloud,
                          double *const score = nullptr) {
   Eigen::MatrixX3f data(cloud.size(), 3);
   size_t i = 0;
@@ -56,7 +56,7 @@ Eigen::Vector3d FitPlane(const pcl::PointCloud<PT> &cloud,
   Eigen::RowVector3f centroid = data.colwise().mean();
   Eigen::MatrixX3f data_centered = data.rowwise() - centroid;
   Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeThinV);
-  Eigen::Vector2f normal = svd.matrixV().col(2);
+  Eigen::Vector3f normal = svd.matrixV().col(2);
   float d = -centroid * normal;
   if (score) {
     *score = svd.singularValues()[1] * svd.singularValues()[1] /

oh_my_loam/base/utils.cc

@@ -1,28 +1,29 @@
-#include "oh_my_loam/base/helper.h"
+#include "oh_my_loam/base/utils.h"
 
 #include "common/pcl/pcl_utils.h"
 
 namespace oh_my_loam {
 
-void TransformToStart(const Pose3d &pose, const TPoint &pt_in,
+void TransformToStart(const common::Pose3d &pose, const TPoint &pt_in,
                       TPoint *const pt_out) {
-  Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
+  common::Pose3d pose_interp =
+      common::Pose3d().Interpolate(pose, GetTime(pt_in));
   common::TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
 }
 
-TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in) {
+TPoint TransformToStart(const common::Pose3d &pose, const TPoint &pt_in) {
   TPoint pt_out;
   TransformToStart(pose, pt_in, &pt_out);
   return pt_out;
 }
 
-void TransformToEnd(const Pose3d &pose, const TPoint &pt_in,
+void TransformToEnd(const common::Pose3d &pose, const TPoint &pt_in,
                     TPoint *const pt_out) {
   TransformToStart(pose, pt_in, pt_out);
   common::TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
 }
 
-TPoint TransformToEnd(const Pose3d &pose, const TPoint &pt_in) {
+TPoint TransformToEnd(const common::Pose3d &pose, const TPoint &pt_in) {
   TPoint pt_out;
   TransformToEnd(pose, pt_in, &pt_out);
   return pt_out;

oh_my_loam/mapper/map.h

@@ -13,6 +13,8 @@ class Grid;
 
 struct Index {
   int k, j, i;
+  Index() = default;
+  Index(int k, int j, int i) : k(k), j(j), i(i) {}
 
   struct Comp {
     bool operator()(const Index &idx1, const Index &idx2) const {

oh_my_loam/mapper/mapper.cc

@@ -173,18 +173,18 @@ void Mapper::AdjustMap(const TPoint &center) {
   }
 }
 
-TPointCloudPtr Mapper::GetCornMapPoints() const {
+TPointCloudPtr Mapper::GetMapCloudCorn() const {
   return corn_map_->GetAllPoints();
 }
 
-TPointCloudPtr Mapper::GetSurfMapPoints() const {
+TPointCloudPtr Mapper::GetMapCloudSurf() const {
   return surf_map_->GetAllPoints();
 }
 
-TPointCloudPtr Mapper::GetMapPoints() const {
+TPointCloudPtr Mapper::GetMapCloud() const {
   TPointCloudPtr map_points(new TPointCloud);
-  *map_points += *GetCornMapPoints();
+  *map_points += *GetMapCloudCorn();
-  *map_points += *GetSurfMapPoints();
+  *map_points += *GetMapCloudSurf();
   return map_points;
 }
 
@@ -198,6 +198,4 @@ void Mapper::SetState(State state) {
   state_ = state;
 }
 
-void Mapper::Visualize() {}
-
 }  // namespace oh_my_loam

oh_my_loam/mapper/mapper.h

@@ -25,11 +25,11 @@ class Mapper {
                const common::Pose3d &pose_curr2odom,
                common::Pose3d *const pose_curr2map);
 
-  TPointCloudPtr GetCornMapPoints() const;
+  TPointCloudPtr GetMapCloudCorn() const;
 
-  TPointCloudPtr GetSurfMapPoints() const;
+  TPointCloudPtr GetMapCloudSurf() const;
 
-  TPointCloudPtr GetMapPoints() const;
+  TPointCloudPtr GetMapCloud() const;
 
   void Reset();
 
@@ -64,8 +64,6 @@ class Mapper {
                  const TCTPointCloudConstPtr &tgt,
                  std::vector<PointLinePair> *const pairs) const;
 
-  void Visualize();
-
   YAML::Node config_;
 
   std::vector<int> map_shape_, submap_shape_;
@@ -74,8 +72,8 @@ class Mapper {
   std::unique_ptr<Map> surf_map_;
 
   mutable std::mutex state_mutex_;
-  common::Pose3d pose_odom2map_;
   State state_ = UN_INIT;
+  common::Pose3d pose_odom2map_;
 
   mutable std::unique_ptr<std::thread> thread_{nullptr};
 

oh_my_loam/odometer/odometer.cc

@@ -23,7 +23,7 @@ void Odometer::Reset() {
 }
 
 void Odometer::Process(double timestamp, const std::vector<Feature> &features,
-                       Pose3d *const pose_out) {
+                       common::Pose3d *const pose_out) {
   if (!is_initialized_) {
     UpdatePre(features);
     is_initialized_ = true;

oh_my_loam/odometer/odometer.h

@@ -18,11 +18,11 @@ class Odometer {
   void Process(double timestamp, const std::vector<Feature> &features,
                common::Pose3d *const pose_out);
 
-  TPointCloudConstPtr cloud_corn() const {
+  TPointCloudConstPtr GetCloudCorn() const {
     return kdtree_corn_.getInputCloud();
   }
 
-  TPointCloudConstPtr cloud_surf() const {
+  TPointCloudConstPtr GetCloudSurf() const {
     return kdtree_surf_.getInputCloud();
   }
 

oh_my_loam/oh_my_loam.cc

@@ -2,8 +2,8 @@
 
 #include <vector>
 
+#include "common/common.h"
 #include "common/pcl/pcl_utils.h"
-#include "oh_my_loam/base/helper.h"
 #include "oh_my_loam/extractor/extractor_VLP16.h"
 
 namespace oh_my_loam {
@@ -33,6 +33,7 @@ bool OhMyLoam::Init() {
 }
 
 void OhMyLoam::Reset() {
+  AWARN << "OhMySlam RESET";
   extractor_->Reset();
   odometer_->Reset();
   mapper_->Reset();
@@ -44,37 +45,27 @@ void OhMyLoam::Run(double timestamp,
   RemoveOutliers(*cloud_in, cloud.get());
   std::vector<Feature> features;
   extractor_->Process(timestamp, cloud, &features);
-  FusionOdometryMapping();
+  common::Pose3d pose_curr2odom;
-  auto pose_odom =
+  odometer_->Process(timestamp, features, &pose_curr2odom);
-      poses_curr2world_.empty() ? TimePose() : poses_curr2world_.back();
+  common::Pose3d pose_curr2map;
-  odometer_->Process(timestamp, features, &pose_odom.pose);
+  const auto &cloud_corn = odometer_->GetCloudCorn();
-  poses_curr2world_.push_back(pose_odom);
+  const auto &cloud_surf = odometer_->GetCloudSurf();
-  const auto &cloud_corn = odometer_->cloud_corn();
+  mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom,
-  const auto &cloud_surf = odometer_->cloud_surf();
+                   &pose_curr2map);
-
+  poses_curr2odom_.push_back(pose_curr2odom);
-  if (!pose_mapping_updated_) return;
+  poses_curr2world_.push_back(pose_curr2map);
-  mapping_thread_.reset(new std::thread(&OhMyLoam::MappingProcess, this,
+  if (is_vis_) Visualize(timestamp);
-                                        timestamp, cloud_corn, cloud_surf));
-  if (mapping_thread_->joinable()) mapping_thread_->detach();
-}
-
-void OhMyLoam::FusionOdometryMapping() {
-  std::lock_guard<std::mutex> lock(mutex_);
-  TimePose pose_m = pose_mapping_;
-  pose_mapping_updated_ = false;
-  for (;;) {
-  }
 }
 
 void OhMyLoam::Visualize(double timestamp) {}
 
 void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in,
                               common::PointCloud *const cloud_out) const {
-  common::RemovePoints<common::Point>(cloud_in, cloud_out, [&](const auto &pt) {
+  common::RemovePoints<common::Point>(
-    return !common::IsFinite<common::Point>(pt) ||
+      cloud_in, cloud_out, [&](const common::Point &pt) {
-           common::DistanceSquare<common::Point>(pt) <
+        return !common::IsFinite(pt) ||
-               kPointMinDist * kPointMinDist;
+               common::DistanceSquare(pt) < kPointMinDist * kPointMinDist;
-  });
+      });
 }
 
 }  // namespace oh_my_loam

oh_my_loam/oh_my_loam.h

@@ -20,8 +20,6 @@ class OhMyLoam {
  private:
   void Reset();
 
-  void FusionOdometryMapping();
-
   void Visualize(double timestamp = 0.0);
 
   std::unique_ptr<Extractor> extractor_{nullptr};
@@ -34,11 +32,13 @@ class OhMyLoam {
   void RemoveOutliers(const common::PointCloud &cloud_in,
                       common::PointCloud *const cloud_out) const;
 
+  std::vector<common::Pose3d> poses_curr2odom_;
   std::vector<common::Pose3d> poses_curr2world_;
 
   YAML::Node config_;
+  bool is_vis_ = false;
 
-  DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
+  DISALLOW_COPY_AND_ASSIGN(OhMyLoam);
 };
 
 }  // namespace oh_my_loam

oh_my_loam/solver/cost_function.h

@@ -174,7 +174,8 @@ bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat,
   }
   Eigen::Matrix<T, 3, 1> p0 = r * p + t;
 
-  residual[0] = coeff.topRows(3).transpose() * p + coeff[3];
+  residual[0] = coeff.topRows(3).transpose() * p0;
+  residual[0] += coeff(3);
   return true;
 }
 

oh_my_loam/visualizer/odometer_visualizer.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include "common/visualizer/lidar_visualizer.h"
-#include "oh_my_loam/base/helper.h"
+#include "oh_my_loam/solver/cost_function.h"
 
 namespace oh_my_loam {
 
@@ -23,9 +23,11 @@ class OdometerVisualizer : public common::LidarVisualizer {
  private:
   void Draw() override;
 
-  void DrawCorn(const Pose3d &pose, const std::vector<PointLinePair> &pairs);
+  void DrawCorn(const common::Pose3d &pose,
+                const std::vector<PointLinePair> &pairs);
 
-  void DrawSurf(const Pose3d &pose, const std::vector<PointPlanePair> &pairs);
+  void DrawSurf(const common::Pose3d &pose,
+                const std::vector<PointPlanePair> &pairs);
 
   void DrawTrajectory();