parent
24220189b7
commit
72b2d2e155
@ -1,64 +0,0 @@
|
|||||||
#include <opencv2/opencv.hpp>
|
|
||||||
#include <iostream>
|
|
||||||
#include <map>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
// 定义相机节点
|
|
||||||
struct CameraNode {
|
|
||||||
int cameraId; // 相机 ID
|
|
||||||
cv::Mat R; // 旋转矩阵
|
|
||||||
cv::Mat t; // 平移向量
|
|
||||||
cv::Mat K; // 内参矩阵
|
|
||||||
};
|
|
||||||
|
|
||||||
// 定义三维点节点
|
|
||||||
struct PointNode {
|
|
||||||
int pointId; // 三维点 ID
|
|
||||||
cv::Point3f X; // 三维点坐标
|
|
||||||
};
|
|
||||||
|
|
||||||
// 定义观测边
|
|
||||||
struct ObservationEdge {
|
|
||||||
int cameraId; // 相机 ID
|
|
||||||
int pointId; // 三维点 ID
|
|
||||||
cv::Point2f x; // 观测到的二维点坐标
|
|
||||||
};
|
|
||||||
|
|
||||||
// 定义 Bundle Adjustment 图结构
|
|
||||||
class BundleAdjustmentGraph {
|
|
||||||
public:
|
|
||||||
// 添加相机节点
|
|
||||||
void addCameraNode(const CameraNode& camera) {
|
|
||||||
cameras[camera.cameraId] = camera;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 添加三维点节点
|
|
||||||
void addPointNode(const PointNode& point) {
|
|
||||||
points[point.pointId] = point;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 添加观测边
|
|
||||||
void addObservationEdge(const ObservationEdge& edge) {
|
|
||||||
observations.push_back(edge);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 获取相机节点
|
|
||||||
CameraNode getCameraNode(int cameraId) {
|
|
||||||
return cameras[cameraId];
|
|
||||||
}
|
|
||||||
|
|
||||||
// 获取三维点节点
|
|
||||||
PointNode getPointNode(int pointId) {
|
|
||||||
return points[pointId];
|
|
||||||
}
|
|
||||||
|
|
||||||
// 获取观测边
|
|
||||||
std::vector<ObservationEdge> getObservationEdges() {
|
|
||||||
return observations;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
std::map<int, CameraNode> cameras; // 相机节点集合
|
|
||||||
std::map<int, PointNode> points; // 三维点节点集合
|
|
||||||
std::vector<ObservationEdge> observations; // 观测边集合
|
|
||||||
};
|
|
||||||
@ -0,0 +1,67 @@
|
|||||||
|
#include "Arith_BATask.h"
|
||||||
|
#include "ceres/ceres.h"
|
||||||
|
using namespace ceres;
|
||||||
|
|
||||||
|
struct CostFunctor {
|
||||||
|
template <typename T>
|
||||||
|
bool operator()(const T* const x, T* residual) const {
|
||||||
|
residual[0] = 10.0 - x[0];
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
BA_Task::BA_Task()
|
||||||
|
{
|
||||||
|
// 来个100帧
|
||||||
|
_imgVec.reserve(100);
|
||||||
|
}
|
||||||
|
|
||||||
|
BA_Task::~BA_Task()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
SINT32 BA_Task::addFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
|
||||||
|
{
|
||||||
|
// 缓存图像帧
|
||||||
|
if (img.enPixelFormat == GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_GRAY_Y8)
|
||||||
|
{
|
||||||
|
_imgVec.emplace_back(Mat(img.u32Height, img.u32Width, CV_8UC1, img.u64VirAddr[0]));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
return _imgVec.size();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void BA_Task::Test()
|
||||||
|
{
|
||||||
|
google::InitGoogleLogging("ceres");
|
||||||
|
|
||||||
|
// The variable to solve for with its initial value. It will be
|
||||||
|
// mutated in place by the solver.
|
||||||
|
double x = 0.5;
|
||||||
|
const double initial_x = x;
|
||||||
|
|
||||||
|
// Build the problem.
|
||||||
|
ceres::Problem problem;
|
||||||
|
|
||||||
|
// Set up the only cost function (also known as residual). This uses
|
||||||
|
// auto-differentiation to obtain the derivative (jacobian).
|
||||||
|
ceres::CostFunction* cost_function =
|
||||||
|
new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
|
||||||
|
problem.AddResidualBlock(cost_function, nullptr, &x);
|
||||||
|
|
||||||
|
// Run the solver!
|
||||||
|
ceres::Solver::Options options;
|
||||||
|
options.minimizer_progress_to_stdout = true;
|
||||||
|
ceres::Solver::Summary summary;
|
||||||
|
ceres::Solve(options, &problem, &summary);
|
||||||
|
|
||||||
|
std::cout << summary.BriefReport() << "\n";
|
||||||
|
std::cout << "x : " << initial_x << " -> " << x << "\n";
|
||||||
|
}
|
||||||
Loading…
Reference in new issue