ceres实现针孔相机bundle adjustment

参考ceres的tutorial
http://ceres-solver.org/nnls_tutorial.html#bundle-adjustment
实现针孔相机bundle adjustment
主要工作如下：
（1）生成理想观测数据
（2）将生成理想的数据作为BD的输入源
（3）将投影模型改为无畸变的针孔模型

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include<time.h>#include <unistd.h>
#include<iostream>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <tf/transform_broadcaster.h>
#include <cmath>
#include <cstdio>
#include <iostream>
#include "ceres/ceres.h"
#include "ceres/rotation.h"
using namespace cv;
using namespace std;
using namespace Eigen;
struct ObsIndexPair{int camIndex;int pointIndex;
};
// Templated pinhole camera model for used with Ceres.  The camera is
// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
// focal length and 2 for radial distortion. The principal point is not modeled
// (i.e. it is assumed be located at the image center).
struct SnavelyReprojectionError {SnavelyReprojectionError(double observed_x, double observed_y): observed_x(observed_x), observed_y(observed_y) {}template <typename T>bool operator()(const T* const camera,const T* const point,const T* const K,T* residuals) const {// camera[0,1,2] are the angle-axis rotation.T p[3];ceres::AngleAxisRotatePoint(camera, point, p);// camera[3,4,5] are the translation.p[0] += camera[3];p[1] += camera[4];p[2] += camera[5];// Compute the center of distortion. The sign change comes from// the camera model that Noah Snavely's Bundler assumes, whereby// the camera coordinate system has a negative z axis.T xp = p[0] / p[2];T yp = p[1] / p[2];// Apply second and fourth order radial distortion.const T& fx = K[0];const T& fy = K[1];const T& cx = K[2];const T& cy = K[3];//    // Compute final projected point position.T predicted_x = fx * xp+cx;T predicted_y = fy * yp+cy;// The error is the difference between the predicted and observed position.residuals[0] = predicted_x - observed_x;residuals[1] = predicted_y - observed_y;return true;}// Factory to hide the construction of the CostFunction object from// the client code.static ceres::CostFunction* Create(const double observed_x,const double observed_y) {return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError,2,6,3,4>(new SnavelyReprojectionError(observed_x, observed_y)));}double observed_x;double observed_y;
};int main( int argc, char** argv )
{ros::init(argc, argv, "projecttest");float len=8;//1.1 get para intrinsicfloat fx = 200,fy=200, w = 2000, h = 2000;float cx=w/2;float cy=h/2;cv::Mat K = (Mat_<float>(3, 3) <<fx, 0, cx,0, fy, cy,0, 0, 1);float k1=0.0,k2=0,k3=0.0,k4=0.0,k5=0.0;cv::Mat coff=(Mat_<float>(1,4) <<k1,k2,k3,k4,k5);//2 get camcv::Mat cv_R12 = (Mat_<float>(3, 3) <<0, 0, 1,-1, 0, 0,0, -1, 0);cv::Mat cv_t12 = (Mat_<float>(3, 1) <<0, 0, len/2);//tvector<Mat> vCams;for(int i=0;i<4;i++){Mat T = cv::Mat::eye(4,4,cv_R12.type());cv_R12.copyTo(T.rowRange(0,3).colRange(0,3));cv::Mat cv_ti = (Mat_<float>(3, 1) <<0, 0, (float) i);cv_ti.copyTo(T.rowRange(0,3).col(3));vCams.push_back(T);}//3.get 3d point//  //x>0 ABCD EFGHPoint3f cube_center={len,0,len/2};Point3f A={cube_center.x-len/2,cube_center.y+len/2,cube_center.z-len/2};Point3f B={cube_center.x-len/2,cube_center.y-len/2,cube_center.z-len/2};Point3f C={cube_center.x+len/2,cube_center.y-len/2,cube_center.z-len/2};Point3f D={cube_center.x+len/2,cube_center.y+len/2,cube_center.z-len/2};cout<<"ABCD"<<A<< B<<C<<D<<endl;vector<Point3f> vp3d;vp3d.push_back(A);vp3d.push_back(B);vp3d.push_back(C);vp3d.push_back(D);//4.get project point2fint num_observations=0;vector<ObsIndexPair> vIndexPair;vector<pair<ObsIndexPair,Point2f>> sObs;for (size_t i=0;i<vCams.size();i++) {Mat T=vCams[i];cv::Mat R = T.rowRange(0,3).colRange(0,3);cv::Mat t = T.rowRange(0,3).col(3);Mat rvec;cv::Rodrigues(R,rvec);//cout<<"rvec"<<rvec<<"t"<<t<<endl;vector<Point2f> prj2d;cv::projectPoints(vp3d,rvec,t,K,coff,prj2d);for(size_t j=0;j<prj2d.size();j++){num_observations++;ObsIndexPair pair;pair.camIndex=(int)i;pair.pointIndex=(int)j;// vIndexPair.push_back(pair);sObs.push_back(make_pair(pair,prj2d[j]));}}
//    vector<pair<ObsIndexPair,Point2f>>::iterator it;
//    for(it=sObs.begin();it!=sObs.end();it++)
//    {
//        cout<<"it camIndex"<<(it->first).camIndex<<endl;
//        cout<<"it pointIndex"<<(it->first).pointIndex<<endl;
//        cout<<" x= "<<(it->second).x<<endl;
//        cout<<" y= "<<(it->second).y<<endl;
//    }ceres::Problem problem;double* ppoints=new double[3*vp3d.size()];for(size_t i=0;i<vp3d.size();i++){double* p=ppoints+3*i;*(p)=vp3d[i].x;*(p+1)=vp3d[i].y;*(p+2)=vp3d[i].z;}double* pcams=new double[6*vCams.size()];double* inPara=new double[4];*inPara=fx;*(inPara+1)=fy;*(inPara+2)=cx;*(inPara+3)=cy;for(size_t i=0;i<vCams.size();i++){double* camPara=pcams+6*i;Mat T=vCams[i];cv::Mat R = T.rowRange(0,3).colRange(0,3);cv::Mat t = T.rowRange(0,3).col(3);Mat rvec;cv::Rodrigues(R,rvec);*(camPara)=rvec.at<float>(0,0);*(camPara+1)=rvec.at<float>(1,0);*(camPara+2)=rvec.at<float>(2,0);*(camPara+3)=t.at<float>(0,0);*(camPara+4)=t.at<float>(1,0);*(camPara+5)=t.at<float>(2,0);}double* point0=ppoints;double* camera0=pcams;std::cout<<"before bd point 0 pos x="<<*(point0)<<" y="<<*(point0+1)<<" z="<<*(point0+2)<<std::endl;std::cout<<"before bd camera 0 pos rx="<<*(camera0+0)*180/M_PI<<" ry="<<*(camera0+1)*180/M_PI<<" rz"<<*(camera0+2)*180/M_PI<<std::endl;std::cout<<"before bd camera 0 pos x="<<*(camera0+3)<<" y="<<*(camera0+4)<<" z"<<*(camera0+5)<<std::endl;for (int i = 0; i < num_observations; ++i){// Each Residual block takes a point and a camera as input and outputs a 2// dimensional residual. Internally, the cost function stores the observed// image location and compares the reprojection against the observation.ceres::CostFunction* cost_function =SnavelyReprojectionError::Create(sObs[i].second.x,sObs[i].second.y);int pointIndex= sObs[i].first.pointIndex;double* point=ppoints+3*pointIndex;int camIndex=sObs[i].first.camIndex;double* camPara=pcams+6*camIndex;problem.AddResidualBlock(cost_function,NULL /* squared loss */,camPara,point,inPara);}// Make Ceres automatically detect the bundle structure. Note that the// standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower// for standard bundle adjustment problems.ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_SCHUR;options.minimizer_progress_to_stdout = true;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);std::cout<<"after bd point 0 pos x="<<*(point0)<<" y="<<*(point0+1)<<" z="<<*(point0+2)<<std::endl;std::cout<<"after bd camera 0 pos rx="<<*(camera0+0)*180/M_PI<<" ry="<<*(camera0+1)*180/M_PI<<" rz"<<*(camera0+2)*180/M_PI<<std::endl;std::cout<<"after bd camera 0 pos x="<<*(camera0+3)<<" y="<<*(camera0+4)<<" z"<<*(camera0+5)<<std::endl;return 0;
}

ceres实现针孔相机bundle adjustment相关推荐

ceres实现鱼眼相机模型bundle adjustment
本文主要工作: (1)投影模型改为鱼眼相机畸变模型 (2)具有数学公式计算的template类的coding #include<iostream> #include<eigen3/E ...
Bundle Adjustment原理及应用（附实战代码）
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达虽然现在的轮子很多,但我们在使用过程中会碰到很多问题,而我们经常不知道从哪里下手,说明轮子不是你造的你 ...
矩阵求逆c语言实现_[V-SLAM] Bundle Adjustment 实现
SLAM问题的后端有主要有滤波和优化两种方案.目前,普遍认为优化的方法在精度上要超过滤波方法,因为它可以进行多次的线性化.近年来出现的SLAM算法也大都是基于优化的算法(如ORB-SLAM.DSO等) ...
OpenCV实现SfM（四）：Bundle Adjustment
http://blog.csdn.net/AIchipmunk/article/details/52433884 OpenCV实现SfM(四):Bundle Adjustment 标签: opencv ...
SLAM专题（10）- 最小化重投影误差与Bundle Adjustment （BA）
在SFM(structure from motion)的计算中BA(Bundle Adjustment)作为最后一步优化具有很重要的作用,在近几年兴起的基于图的SLAM(simultaneous lo ...
ORB_SLAM2代码阅读(5)——Bundle Adjustment
ORB_SLAM2代码阅读(5)--Bundle Adjustment 1. 说明 2. Bundle Adjustment(BA)的物理意义 3. BA的数学表达 4. BA的求解方法 4.1 最速 ...
SLAM笔记（五）光束平差法(Bundle Adjustment)
1.什么是光束平差法前边的八点法,五点法等可以求出闭式解的前提是已经知道确切的点对.但实际情况中往往存在大量的噪声,点与点不是精确地对应甚至出现一些错误匹配. 光束平差法由Bundle Adjus ...
史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解(Bundle Adjustment)
本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证{\color{blue}{文末正下方中心}提供了本人 \co ...
[论文笔记|VIO]ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM
文章目录摘要 1. 介绍 2. Related Work 3. Framework 3.1. Constraint Functions 3.2. Local and Global Optimizat ...
Bundle Adjustment (BA) in vSLAM or SFM
文章目录 Overview BA as a NonLinear Least Squares Problem Solved with LM JTJ\mathbf{J}^T \mathbf{J}JTJ s ...

ceres实现针孔相机bundle adjustment

ceres实现针孔相机bundle adjustment相关推荐

最新文章

热门文章