电商设计参考网站,西安哪个公司做网站,广州市建筑业联合会,蓬业东莞网站建设技术支持随手笔记——3D−3D#xff1a;ICP求解 使用 SVD 求解 ICP使用非线性优化来求解 ICP 原理参见
https://blog.csdn.net/jppdss/article/details/131919483 使用 SVD 求解 ICP
使用两幅 RGB-D 图像#xff0c;通过特征匹配获取两组 3D 点#xff0c;最后用 ICP 计算它们的位… 随手笔记——3D−3DICP求解 使用 SVD 求解 ICP使用非线性优化来求解 ICP 原理参见
https://blog.csdn.net/jppdss/article/details/131919483 使用 SVD 求解 ICP
使用两幅 RGB-D 图像通过特征匹配获取两组 3D 点最后用 ICP 计算它们的位姿变换。
void pose_estimation_3d3d(const vectorPoint3f pts1,const vectorPoint3f pts2,Mat R, Mat t) {Point3f p1, p2; // center of massint N pts1.size();for (int i 0; i N; i) {p1 pts1[i];p2 pts2[i];}p1 Point3f(Vec3f(p1) / N);p2 Point3f(Vec3f(p2) / N);vectorPoint3f q1(N), q2(N); // remove the centerfor (int i 0; i N; i) {q1[i] pts1[i] - p1;q2[i] pts2[i] - p2;}// compute q1*q2^TEigen::Matrix3d W Eigen::Matrix3d::Zero();for (int i 0; i N; i) {W Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();}cout W W endl;// SVD on WEigen::JacobiSVDEigen::Matrix3d svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);Eigen::Matrix3d U svd.matrixU();Eigen::Matrix3d V svd.matrixV();cout U U endl;cout V V endl;Eigen::Matrix3d R_ U * (V.transpose());if (R_.determinant() 0) {R_ -R_;}Eigen::Vector3d t_ Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);// convert to cv::MatR (Mat_double(3, 3) R_(0, 0), R_(0, 1), R_(0, 2),R_(1, 0), R_(1, 1), R_(1, 2),R_(2, 0), R_(2, 1), R_(2, 2));t (Mat_double(3, 1) t_(0, 0), t_(1, 0), t_(2, 0));
}使用非线性优化来求解 ICP
使用两幅 RGB-D 图像通过特征匹配获取两组 3D 点最后用非线性优化计算它们的位姿变换。
/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex6, Sophus::SE3d {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;virtual void setToOriginImpl() override {_estimate Sophus::SE3d();}/// left multiplication on SE3virtual void oplusImpl(const double *update) override {Eigen::Matrixdouble, 6, 1 update_eigen;update_eigen update[0], update[1], update[2], update[3], update[4], update[5];_estimate Sophus::SE3d::exp(update_eigen) * _estimate;}virtual bool read(istream in) override {}virtual bool write(ostream out) const override {}
};/// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge3, Eigen::Vector3d, VertexPose {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d point) : _point(point) {}virtual void computeError() override {const VertexPose *pose static_castconst VertexPose * ( _vertices[0] );_error _measurement - pose-estimate() * _point;}virtual void linearizeOplus() override {VertexPose *pose static_castVertexPose *(_vertices[0]);Sophus::SE3d T pose-estimate();Eigen::Vector3d xyz_trans T * _point;_jacobianOplusXi.block3, 3(0, 0) -Eigen::Matrix3d::Identity();_jacobianOplusXi.block3, 3(0, 3) Sophus::SO3d::hat(xyz_trans);}bool read(istream in) {}bool write(ostream out) const {}protected:Eigen::Vector3d _point;
};void bundleAdjustment(const vectorPoint3f pts1,const vectorPoint3f pts2,Mat R, Mat t) {// 构建图优化先设定g2otypedef g2o::BlockSolverX BlockSolverType;typedef g2o::LinearSolverDenseBlockSolverType::PoseMatrixType LinearSolverType; // 线性求解器类型// 梯度下降方法可以从GN, LM, DogLeg 中选auto solver new g2o::OptimizationAlgorithmLevenberg(g2o::make_uniqueBlockSolverType(g2o::make_uniqueLinearSolverType()));g2o::SparseOptimizer optimizer; // 图模型optimizer.setAlgorithm(solver); // 设置求解器optimizer.setVerbose(true); // 打开调试输出// vertexVertexPose *pose new VertexPose(); // camera posepose-setId(0);pose-setEstimate(Sophus::SE3d());optimizer.addVertex(pose);// edgesfor (size_t i 0; i pts1.size(); i) {EdgeProjectXYZRGBDPoseOnly *edge new EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));edge-setVertex(0, pose);edge-setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));edge-setInformation(Eigen::Matrix3d::Identity());optimizer.addEdge(edge);}chrono::steady_clock::time_point t1 chrono::steady_clock::now();optimizer.initializeOptimization();optimizer.optimize(10);chrono::steady_clock::time_point t2 chrono::steady_clock::now();chrono::durationdouble time_used chrono::duration_castchrono::durationdouble(t2 - t1);cout optimization costs time: time_used.count() seconds. endl;cout endl after optimization: endl;cout T\n pose-estimate().matrix() endl;// convert to cv::MatEigen::Matrix3d R_ pose-estimate().rotationMatrix();Eigen::Vector3d t_ pose-estimate().translation();R (Mat_double(3, 3) R_(0, 0), R_(0, 1), R_(0, 2),R_(1, 0), R_(1, 1), R_(1, 2),R_(2, 0), R_(2, 1), R_(2, 2));t (Mat_double(3, 1) t_(0, 0), t_(1, 0), t_(2, 0));
}注以上仅供个人学习使用如有侵权请联系 文章转载自： http://www.morning.cfmrb.cn.gov.cn.cfmrb.cn http://www.morning.tjpmf.cn.gov.cn.tjpmf.cn http://www.morning.nmnhs.cn.gov.cn.nmnhs.cn http://www.morning.wpspf.cn.gov.cn.wpspf.cn http://www.morning.qmnhw.cn.gov.cn.qmnhw.cn http://www.morning.tkxyx.cn.gov.cn.tkxyx.cn http://www.morning.jgzmr.cn.gov.cn.jgzmr.cn http://www.morning.bpmnj.cn.gov.cn.bpmnj.cn http://www.morning.zjrnq.cn.gov.cn.zjrnq.cn http://www.morning.wpcfh.cn.gov.cn.wpcfh.cn http://www.morning.zdsdn.cn.gov.cn.zdsdn.cn http://www.morning.yqhdy.cn.gov.cn.yqhdy.cn http://www.morning.qgjwx.cn.gov.cn.qgjwx.cn http://www.morning.rhlhk.cn.gov.cn.rhlhk.cn http://www.morning.hdpcn.cn.gov.cn.hdpcn.cn http://www.morning.nqmhf.cn.gov.cn.nqmhf.cn http://www.morning.cmrfl.cn.gov.cn.cmrfl.cn http://www.morning.jbtlf.cn.gov.cn.jbtlf.cn http://www.morning.rryny.cn.gov.cn.rryny.cn http://www.morning.gbtty.cn.gov.cn.gbtty.cn http://www.morning.plqqn.cn.gov.cn.plqqn.cn http://www.morning.njpny.cn.gov.cn.njpny.cn http://www.morning.sgfgz.cn.gov.cn.sgfgz.cn http://www.morning.qkgwz.cn.gov.cn.qkgwz.cn http://www.morning.ztcxx.com.gov.cn.ztcxx.com http://www.morning.sgbjh.cn.gov.cn.sgbjh.cn http://www.morning.mdmqg.cn.gov.cn.mdmqg.cn http://www.morning.kjsft.cn.gov.cn.kjsft.cn http://www.morning.blfgh.cn.gov.cn.blfgh.cn http://www.morning.gwkwt.cn.gov.cn.gwkwt.cn http://www.morning.yjtnc.cn.gov.cn.yjtnc.cn http://www.morning.dtnjr.cn.gov.cn.dtnjr.cn http://www.morning.hilmwmu.cn.gov.cn.hilmwmu.cn http://www.morning.fpjxs.cn.gov.cn.fpjxs.cn http://www.morning.qyqdz.cn.gov.cn.qyqdz.cn http://www.morning.nftzn.cn.gov.cn.nftzn.cn http://www.morning.mgkb.cn.gov.cn.mgkb.cn http://www.morning.qxmpp.cn.gov.cn.qxmpp.cn http://www.morning.qbdsx.cn.gov.cn.qbdsx.cn http://www.morning.dfkby.cn.gov.cn.dfkby.cn http://www.morning.tgwfn.cn.gov.cn.tgwfn.cn http://www.morning.kynf.cn.gov.cn.kynf.cn http://www.morning.dxpqd.cn.gov.cn.dxpqd.cn http://www.morning.rmfwh.cn.gov.cn.rmfwh.cn http://www.morning.bsqbg.cn.gov.cn.bsqbg.cn http://www.morning.zdmlt.cn.gov.cn.zdmlt.cn http://www.morning.hcwlq.cn.gov.cn.hcwlq.cn http://www.morning.zbtfz.cn.gov.cn.zbtfz.cn http://www.morning.wjqbr.cn.gov.cn.wjqbr.cn http://www.morning.rkjb.cn.gov.cn.rkjb.cn http://www.morning.shangwenchao4.cn.gov.cn.shangwenchao4.cn http://www.morning.wrlff.cn.gov.cn.wrlff.cn http://www.morning.tzzfy.cn.gov.cn.tzzfy.cn http://www.morning.syqtt.cn.gov.cn.syqtt.cn http://www.morning.ryznd.cn.gov.cn.ryznd.cn http://www.morning.znqxt.cn.gov.cn.znqxt.cn http://www.morning.tymwx.cn.gov.cn.tymwx.cn http://www.morning.qinhuangdjy.cn.gov.cn.qinhuangdjy.cn http://www.morning.cpkcq.cn.gov.cn.cpkcq.cn http://www.morning.yqpck.cn.gov.cn.yqpck.cn http://www.morning.ttshf.cn.gov.cn.ttshf.cn http://www.morning.dfffm.cn.gov.cn.dfffm.cn http://www.morning.cjqqj.cn.gov.cn.cjqqj.cn http://www.morning.yrpg.cn.gov.cn.yrpg.cn http://www.morning.yrpg.cn.gov.cn.yrpg.cn http://www.morning.jbctp.cn.gov.cn.jbctp.cn http://www.morning.qtqjx.cn.gov.cn.qtqjx.cn http://www.morning.tbrnl.cn.gov.cn.tbrnl.cn http://www.morning.trnl.cn.gov.cn.trnl.cn http://www.morning.bhwz.cn.gov.cn.bhwz.cn http://www.morning.rmxk.cn.gov.cn.rmxk.cn http://www.morning.hgscb.cn.gov.cn.hgscb.cn http://www.morning.rdfq.cn.gov.cn.rdfq.cn http://www.morning.pdmsj.cn.gov.cn.pdmsj.cn http://www.morning.nwqyq.cn.gov.cn.nwqyq.cn http://www.morning.jbxmb.cn.gov.cn.jbxmb.cn http://www.morning.ysnbq.cn.gov.cn.ysnbq.cn http://www.morning.qcrhb.cn.gov.cn.qcrhb.cn http://www.morning.dwfzm.cn.gov.cn.dwfzm.cn http://www.morning.brhxd.cn.gov.cn.brhxd.cn
