上传新文件

main.cpp

+#include "Thirdparty/g2o/g2o/types/types_sba.h"
+#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
+#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
+#include "Thirdparty/g2o/g2o/core/sparse_optimizer.h"
+#include "Thirdparty/g2o/g2o/core/block_solver.h"
+#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
+
+using namespace std;
+using namespace Eigen;
+
+#include <iostream>
+#include <random>
+#include <chrono>
+#include <eigen3/Eigen/Dense>
+#include <unordered_map>
+#include <sophus/se3.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/eigen.hpp>
+
+//using Point = Eigen::Vector3d;
+
+//#include "g2o_opt_type.h"
+#include "so3.h"
+#include "types_six_dof_expmap_ours.h"
+
+struct sFrame
+{
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    sFrame(Eigen::Matrix4d Tcw): Tcw_(Tcw){}
+
+    Eigen::Matrix4d Tcw_;
+
+    //std::unordered_map<int, Eigen::Vector2d> per_point_feature_;
+    //std::unordered_map<int, Eigen::Vector4d> per_plane_feature_;
+    std::unordered_map<int,g2o::SO3> per_plane_feature_;
+    
+};
+
+
+
+const int nCameras = 2;
+const int nMapplanes = 4;
+const double Height = 1024;
+const double Width = 768;
+
+double fx = 1;//750;
+double fy = 1;//500;
+double cx = 0;//250;
+double cy = 0;//250;
+
+
+void CreateCameras(std::vector<sFrame>& vFrames, std::vector<Sophus::SE3d>& gtpose, int nCams);
+//void CreateSceneFeatures(const std::vector<sFrame>& vFrames, std::vector<QuatPlane>& vpGeometryFeatures);
+void CreateSceneFeatures(const std::vector<sFrame>& vFrames, std::vector<Eigen::Vector4d>& vpGeometryFeatures);
+//void FeatureDetection(const Eigen::Matrix3d& K, const std::vector<QuatPlane> &scenefeatures,std::vector<sFrame> &vframes, bool is_add_noise = true);
+void FeatureDetection(const Eigen::Matrix3d& K, const std::vector<Eigen::Vector4d> &scenefeatures,std::vector<sFrame> &vframes, bool is_add_noise = true);
+void AddNoise(std::vector<sFrame>& vFrames, std::vector<Eigen::Vector4d>& vpGeometryFeatures, double gs_noise, double cam_noise);
+
+using PointLineBlockSolver = g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1>>;
+using InitilizationLinearSolverEigen = g2o::LinearSolverEigen<PointLineBlockSolver::PoseMatrixType>;
+
+int main()
+{
+/*
+    double theta = 2 * M_PI / 6; // 1/4 圆弧
+
+    Eigen::Matrix3d R1,R2;
+    R1 = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
+    R2.setIdentity();
+    Eigen::Quaterniond q1(R1);
+    Eigen::Quaterniond q2(R2);
+    std::cout<<"q1 is "<<q1.coeffs()<<std::endl;
+    std::cout<<"q2 is "<<q2.coeffs()<<std::endl;
+
+
+    QuatPlane plane1(q1);
+    QuatPlane plane2(q2);
+    Eigen::Quaterniond q3;
+    QuatPlane plane3=QuatPlane::exp(QuatPlane::log(plane1._plane.inverse()*plane2._plane));
+    q3 = plane3._plane;
+    std::cout<<"q3 is"<<q3.coeffs()<<std::endl;
+*/
+    std::vector<sFrame> vGTCameras;
+    std::vector<Eigen::Vector4d> vGTGeomeryFeatures;
+    std::vector<Sophus::SE3d> gt_cam_pose;
+    CreateCameras(vGTCameras, gt_cam_pose, nCameras);
+    CreateSceneFeatures(vGTCameras, vGTGeomeryFeatures);
+
+    cv::Mat cv_K = (cv::Mat_<double>(3,3) << fx, 0,  cx, 0, fy, cy,0,  0,  1);
+    Eigen::Matrix3d eigen_K;
+    cv::cv2eigen(cv_K, eigen_K);
+   
+    FeatureDetection(eigen_K, vGTGeomeryFeatures, vGTCameras, false);
+
+    std::vector<Eigen::Vector4d> vNoisyGeomeryFeatures = vGTGeomeryFeatures;
+    std::vector<sFrame> vNoisyCameras = vGTCameras;
+    double cam_noise = 0.001;
+    double gs_noise = 0.02;
+    AddNoise(vNoisyCameras, vNoisyGeomeryFeatures, gs_noise, cam_noise);
+
+    double error_structure = 0.0;
+    for(int i = 0; i < vNoisyGeomeryFeatures.size(); i++)
+    {
+        Eigen::Vector4d gs = vNoisyGeomeryFeatures[i];
+        Eigen::Vector4d gt = vGTGeomeryFeatures[i];
+
+        //error_structure += (gs - gt_plane).norm();
+        Eigen::Quaterniond q;
+        q=g2o::SO3::HomoToQuad(gs(0),gs(1),gs(2),gs(3));
+        g2o::SO3 gs_plane_so3(q);
+        Eigen::Quaterniond q2;
+        q2=g2o::SO3::HomoToQuad(gt(0),gt(1),gt(2),gt(3));
+        g2o::SO3 gt_plane_so3(q2);
+        error_structure += g2o::SO3::log(gs_plane_so3* gt_plane_so3.inverse()).norm();
+    }
+
+    std::cout << "RMSE for the structure is:                " << error_structure / (double)nMapplanes << std::endl;
+
+    double error_cam_rotation = 0.0;
+    double error_cam_translation = 0.0;
+    for(int i = 0; i < vNoisyCameras.size(); i++)
+    {
+        Eigen::Matrix4d Tcw = vNoisyCameras[i].Tcw_;
+        Eigen::Matrix3d Rcw = Tcw.block(0,0,3,3);
+        Eigen::Vector3d tcw = Tcw.block(0,3,3,1);
+
+        Sophus::SE3d noise_cam = Sophus::SE3d(Rcw, tcw);
+        Sophus::SE3d gt_cam = gt_cam_pose[i];
+
+        Sophus::SE3d pose_err = gt_cam.inverse() * noise_cam;
+        error_cam_rotation += pose_err.so3().log().norm();
+        error_cam_translation += pose_err.translation().norm();
+    }
+    std::cout <<"Before"<<std::endl;
+    std::cout << "RMSE for the rotation of camera is:       " << error_cam_rotation / (double)nCameras << std::endl;
+    std::cout << "RMSE for the translation of camera is:    " << error_cam_translation / (double)nCameras << std::endl;
+
+
+    g2o::SparseOptimizer optimizer;
+    g2o::BlockSolverX::LinearSolverType * linearSolver;
+
+    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();
+
+    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
+    // LM大法好
+    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+    optimizer.setAlgorithm(solver);
+    // add camera vertex
+    for(int i = 0; i < nCameras; i++)
+    {
+        g2o::VertexMyExpmap* v = new g2o::VertexMyExpmap();
+        Eigen::Matrix4d Tcw = vNoisyCameras[i].Tcw_;
+        Eigen::Matrix3d Rcw = Tcw.block(0,0,3,3);
+        Eigen::Vector3d tcw = Tcw.block(0,3,3,1);
+        v->setId(i);
+        v->setFixed(i==0);
+        /*
+        Eigen::VectorXd cam_v;
+        cam_v.head(3) = tcw;
+        Eigen::Quaterniond cam_q(Rcw);
+        cam_v(3) = */
+        v->setEstimate( g2o::SE3Quat(Rcw, tcw));
+        optimizer.addVertex(v);
+    }
+    std::cout << "camera vertex have been added " << std::endl;
+    // add plane landmark
+    for(int i = 0; i < vNoisyGeomeryFeatures.size(); i++)
+    {
+        g2o::VertexSO3Expmap* vp = new g2o::VertexSO3Expmap();
+        vp->setId(i+nCameras);
+        vp->setFixed(true);
+        Eigen::Quaterniond q;
+        q=g2o::SO3::HomoToQuad(vNoisyGeomeryFeatures[i](0),vNoisyGeomeryFeatures[i](1),vNoisyGeomeryFeatures[i](2),vNoisyGeomeryFeatures[i](3));
+        g2o::SO3 q_vNoisyGeomeryFeatures(q);
+        vp->setEstimate(q_vNoisyGeomeryFeatures);
+        
+        optimizer.addVertex(vp);
+        
+
+        for(int j = 0; j < vNoisyCameras.size(); j++)
+        {
+            g2o::EdgeSE3ProjectPlaneSO3* e = new g2o::EdgeSE3ProjectPlaneSO3();
+
+            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*> (optimizer.vertex(i + nCameras)));
+            e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*> (optimizer.vertex(j)));
+/*
+            e->fx = fx;
+            e->fy = fy;
+            e->cx = cx;
+            e->cy = cy;*/
+            // Eigen::Vector4d  measurement_homo_para = vNoisyCameras[j].per_plane_feature_.find(i)->second;
+            //e->setMeasurement(vNoisyCameras[j].per_plane_feature_.find(i)->second);
+            e->setParams(vNoisyCameras[j].per_plane_feature_.find(i)->second);
+            e->setInformation(Eigen::Matrix3d::Identity());
+
+            optimizer.addEdge(e);
+        }
+    }
+    std::cout << "plane landmark have been added " << std::endl;
+    optimizer.setVerbose(true);
+    optimizer.initializeOptimization();
+    optimizer.optimize(10);
+
+    double rmse_structure = 0.0;
+    for(int i = 0; i < vNoisyGeomeryFeatures.size(); i++)
+    {
+        
+        g2o::VertexSO3Expmap* vGeoStructure_recov = static_cast<g2o::VertexSO3Expmap*>(optimizer.vertex(i+nCameras));
+        //Eigen::Vector4d gs = vGeoStructure_recov->estimate();
+        g2o::SO3 opt_plane(vGeoStructure_recov->estimate());
+
+      /**************************************************change*********************************************************************/
+        Eigen::Quaterniond q;
+        q=g2o::SO3::HomoToQuad(vGTGeomeryFeatures[i](0),vGTGeomeryFeatures[i](1),vGTGeomeryFeatures[i](2),vGTGeomeryFeatures[i](3));
+        g2o::SO3 gt_plane(q);
+        //QuatPlane opt_plane(gs);
+
+        rmse_structure += g2o::SO3::log(opt_plane* gt_plane.inverse()).norm();
+        std::cout<<"our opt_plane is "<<opt_plane<<std::endl;
+        //rmse_structure += (q_u.log_map(q_u)).norm();
+    }
+
+    std::cout << "RMSE for the structure is:                " << rmse_structure / (double)nMapplanes << std::endl;
+
+    double rmse_cam_rotation = 0.0;
+    double rmse_cam_translation = 0.0;
+    for(int i = 0; i < vNoisyCameras.size(); i++)
+    {
+        g2o::VertexMyExpmap * vPose_recov = static_cast<g2o::VertexMyExpmap *>(optimizer.vertex(i));
+        Eigen::Matrix4d Tcw = vPose_recov->estimate().to_homogeneous_matrix();
+        Eigen::Matrix3d Rcw = Tcw.block(0,0,3,3);
+        Eigen::Vector3d tcw = Tcw.block(0,3,3,1);
+
+        Sophus::SE3d opt_cam = Sophus::SE3d(Rcw, tcw);
+        Sophus::SE3d gt_cam = gt_cam_pose[i];
+
+        Sophus::SE3d pose_err = gt_cam * opt_cam.inverse();
+        rmse_cam_rotation += pose_err.so3().log().norm();
+        rmse_cam_translation += pose_err.translation().norm();
+    }
+
+    std::cout << "RMSE for the rotation of camera is:       " << rmse_cam_rotation / (double)nCameras << std::endl;
+    std::cout << "RMSE for the translation of camera is:    " << rmse_cam_translation / (double)nCameras << std::endl;
+
+
+    return 0;
+}
+
+
+void CreateCameras(std::vector<sFrame>& vFrames, std::vector<Sophus::SE3d>& gtpose, int nCams)
+{
+
+    Eigen::Matrix4d T;
+    T.setIdentity();
+
+    Eigen::Matrix3d Rx, Ry, Rz;
+    Eigen::Matrix3d R; 
+    Eigen::Vector3d t;
+    cv::RNG rng ( cv::getTickCount() );
+
+    double radius = 8;
+    for ( int i = 0; i < nCams; i ++ ) {
+        // Rotation.
+        if(i == 0)
+        {
+            sFrame frame(T);
+            vFrames.push_back(frame);
+            R = T.block(0,0,3,3);
+            t = T.block(0,3,3,1);
+            Sophus::SE3d cam ( R, t );
+            gtpose.push_back ( cam );
+            
+            continue;
+        }
+
+        double theta = i * 2 * M_PI / (6 * 4); // 1/4 圆弧
+
+        Eigen::Matrix3d R;
+        R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
+        Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
+
+        T.block(0,0,3,3) = R;
+        T.block(0,3,3,1) = t;
+        // SE3
+        sFrame frame(T);
+        vFrames.push_back(frame);
+
+        Sophus::SE3d cam (R, t);
+        gtpose.push_back(cam);
+		
+    }
+}
+
+
+void CreateSceneFeatures(const std::vector<sFrame>& vFrames, std::vector<Eigen::Vector4d>& vpGeometryFeatures)
+{
+    std::mt19937 gen(std::chrono::system_clock::now().time_since_epoch().count());
+    
+    std::uniform_real_distribution<double> d_x{0.0, 4.0};
+    std::uniform_real_distribution<double> d_y{0.0, 5.0};
+    std::uniform_real_distribution<double> d_z{0.0, 5.0};
+    std::uniform_real_distribution<double> d_h{0.1, 5.0};
+
+    int cnt = 0;
+    while(true)
+    {/*
+        Eigen::Vector4d pl;
+        pl[0] = d_x(gen);
+        pl[1] = d_y(gen);
+        pl[2] = d_z(gen);
+        pl[3] = d_h(gen);*/
+        Eigen::Vector4d homo_para_w ;
+        homo_para_w[0] =  std::sqrt(3);
+        homo_para_w[1] =  std::sqrt(3);
+        homo_para_w[2] =  std::sqrt(3);
+        homo_para_w[3] = -1.5;
+
+        int n_obervation_times = 0;
+
+        for(int j = 0; j < nCameras; j++)
+        {
+            //Eigen::Matrix3d Rcw = vFrames[j].Tcw_.block(0, 0, 3, 3);
+            //Eigen::Vector3d tcw = vFrames[j].Tcw_.block(0, 3, 3, 1);
+            // Eigen::Matrix3d Rcw = Rwc.transpose();
+            // Eigen::Vector3d tcw = -Rcw * twc;
+            Eigen::Matrix4d camera_T_world = vFrames[j].Tcw_;
+            Eigen::Vector4d homo_para_c = camera_T_world.inverse().transpose()* homo_para_w;
+            /*Eigen::Vector2d uv (
+                    fx * homo_para_c[0] / homo_para_c[2] + cx,
+                    fy * homo_para_c[1] / homo_para_c[2] + cy
+            );*/
+            double d = -homo_para_c[3] / std::sqrt(homo_para_c[0] * homo_para_c[0] + homo_para_c[1] * homo_para_c[1] + homo_para_c[2] * homo_para_c[2]);
+            if( d < 0)
+                continue;
+            n_obervation_times ++;
+        }
+
+        if(n_obervation_times == nCameras)
+        {
+            vpGeometryFeatures.push_back(homo_para_w);
+            cnt ++;
+        }
+            
+        if(cnt == nMapplanes)
+            break;
+    }
+    
+}
+
+
+void FeatureDetection(const Eigen::Matrix3d& K, const std::vector<Eigen::Vector4d> &scenefeatures, std::vector<sFrame> &vframes, bool is_add_noise)
+{
+    // detect point feature
+    std::mt19937 gen{12345};
+    const double pixel_sigma = 0.0;
+    std::normal_distribution<> d{0.0, pixel_sigma};
+
+    int planeID = 0;
+    for(auto mIter = scenefeatures.begin(); mIter != scenefeatures.end(); ++mIter)
+    {
+        Eigen::Vector4d Pw = (*mIter);
+        
+        for(auto fIter = vframes.begin(); fIter != vframes.end(); ++fIter)
+        {/*
+            Eigen::Matrix3d Rcw = (*fIter).Tcw_.block(0,0,3,3);
+            Eigen::Vector3d tcw = (*fIter).Tcw_.block(0,3,3,1);
+
+            Eigen::Vector3d Pc = Rcw * Pw + tcw;*/
+            Eigen::Matrix4d camera_T_world =(*fIter).Tcw_;
+            Eigen::Vector4d Pc = camera_T_world.inverse().transpose()* Pw;
+            double noise_pc0 = is_add_noise ? d(gen) : 0.0;
+            double noise_pc1 = is_add_noise ? d(gen) : 0.0;
+            double noise_pc2 = is_add_noise ? d(gen) : 0.0;
+            double noise_pc3 = is_add_noise ? d(gen) : 0.0;
+
+            double pc0 = Pc[0]+noise_pc0 ;
+            double pc1 = Pc[1]+noise_pc1 ;
+            double pc2 = Pc[2]+noise_pc2 ;
+            double pc3 = Pc[3]+noise_pc3 ;
+            //Eigen::Vector4d homo_pc(pc0,pc1,pc2,pc3);
+            Eigen::Quaterniond q;
+            q=g2o::SO3::HomoToQuad(pc0,pc1,pc2,pc3);
+            g2o::SO3 obs(q);
+            std::cout<<"our original plane is "<<obs<<std::endl;
+            (*fIter).per_plane_feature_.insert(make_pair(planeID, obs));
+
+
+        }
+        planeID++;
+    }
+
+    
+}
+
+void AddNoise(std::vector<sFrame>& vFrames, std::vector<Eigen::Vector4d>& vpGeometryFeatures, double gs_noise = 0.0, double cam_noise = 0.0)
+{
+    cv::RNG rng ( cv::getTickCount() );
+
+    // add noise for structure
+    for(auto fIter = vpGeometryFeatures.begin(); fIter != vpGeometryFeatures.end(); ++fIter)
+    {
+        
+        double noise_pi0 = rng.gaussian ( gs_noise );
+        double noise_pi1 = rng.gaussian ( gs_noise );
+        double noise_pi2 = rng.gaussian ( gs_noise );
+        double noise_pi3 = rng.gaussian ( gs_noise );
+
+        Eigen::Vector4d error = Eigen::Vector4d(noise_pi0, noise_pi1, noise_pi2,noise_pi3);
+
+        Eigen::Vector4d plane = (*fIter);
+        plane += error;
+        (*fIter) = plane;
+    }
+
+    std::normal_distribution<> noise_cam{0.0, cam_noise};
+    Eigen::Matrix3d Rx, Ry, Rz;
+	Eigen::Matrix3d R; 
+	Eigen::Vector3d t;
+    Eigen::Matrix4d T;
+    for(auto cIter = vFrames.begin(); cIter != vFrames.end(); ++cIter)
+    {
+        // skip the first camera.
+		if(cIter == vFrames.begin())
+			continue;
+		
+		double tz = rng.gaussian ( cam_noise );
+		double ty = rng.gaussian ( cam_noise );
+		double tx = rng.gaussian ( cam_noise );
+		
+		Rz << cos ( tz ), -sin ( tz ), 0.0,
+		    sin ( tz ), cos ( tz ), 0.0,
+		    0.0, 0.0, 1.0;
+		Ry << cos ( ty ), 0.0, sin ( ty ),
+		    0.0, 1.0, 0.0,
+		    -sin ( ty ), 0.0, cos ( ty );
+		Rx << 1.0, 0.0, 0.0,
+		    0.0, cos ( tx ), -sin ( tx ),
+		    0.0, sin ( tx ), cos ( tx );
+		R = Rz * Ry * Rx;
+		
+		// translation.
+		double x = rng.gaussian ( cam_noise );
+		double y = rng.gaussian ( cam_noise );
+		double z = rng.gaussian ( cam_noise );
+		t << x, y, z;
+		
+		// SE3
+		Sophus::SE3d cam_noise ( R, t );
+
+        Eigen::Matrix3d Rcw = (*cIter).Tcw_.block(0,0,3,3);
+        Eigen::Vector3d tcw = (*cIter).Tcw_.block(0,3,3,1);
+        Sophus::SE3d cam_temp(Rcw, tcw);
+        Sophus::SE3d newcamera = cam_temp * cam_noise;
+		(*cIter).Tcw_.block(0,0,3,3) = newcamera.rotationMatrix();
+        (*cIter).Tcw_.block(0,3,3,1) = newcamera.translation();
+
+    }
+}
+
+/*
+ * 03/06/2021 first  operation *
+ *            second update the state x + delta = x*exp(delta)
+ *            third read the paper about manifold
+ *
+ */
\ No newline at end of file