上传新文件

src/simworld.cpp

+#include "mylib/simworld.h"
+
+const double SMALL_EPS = 1e-5;
+Eigen::Quaterniond SimWord::ExpUpDateQuat(const Eigen::Vector3d &omg,const Eigen::Quaterniond q){
+    Eigen::Quaterniond res;
+    double theta,half_theta;
+    theta = omg.norm();//M_PI / 6;
+    half_theta = 0.5*theta;
+    EIGEN_USING_STD(sin)
+    EIGEN_USING_STD(cos)
+    // if(theta<SMALL_EPS) TODO
+    if(theta<SMALL_EPS){
+        res.vec() = omg;
+        res.w() = 1;
+    }else{
+        res.vec() = sin(half_theta)/theta*omg;
+        res.w() = cos(half_theta);
+    }
+
+    return res;
+}
+// inverse of the exponential map:log map
+Eigen::Vector3d SimWord::logQuat(Eigen::Quaterniond q){
+    Eigen::Vector3d w;
+    Eigen::Vector3d q_vec = q.vec();
+    double q_norm = q.vec().norm();
+    double q_w = q.w();
+    if (q_norm<SMALL_EPS){
+        w = q_vec;
+    }else {
+        w = 2 * acos(q_w) / q_norm * q_vec;
+    }
+    return w;
+}
+/***********************************************************************************************************************/
+
+std::vector<std::vector<Eigen::Vector3d>> SimWord::readPlaneFile(){
+    std::vector<std::vector<Eigen::Vector3d>> planesPoints;
+
+    std::ifstream planesFile("../res/planes.txt");
+
+    if(!planesFile.is_open()){
+        assert("Error - planes file not open");//throw is not from std
+    }
+
+    while(planesFile.good()){
+        std::vector<Eigen::Vector3d> curPlane;
+        int id;
+        planesFile >> id;
+        for(int v = 0; v < 4; ++v){ //four points to describe a plane
+            Eigen::Vector3d curPoint;
+            for(int c = 0; c < 3; ++c){
+                double val;
+                planesFile >> val;
+                if(planesFile.good()){
+                    curPoint[c] = val;
+                }
+            }
+            if(planesFile.good()){
+                //std::cout << "curPoint = " << curPoint.transpose() << std::endl;
+                curPlane.push_back(curPoint);
+            }
+        }
+        if(planesFile.good()){
+//            std::cout << "curPlane.size() = " << curPlane.size() << std::endl;
+            planesPoints.push_back(curPlane);
+        }
+    }
+//    std::cout << "planesPoints.size() = " << planesPoints.size() << std::endl;
+    return planesPoints;
+}
+
+Eigen::Vector4d SimWord::CreatePlanebyPoints(Eigen::Vector4d pt1_,Eigen::Vector4d pt2_,Eigen::Vector4d pt3_,Eigen::Vector4d pt4_)
+{
+    Eigen::Vector3d pt1=pt1_.head(3);Eigen::Vector3d pt2=pt2_.head(3);Eigen::Vector3d pt3=pt3_.head(3);Eigen::Vector3d pt4=pt4_.head(3);
+    Eigen::Vector4d plane_homo;
+    plane_homo.head(3) =  (pt1-pt3).cross((pt2-pt3));
+    plane_homo(3) = -pt3.transpose()*(pt1.cross(pt2));
+    Eigen::Vector3d normal;double dis;
+    normal =plane_homo.head(3)/plane_homo.head(3).norm();//.normalized();
+    dis = -plane_homo(3)/plane_homo.head(3).norm();
+    Eigen::Vector4d HessePlane;
+    HessePlane.head(3) = normal;
+    HessePlane(3) = dis;
+    return HessePlane;
+}
+
+
+void SimWord::CreateCameras(std::vector<sFrame>& vFrames, std::vector<Sophus::SE3d>& gtpose, int nCams) {
+
+        std::ifstream trajFile("../res/groundtruth.txt");
+
+        if (!trajFile.is_open()) {
+            assert("Error - groundtruth file not open");
+        }
+
+        std::vector<Eigen::Matrix<double, 7, 1>> gtPoses_7;
+        while (trajFile.good()) {
+            Eigen::Matrix<double, 7, 1> curPose;
+            int id;
+            trajFile >> id;
+            for (int v = 0; v < 7; ++v) {
+                double val;
+                trajFile >> val;
+                if (trajFile.good()) {
+                    curPose[v] = val;
+                }
+            }
+            if (trajFile.good()) {
+                //std::cout << "curPose = " << curPose.transpose() << std::endl;
+                gtPoses_7.push_back(curPose);
+            }
+        }
+        for (int i=0;i<nCams;++i){
+            Eigen::Matrix<double, 7, 1> camPose;
+            camPose = gtPoses_7[i];
+            Eigen::Vector3d t = camPose.head(3);
+            Eigen::Quaterniond q;
+            q.x() = camPose(3);q.y() = camPose(4);q.z() = camPose(5);q.w() = camPose(6);
+            q.normalize();
+            Eigen::Matrix3d R(q);
+            Eigen::Matrix4d T;
+            T<<1,0,0,0, 0,1,0,0, 0,0,1,0,0,0,0,1;
+            T.block(0,0,3,3) = R;
+            T.block(0,3,3,1) = t;
+            sFrame frame(T);
+            vFrames.push_back(frame);
+            Sophus::SE3d SE3_qt_cam(q, t);
+            gtpose.push_back(SE3_qt_cam);
+        }
+
+}
+
+void SimWord::CreateSceneFeatures(const std::vector<sFrame>& vFrames, std::vector<Eigen::Vector3d>& PlaneFeature)
+{/*
+    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;
+    Eigen::Vector4d pt1_,pt2_,pt3_,pt4_;
+    std::vector<Eigen::Vector3d> curPlane;
+    std::vector<std::vector<Eigen::Vector3d>> planesPoints = readPlaneFile();
+    //std::vector<Eigen::Vector3d> PlaneFeature;  //3 planes
+    //std::vector<std::vector<Eigen::Vector3d>>::iterator ele;
+    for (int i = 0;i<planesPoints.size();++i) {
+        curPlane = planesPoints[i];
+        pt1_.head(3) = curPlane[0];pt1_(3) = 1;
+        pt2_.head(3) = curPlane[1];pt2_(3) = 1;
+        pt3_.head(3) = curPlane[2];pt3_(3) = 1;
+        pt4_.head(3) = curPlane[3];pt4_(3) = 1;
+        Eigen::Vector4d curPlane_hesse = CreatePlanebyPoints(pt1_,pt2_,pt3_,pt4_);
+        // std::cout <<"curPlane_hesse is "<<curPlane_hesse.transpose()<<std::endl;
+        // PlaneFeature.push_back(curPlane_hesse.head(3)*curPlane_hesse(3));
+        Eigen::Vector4d homo_para_w;
+        double n_norm = 1;
+        Eigen::Vector3d norm = curPlane_hesse.head(3);
+        double dis = curPlane_hesse(3);
+        norm.normalized();
+        homo_para_w[0] = norm[0] * n_norm;
+        homo_para_w[1] = norm[1] * n_norm;
+        homo_para_w[2] = norm[2] * n_norm;
+        homo_para_w[3] = -1 * dis * n_norm;
+        int n_obervation_times = 0;
+        //PlaneFeature.push_back(curPlane_hesse.head(3)*curPlane_hesse(3));}
+
+            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::Matrix4d transM =camera_T_world.transpose().inverse();
+                Eigen::Vector4d homo_para_c = transM* homo_para_w;
+
+                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)
+            {
+                PlaneFeature.push_back(curPlane_hesse.head(3)*curPlane_hesse(3));
+                cnt ++;
+            }
+
+            if(cnt == nMapplanes)
+                break;
+    }
+}
+
+
+void SimWord::FeatureDetection(const Eigen::Matrix3d& K, const std::vector<Eigen::Vector3d> &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::Vector3d Pw = (*mIter);
+        double dis = Pw.norm();
+        Eigen::Vector3d normal = Pw/dis;
+        Eigen::Vector4d homo_para_w;
+        double n_norm = 1;
+        normal.normalized();
+        homo_para_w[0] = normal[0] * n_norm;
+        homo_para_w[1] = normal[1] * n_norm;
+        homo_para_w[2] = normal[2] * n_norm;
+        homo_para_w[3] = -1 * dis * n_norm;
+
+        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::Vector4d homo_para_c = (*fIter).Tcw_.transpose().inverse()*homo_para_w;
+            //QuatPlane::HomogeneousToHesse(const Eigen::Vector4d &homo_para, Eigen::Vector3d &norm, double &dis)
+            Eigen::Vector3d normh;
+            double dish;
+            QuatPlane::HomogeneousToHesse(homo_para_c,normh,dish);
+            Eigen::Vector3d ob = normh*dish;
+
+            (*fIter).per_plane_feature_.insert(std::make_pair(planeID, ob));
+
+
+        }
+        planeID++;
+    }
+
+
+}
+
+
+void SimWord::AddNoise(std::vector<sFrame>& vFrames, std::vector<Eigen::Vector3d>& vpGeometryFeatures, double gs_noise , double cam_noise )
+{
+    cv::RNG rng ( cv::getTickCount() );
+
+    // add noise for structure
+    for(auto fIter = vpGeometryFeatures.begin(); fIter != vpGeometryFeatures.end(); ++fIter)
+    {
+
+        double noise_x = rng.gaussian ( gs_noise );
+        double noise_y = rng.gaussian ( gs_noise );
+        double noise_z = rng.gaussian ( gs_noise );
+
+        Eigen::Vector3d error = Eigen::Vector3d(noise_x, noise_y, noise_z);
+
+        Eigen::Vector3d plane_3d = (*fIter);
+        plane_3d += error;
+        (*fIter) = plane_3d;
+
+    }
+
+    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();
+
+    }
+}