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