离散点 plane to fit (最小二乘)

 

 

using namespace Eigen;
int readStreamFile(
    const std::string &stream_file,
    std::vector<std::vector<Eigen::Vector3f>> &cloud_p)
{
    std::ifstream inFile(stream_file, std::ios::in | std::ios::binary);
    if (!inFile)
    {
        std::cout << "readstreamfile filed ..." << std::endl;
        return -1;
    }

    uint64_t time_m;
    int head = 0;
    int source_num_temp;
    int obs_num_temp;
    while (1)
    {
        if (head == 0)
        {
            if (!inFile.read((char *)&time_m, sizeof(uint64_t)))
                break;

            head++;
            continue;
        }
        else if (head == 1)
        {
            if (!inFile.read((char *)&source_num_temp, sizeof(int)))
                break;
            head++;
            continue;
        }
        else if (head == 2)
        {
            if (!inFile.read((char *)&obs_num_temp, sizeof(int)))
                break;
            head++;
            continue;
        }
        else
        {
            std::vector<Eigen::Vector3f> cloud(source_num_temp);
            for (int i = 0; i < source_num_temp; i++)
            {
                float pt[3];
                if (!inFile.read((char *)pt, 3 * sizeof(float)))
                {
                    break;
                }

                cloud[i][0] = pt[0];
                cloud[i][1] = pt[1];
                cloud[i][2] = pt[2];
            }

            std::vector<Eigen::Vector3f> obs_cloud(obs_num_temp);
            for (int i = 0; i < obs_num_temp; i++)
            {
                float pt[3];
                if (!inFile.read((char *)pt, 3 * sizeof(float)))
                {
                    return -1;
                }

                obs_cloud[i][0] = pt[0];
                obs_cloud[i][1] = pt[1];
                obs_cloud[i][2] = pt[2];
            }

            cloud_p.push_back(cloud);
            head = 0;
            source_num_temp = 0;
            obs_num_temp = 0;

        }
    }

    inFile.close();
    return 0;
}


int fitPlane(const std::vector<Eigen::Vector3f> & in_points) {

    Eigen::Quaterniond q = 
        Eigen::AngleAxisd(0, Eigen::Vector3d::Unit(2)) *
        Eigen::AngleAxisd((-3.1515926/4.0), Eigen::Vector3d::Unit(1)) *
        Eigen::AngleAxisd(0, Eigen::Vector3d::Unit(0));

    int count = in_points.size();
    //解最小二乘解问题
    Eigen::MatrixXf A(count, 4);   //(行，列)
    for (int i = 0; i < count; i++)
    {
        Eigen::Vector3f temp = q.cast<float>() * in_points[i];
        A(i, 0) = static_cast<float>(temp.x());
        A(i, 1) = static_cast<float>(temp.y());
        A(i, 2) = static_cast<float>(temp.z());
        A(i, 3) = 1.0f;
    }

    Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXf V = svd.matrixV(), U = svd.matrixU();
    //Eigen::Matrix3f S = U.inverse() * A * V.transpose().inverse(); // S = U^-1 * A * VT * -1
    //float A = V(0, 3);
    //float B = V(1, 3);
    //float C = V(2, 3);
    //float D = V(3, 3);
    std::cout << V << std::endl;
    Eigen::Vector3f normal(V(0, 3), V(1, 3), V(2, 3));
    return 0;
}

int main1515() {
    std::string stream_file = "./cloud_stream_11.stream";
    std::vector<std::vector<Eigen::Vector3f>> cloud_p;
    int ret = readStreamFile(stream_file, cloud_p);
    
    float box[6] = { -1.0,1.0,-1.0,1.0,-0.3, 0.3 };

    std::ofstream  file_xyz("./test.xyz");
    std::vector<Eigen::Vector3f> in_points;
    for (size_t i = 0; i < 10; i++)
    {
        for (size_t j = 0; j < cloud_p[i].size(); j++)
        {
            if (cloud_p[i][j].x() > box[0] &&
                cloud_p[i][j].x() < box[1] &&
                cloud_p[i][j].y() > box[2] &&
                cloud_p[i][j].y() < box[3] &&
                cloud_p[i][j].z() > box[4] &&
                cloud_p[i][j].z() < box[5]) {

                file_xyz << cloud_p[i][j].x() << " " << cloud_p[i][j].y() << " " << cloud_p[i][j].z() << "\n";
                in_points.emplace_back(cloud_p[i][j]);
            }
        }
    }
    file_xyz.close();
    fitPlane(in_points);

    return 0;
}