网站建设公司的案例,wordpress菜单的作用,龙岗网站建设设计服务,广东省建设集团有限公司随手笔记——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)); }注以上仅供个人学习使用如有侵权请联系