12345qiupeng
/
oh_my_loam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

odometry nearly ok, link error

main
feixyz10 2020-10-29 20:32:19 +08:00 committed by feixyz
parent acc6f39660
commit 9663294c81
21 changed files with 286 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
CMakeLists.txt
View File

@ -3,7 +3,7 @@ project(oh_my_loam)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++17 -lpthread")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall")
find_package(Ceres REQUIRED)
find_package(PCL QUIET)
@ -30,6 +30,9 @@ include_directories(SYSTEM
${G3LOG_INCLUDE_DIRS}
)
# link_directories(${PCL_LIBRARY_DIRS})
# add_definitions(${PCL_DEFINITIONS})
include_directories(
src
common
@ -53,10 +56,10 @@ target_link_libraries(main
${YAML_CPP_LIBRARIES}
g3log
common
oh_my_loam
# helper
# solver
extractor
visualizer
# odometry
helper
solver
odometry
oh_my_loam
)

1
common/common.h
View File

@ -4,6 +4,7 @@
#include "config.h"
#include "log.h"
#include "macros.h"
#include "pose.h"
#include "tic_toc.h"
#include "types.h"
#include "utils.h"

11
common/pose.cc
View File

@ -6,4 +6,15 @@ Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t) {
return pose_from.InterPolate(pose_to, t);
}
Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs) {
return Pose3D(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
}
std::string Pose3D::ToString() const {
std::ostringstream oss;
oss << "[Pose3D] q = (" << q_.coeffs().transpose() << "), p = ("
<< p_.transpose() << ")";
return oss.str();
}
} // namespace oh_my_loam

26
common/pose.h
View File

@ -14,10 +14,10 @@ class Pose3D {
Pose3D(const Eigen::Quaterniond& q, const Eigen::Vector3d& p)
: q_(q), p_(p) {}
Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p) {
q_ = Eigen::Quaterniond(r_mat);
p_ = p;
}
Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p)
: q_(r_mat), p_(p) {}
Pose3D(const double* const q, const double* const p) : q_(q), p_(p) {}
Pose3D Inv() const {
auto q_inv = q_.inverse();
@ -29,6 +29,10 @@ class Pose3D {
return q_ * vec + p_;
}
Eigen::Vector3d operator*(const Eigen::Vector3d& vec) const {
return Transform(vec);
}
template <typename PointT>
PointT Transform(const PointT& pt) const {
PointT pt_n = pt;
@ -39,6 +43,11 @@ class Pose3D {
return pt_n;
}
template <typename PointT>
PointT operator*(const PointT& vec) const {
return Transform<PointT>(vec);
}
Eigen::Vector3d Translate(const Eigen::Vector3d& vec) const {
return vec + p_;
}
@ -52,6 +61,11 @@ class Pose3D {
return {q_interp, p_interp};
}
std::string ToString() const;
Eigen::Quaterniond q() const { return q_; }
Eigen::Vector3d p() const { return p_; }
protected:
Eigen::Quaterniond q_; // orientation
Eigen::Vector3d p_; // position
@ -59,4 +73,8 @@ class Pose3D {
Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t);
Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs);
using Trans3D = Pose3D;
} // namespace oh_my_loam

6
common/types.h
View File

@ -8,9 +8,13 @@
#include <pcl/visualization/point_cloud_handlers.h>
#include <cmath>
// This hpp file should be included if user-defined point type is added, see
// Thses hpp file should be included if user-defined point type is added, see
// "How are the point types exposed?" section in
// https://pointclouds.org/documentation/tutorials/adding_custom_ptype.html
#include <pcl/filters/impl/voxel_grid.hpp>
#include <pcl/impl/pcl_base.hpp>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/visualization/impl/pcl_visualizer.hpp>
#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
namespace oh_my_loam {

5
configs/config.yaml
View File

@ -11,8 +11,11 @@ extractor_config:
sharp_corner_point_num: 2
corner_point_num: 20
flat_surf_point_num: 4
surf_point_num: 1000
surf_point_num: 20 ## useless currently
corner_point_curvature_thres: 0.5
surf_point_curvature_thres: 0.5
neighbor_point_dist_thres: 0.05
downsample_voxel_size: 0.2
odometry_config:
icp_iter_num : 2

2
main.cc
View File

@ -4,7 +4,7 @@
#include <functional>
#include "common/common.h"
#include "common.h"
#include "src/oh_my_loam.h"
using namespace oh_my_loam;

BIN
oh_my_loam
View File

Binary file not shown.

2
src/CMakeLists.txt
View File

@ -1,6 +1,6 @@
add_subdirectory(extractor)
add_subdirectory(visualizer)
# add_subdirectory(odometry)
add_subdirectory(odometry)
add_subdirectory(helper)
add_subdirectory(solver)

22
src/extractor/base_extractor.cc
View File

@ -44,7 +44,7 @@ void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) {
oss << "Feature point num: ";
for (const auto& scan : scans) {
FeaturePoints scan_feature;
StoreToFeaturePoints(scan, &scan_feature);
GenerateFeaturePoints(scan, &scan_feature);
feature->Add(scan_feature);
oss << scan.size() << ":" << scan_feature.sharp_corner_pts->size() << ":"
<< scan_feature.less_sharp_corner_pts->size() << ":"
@ -187,29 +187,33 @@ void Extractor::SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
}
}
void Extractor::StoreToFeaturePoints(const TCTPointCloud& scan,
FeaturePoints* const feature) const {
void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan,
FeaturePoints* const feature) const {
for (const auto& pt : scan.points) {
switch (pt.type) {
case PointType::FLAT:
feature->flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z);
feature->flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
// no break: FLAT points are also LESS_FLAT
case PointType::LESS_FLAT:
feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z);
feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z,
pt.time);
break;
case PointType::SHARP:
feature->sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z);
feature->sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z,
pt.time);
// no break: SHARP points are also LESS_SHARP
case PointType::LESS_SHARP:
feature->less_sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z);
feature->less_sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z,
pt.time);
break;
default:
// all the rest are also LESS_FLAT
feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z);
feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z,
pt.time);
break;
}
}
PointCloudPtr filtered_less_flat_surf_pts(new PointCloud);
TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud);
VoxelDownSample(*feature->less_flat_surf_pts,
filtered_less_flat_surf_pts.get(),
config_["downsample_voxel_size"].as<double>());

4
src/extractor/base_extractor.h
View File

@ -37,8 +37,8 @@ class Extractor {
void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
std::vector<bool>* const picked) const;
void StoreToFeaturePoints(const TCTPointCloud& scan,
FeaturePoints* const feature) const;
void GenerateFeaturePoints(const TCTPointCloud& scan,
FeaturePoints* const feature) const;
bool is_vis_ = false;

18
src/extractor/feature_points.h
View File

@ -1,20 +1,20 @@
#pragma once
#include "common.h"
#include "types.h"
namespace oh_my_loam {
struct FeaturePoints {
PointCloudPtr sharp_corner_pts;
PointCloudPtr less_sharp_corner_pts;
PointCloudPtr flat_surf_pts;
PointCloudPtr less_flat_surf_pts;
TPointCloudPtr sharp_corner_pts;
TPointCloudPtr less_sharp_corner_pts;
TPointCloudPtr flat_surf_pts;
TPointCloudPtr less_flat_surf_pts;
FeaturePoints() {
sharp_corner_pts.reset(new PointCloud);
less_sharp_corner_pts.reset(new PointCloud);
flat_surf_pts.reset(new PointCloud);
less_flat_surf_pts.reset(new PointCloud);
sharp_corner_pts.reset(new TPointCloud);
less_sharp_corner_pts.reset(new TPointCloud);
flat_surf_pts.reset(new TPointCloud);
less_flat_surf_pts.reset(new TPointCloud);
}
void Add(const FeaturePoints& rhs) {

4
src/helper/helper.cc
View File

@ -6,8 +6,4 @@ float GetTime(const TPoint& pt) { return pt.time - GetScanId(pt); }
int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
double PointLinePair::DistPointToLine() const { return 0.0; }
double PointPlanePair::DistPointToPlane() const { return 0.0; }
} // oh_my_loam

7
src/helper/helper.h
View File

@ -17,7 +17,8 @@ struct PointLinePair {
};
Line line;
PointLinePair(const TPoint& pt, const Line& line) : pt(pt), line(line) {}
double DistPointToLine() const;
PointLinePair(const TPoint& pt, const TPoint& pt1, const TPoint& pt2)
: pt(pt), line(pt1, pt2) {}
};
struct PointPlanePair {
@ -30,7 +31,9 @@ struct PointPlanePair {
};
Plane plane;
PointPlanePair(const TPoint& pt, const Plane& plane) : pt(pt), plane(plane) {}
double DistPointToPlane() const;
PointPlanePair(const TPoint& pt, const TPoint& pt1, const TPoint& pt2,
const TPoint& pt3)
: pt(pt), plane(pt1, pt2, pt3) {}
};
} // oh_my_loam

70
src/odometry/odometry.cc
View File

@ -1,32 +1,52 @@
#include "odometry.h"
#include "solver/solver.h"
namespace oh_my_loam {
namespace {
int kNearbyScanNum = 2;
double kDistSquareThresh = 25;
}
size_t kMinMatchNum = 10;
} // namespace
bool Odometry::Init(const YAML::Node& config) {
config_ = config;
kdtree_surf_pts.reset(new pcl::KdTreeFLANN<TPoint>);
kdtree_corn_pts.reset(new pcl::KdTreeFLANN<TPoint>);
kdtree_surf_pts_.reset(new pcl::KdTreeFLANN<TPoint>);
kdtree_corn_pts_.reset(new pcl::KdTreeFLANN<TPoint>);
return true;
}
void Odometry::Process(const FeaturePoints& feature) {
void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
if (!is_initialized) {
is_initialized = true;
UpdatePre(feature);
*pose = pose_curr2world_;
return;
}
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
AssociateCornPoints(*feature.sharp_corner_pts, corn_pts_pre_, pl_pairs,
kDistSquareThresh);
AssociateSurfPoints(*feature.flat_surf_pts, surf_pts_pre_, pp_pairs,
kDistSquareThresh);
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
AssociateCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
kDistSquareThresh);
AssociateSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs,
kDistSquareThresh);
if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
AWARN << "Too less correspondence";
}
double* q = pose_curr2last_.q().coeffs().data();
double* p = pose_curr2last_.p().data();
PoseSolver solver(q, p);
for (const auto& pair : pl_pairs) {
solver.AddPointLinePair(pair, GetTime(pair.pt));
}
for (const auto& pair : pp_pairs) {
solver.AddPointPlanePair(pair, GetTime(pair.pt));
}
solver.Solve();
pose_curr2last_ = Pose3D(q, p);
}
pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
*pose = pose_curr2world_;
UpdatePre(feature);
}
@ -34,17 +54,17 @@ void Odometry::AssociateCornPoints(const TPointCloud& src,
const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs,
double dist_thresh) const {
kdtree_corn_pts_->setInputCloud(tgt);
for (const query_pt : src) {
kdtree_corn_pts_->setInputCloud(tgt.makeShared());
for (const auto& query_pt : src) {
std::vector<int> indices;
std::vector<float> dists;
kdtree_corn_pts->nearestKSearch(query_pt, 1, indices, dists);
kdtree_corn_pts_->nearestKSearch(query_pt, 1, indices, dists);
if (dists[0] >= dist_thresh) continue;
Point pt1 = tgt.points[indices[0]];
TPoint pt1 = tgt.points[indices[0]];
int pt2_idx = -1;
double min_dist_pt2_squre = dist_thresh * dist_thresh;
int query_pt_scan_id = GetScanId(query_pt);
for (int i = indices[0] + 1; i < tgt.size(); ++i) {
for (int i = indices[0] + 1; i < static_cast<int>(tgt.size()); ++i) {
const auto pt = tgt.points[i];
int scan_id = GetScanId(pt);
if (scan_id <= query_pt_scan_id) continue;
@ -68,7 +88,7 @@ void Odometry::AssociateCornPoints(const TPointCloud& src,
}
if (pt2_idx >= 0) {
TPoint pt2 = tgt.points[pt2_idx];
pairs->emplace_back(query_pt, {pt1, pt2});
pairs->emplace_back(query_pt, pt1, pt2);
}
}
}
@ -77,18 +97,18 @@ void Odometry::AssociateSurfPoints(const TPointCloud& src,
const TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs,
double dist_thresh) const {
kdtree_surf_pts_->setInputCloud(tgt);
for (const query_pt : src) {
kdtree_surf_pts_->setInputCloud(tgt.makeShared());
for (const auto& query_pt : src) {
std::vector<int> indices;
std::vector<float> dists;
kdtree_corn_pts->nearestKSearch(query_pt, 1, indices, dists);
kdtree_surf_pts_->nearestKSearch(query_pt, 1, indices, dists);
if (dists[0] >= dist_thresh) continue;
Point pt1 = tgt.points[indices[0]];
TPoint pt1 = tgt.points[indices[0]];
int pt2_idx = -1, pt3_idx = -1;
double min_dist_pt2_squre = dist_thresh * dist_thresh;
double min_dist_pt3_squre = dist_thresh * dist_thresh;
int query_pt_scan_id = GetScanId(query_pt);
for (int i = indices[0] + 1; i < tgt.size(); ++i) {
for (int i = indices[0] + 1; i < static_cast<int>(tgt.size()); ++i) {
const auto pt = tgt.points[i];
int scan_id = GetScanId(pt);
if (scan_id > query_pt_scan_id + kNearbyScanNum) break;
@ -97,7 +117,7 @@ void Odometry::AssociateSurfPoints(const TPointCloud& src,
pt2_idx = i;
min_dist_pt2_squre = dist_squre;
}
if (scan_id > query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
if (scan_id > query_pt_scan_id && dist_squre < min_dist_pt3_squre) {
pt3_idx = i;
min_dist_pt3_squre = dist_squre;
}
@ -113,7 +133,7 @@ void Odometry::AssociateSurfPoints(const TPointCloud& src,
pt2_idx = i;
min_dist_pt2_squre = dist_squre;
}
if (scan_id < query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
if (scan_id < query_pt_scan_id && dist_squre < min_dist_pt3_squre) {
pt3_idx = i;
min_dist_pt3_squre = dist_squre;
}
@ -121,7 +141,7 @@ void Odometry::AssociateSurfPoints(const TPointCloud& src,
}
if (pt2_idx >= 0 && pt3_idx >= 0) {
TPoint pt2 = tgt.points[pt2_idx], pt3 = tgt.points[pt3_idx];
pairs->emplace_back(query_pt, {pt1, pt2, pt3});
pairs->emplace_back(query_pt, pt1, pt2, pt3);
}
}
}

16
src/odometry/odometry.h
View File

@ -1,8 +1,6 @@
#pragma once
#include <pcl/kdtree/kdtree_flann.h>
#include <eigen3/Eigen/Dense>
#include <extractor/feature_points.h>
#include "common.h"
#include "helper/helper.h"
@ -16,7 +14,7 @@ class Odometry {
bool Init(const YAML::Node& config);
void Process(const FeaturePoints& feature);
void Process(const FeaturePoints& feature, Pose3D* const pose);
protected:
void UpdatePre(const FeaturePoints& feature);
@ -28,15 +26,19 @@ class Odometry {
std::vector<PointPlanePair>* const pairs,
double dist_thresh) const;
Pose3D pose_curr2world_;
Pose3D pose_curr2last_;
TPointCloudPtr surf_pts_pre_;
TPointCloudPtr corn_pts_pre_;
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts;
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts;
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts_;
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts_;
bool is_initialized = false;
YAML::Node config_;
DISALLOW_COPY_AND_ASSIGN(Odometry)
};
} // namespace oh_my_loam

9
src/oh_my_loam.cc
View File

@ -15,6 +15,11 @@ bool OhMyLoam::Init() {
AERROR << "Failed to initialize extractor";
return false;
}
odometry_.reset(new Odometry);
if (!odometry_->Init(config["odometry_config"])) {
AERROR << "Failed to initialize odometry";
return false;
}
return true;
}
@ -25,6 +30,10 @@ void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) {
<< cloud->size();
FeaturePoints feature_points;
extractor_->Process(*cloud, &feature_points);
Pose3D pose;
odometry_->Process(feature_points, &pose);
poses_.emplace_back(pose);
AINFO << pose.ToString();
}
void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in,

5
src/oh_my_loam.h
View File

@ -1,9 +1,8 @@
#pragma once
#include <yaml-cpp/yaml.h>
#include "common.h"
#include "extractor/base_extractor.h"
#include "odometry/odometry.h"
namespace oh_my_loam {
@ -17,10 +16,12 @@ class OhMyLoam {
private:
std::unique_ptr<Extractor> extractor_{nullptr};
std::unique_ptr<Odometry> odometry_{nullptr};
// remove outliers: nan and very close points
void RemoveOutliers(const PointCloud& cloud_in,
PointCloud* const cloud_out) const;
std::vector<Pose3D> poses_;
DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
};

90
src/solver/cost_function.h Normal file
View File

@ -0,0 +1,90 @@
#pragma once
#include "ceres/ceres.h"
#include "common.h"
#include "helper/helper.h"
namespace oh_my_loam {
class PointLineCostFunction {
public:
PointLineCostFunction(const PointLinePair& pair, double time)
: pair_(pair), time_(time){};
template <typename T>
bool operator()(const T* const q, const T* const p, T* residual) const;
static ceres::CostFunction* Create(const PointLinePair& pair, double time) {
return new ceres::AutoDiffCostFunction<PointLineCostFunction, 1, 4, 3>(
new PointLineCostFunction(pair, time));
}
private:
PointLinePair pair_;
double time_;
DISALLOW_COPY_AND_ASSIGN(PointLineCostFunction)
};
class PointPlaneCostFunction {
public:
PointPlaneCostFunction(const PointPlanePair& pair, double time)
: pair_(pair), time_(time){};
template <typename T>
bool operator()(const T* const q, const T* const p, T* residual) const;
static ceres::CostFunction* Create(const PointPlanePair& pair, double time) {
return new ceres::AutoDiffCostFunction<PointPlaneCostFunction, 1, 4, 3>(
new PointPlaneCostFunction(pair, time));
}
private:
PointPlanePair pair_;
double time_;
DISALLOW_COPY_AND_ASSIGN(PointPlaneCostFunction)
};
template <typename T>
bool PointLineCostFunction::operator()(const T* const q, const T* const p,
T* residual) const {
const auto pt = pair_.pt, pt1 = pair_.line.pt1, pt2 = pair_.line.pt2;
Eigen::Matrix<T, 3, 1> pnt(T(pt.x), T(pt.y), T(pt.z));
Eigen::Matrix<T, 3, 1> pnt1(T(pt1.x), T(pt1.y), T(pt1.z));
Eigen::Matrix<T, 3, 1> pnt2(T(pt2.x), T(pt2.y), T(pt2.z));
Eigen::Quaternion<T> r(q[3], q[0], q[1], q[2]);
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
r = q_identity.slerp(T(time_), r);
Eigen::Matrix<T, 3, 1> t(T(time_) * p[0], T(time_) * p[1], T(time_) * p[2]);
Eigen::Matrix<T, 3, 1> pnt_n = r * pnt + t;
// norm of cross product: triangle area x 2
T area = (pnt_n - pnt1).cross(pnt2 - pnt1).norm() * 0.5;
T base_length = (pnt2 - pnt1).norm();
residual[0] = area / base_length;
return true;
}
template <typename T>
bool PointPlaneCostFunction::operator()(const T* const q, const T* const p,
T* residual) const {
const auto &pt = pair_.pt, &pt1 = pair_.plane.pt1, &pt2 = pair_.plane.pt2,
&pt3 = pair_.plane.pt3;
Eigen::Matrix<T, 3, 1> pnt(T(pt.x), T(pt.y), T(pt.z));
Eigen::Matrix<T, 3, 1> pnt1(T(pt1.x), T(pt1.y), T(pt1.z));
Eigen::Matrix<T, 3, 1> pnt2(T(pt2.x), T(pt2.y), T(pt2.z));
Eigen::Matrix<T, 3, 1> pnt3(T(pt3.x), T(pt3.y), T(pt3.z));
Eigen::Quaternion<T> r(q[3], q[0], q[1], q[2]);
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
r = q_identity.slerp(T(time_), r);
Eigen::Matrix<T, 3, 1> t(T(time_) * p[0], T(time_) * p[1], T(time_) * p[2]);
Eigen::Matrix<T, 3, 1> pnt_n = r * pnt + t;
Eigen::Matrix<T, 3, 1> normal = (pnt2 - pnt1).cross(pnt3 - pnt1).normalized();
residual[0] = (pnt_n - pnt1).dot(normal);
return true;
}
} // oh_my_loam

33
src/solver/solver.cc
View File

@ -2,6 +2,37 @@
namespace oh_my_loam {
//
namespace {
double kHuberLossScale = 0.1;
}
PoseSolver::PoseSolver(double *q, double *p) : q_(q), p_(p) {
loss_function_ = new ceres::HuberLoss(kHuberLossScale);
problem_.AddParameterBlock(q_, 4,
new ceres::EigenQuaternionParameterization());
problem_.AddParameterBlock(p_, 3);
}
void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) {
ceres::CostFunction *cost_function =
PointLineCostFunction::Create(pair, time);
problem_.AddResidualBlock(cost_function, loss_function_, q_, p_);
}
void PoseSolver::AddPointPlanePair(const PointPlanePair &pair, double time) {
ceres::CostFunction *cost_function =
PointPlaneCostFunction::Create(pair, time);
problem_.AddResidualBlock(cost_function, loss_function_, q_, p_);
}
void PoseSolver::Solve(int max_iter_num, bool verbose) {
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = max_iter_num;
options.minimizer_progress_to_stdout = verbose;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem_, &summary);
AINFO_IF(verbose) << summary.BriefReport();
}
} // oh_my_loam

22
src/solver/solver.h
View File

@ -1,19 +1,27 @@
#pragma once
#include "ceres/ceres.h"
#include "cost_function.h"
namespace oh_my_loam {
class Solver {
class PoseSolver {
public:
Solver() = default;
PoseSolver(double* q, double* p);
void SetInitialGuess(double* param_q, double* param_p);
void AddPointLinePair(const PointLinePair& pair, double time);
void AddPointPlanePair(const PointPlanePair& pair, double time);
void Solve(int max_iter_num = 5, bool verbose = false);
private:
ceres::Problem problem_;
protected:
std::unique_ptr<ceres::Problem> problem_;
ceres::LocalParameterization* parameterization_;
ceres::LossFunction* loss_function_;
double *q_, *p_;
DISALLOW_COPY_AND_ASSIGN(PoseSolver)
};
} // oh_my_loam