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();

    }
}