oh_my_loam/src/solver/cost_function.h

92 lines
3.1 KiB
C++

#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, 3, 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
Eigen::Matrix<T, 3, 1> area = (pnt_n - pnt1).cross(pnt_n - pnt2) * 0.5;
T base_length = (pnt2 - pnt1).norm();
residual[0] = area[0] / base_length;
residual[1] = area[1] / base_length;
residual[2] = area[2] / 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