<commit_before>//
//  PointCloudManipulation.cpp
//  PointPairFeatures
//
//  Created by <NAME> on 02.08.14.
//  Copyright (c) 2014 <NAME>. All rights reserved.
//

#include "PointCloudManipulation.h"
#include "Visualize.h"
#include "PointPairFeatures.h"

namespace ICP {

//K called correlation matrix in eggert_comparison_mva97, is actually a covariance matrix multiplied by N
//http://graphics.stanford.edu/~smr/ICP/comparison/eggert_comparison_mva97.pdf
Isometry3f pointToPoint(vector<Vector3f>&src,vector<Vector3f>&dst){
    int N = src.size(); assert(N==dst.size());
    Map<Matrix3Xf> ps(&src[0].x(),3,N); //maps vector<Vector3f>
    Map<Matrix3Xf> qs(&dst[0].x(),3,N); //to Matrix3Nf columnwise
    Vector3f p_dash = ps.rowwise().mean();
    Vector3f q_dash = qs.rowwise().mean();
    Matrix3Xf ps_centered = ps.colwise() - p_dash;
    Matrix3Xf qs_centered = qs.colwise() - q_dash;
    Matrix3f K = qs_centered * ps_centered.transpose();
    JacobiSVD<Matrix3f> svd(K, ComputeFullU | ComputeFullV);
    
    //this flipping scheme (to make sure we have rotation, not mirroring) is wrong / not always correct!!!!
    /*Matrix3d R = svd.matrixU()*svd.matrixV().transpose();
    if(R.determinant()<0){
        R.col(2) *= -1;
    }*/

    //this is the correct way!!!
    Matrix3d S = Matrix3d::Identity();
    if(svd.matrixU().determinant() * svd.matrixV().transpose().determinant()<0){
        S(2,2)= -1;
    }
    
    Isometry3f T = Isometry3f::Identity();
    T.linear() = R;
    T.translation() = q_dash - R*p_dash; return T;
}

// https://www.comp.nus.edu.sg/~lowkl/publications/lowk_point-to-plane_icp_techrep.pdf
// http://www.cs.princeton.edu/~smr/papers/icpstability.pdf
Isometry3f pointToPlane(vector<Vector3f> &src,vector<Vector3f> &dst,vector<Vector3f> &nor){
    assert(src.size()==dst.size() && src.size()==nor.size());
    Matrix<float,6,6> C; C.setZero();
    Matrix<float,6,1> d; d.setZero();

    for(uint i=0;i<src.size();++i){
        Vector3f cro = src[i].cross(nor[i]);
        C.block<3,3>(0,0) += cro*cro.transpose();
        C.block<3,3>(0,3) += nor[i]*cro.transpose();
        C.block<3,3>(3,3) += nor[i]*nor[i].transpose();
        float sum = (src[i]-dst[i]).dot(nor[i]);
        d.head(3) -= cro*sum;
        d.tail(3) -= nor[i]*sum;<commit_msg>float instead of double<commit_after>//
//  PointCloudManipulation.cpp
//  PointPairFeatures
//
//  Created by <NAME> on 02.08.14.
//  Copyright (c) 2014 <NAME>. All rights reserved.
//

#include "PointCloudManipulation.h"
#include "Visualize.h"
#include "PointPairFeatures.h"

namespace ICP {

//K called correlation matrix in eggert_comparison_mva97, is actually a covariance matrix multiplied by N
//http://graphics.stanford.edu/~smr/ICP/comparison/eggert_comparison_mva97.pdf
Isometry3f pointToPoint(vector<Vector3f>&src,vector<Vector3f>&dst){
    int N = src.size(); assert(N==dst.size());
    Map<Matrix3Xf> ps(&src[0].x(),3,N); //maps vector<Vector3f>
    Map<Matrix3Xf> qs(&dst[0].x(),3,N); //to Matrix3Nf columnwise
    Vector3f p_dash = ps.rowwise().mean();
    Vector3f q_dash = qs.rowwise().mean();
    Matrix3Xf ps_centered = ps.colwise() - p_dash;
    Matrix3Xf qs_centered = qs.colwise() - q_dash;
    Matrix3f K = qs_centered * ps_centered.transpose();
    JacobiSVD<Matrix3f> svd(K, ComputeFullU | ComputeFullV);
    
    //this flipping scheme (to make sure we have rotation, not mirroring) is wrong / not always correct!!!!
    /*Matrix3f R = svd.matrixU()*svd.matrixV().transpose();
    if(R.determinant()<0){
        R.col(2) *= -1;
    }*/

    //this is the correct way!!!
    Matrix3f S = Matrix3f::Identity();
    if(svd.matrixU().determinant() * svd.matrixV().transpose().determinant()<0){
        S(2,2)= -1;
    }
    
    Isometry3f T = Isometry3f::Identity();
    T.linear() = R;
    T.translation() = q_dash - R*p_dash; return T;
}

// https://www.comp.nus.edu.sg/~lowkl/publications/lowk_point-to-plane_icp_techrep.pdf
// http://www.cs.princeton.edu/~smr/papers/icpstability.pdf
Isometry3f pointToPlane(vector<Vector3f> &src,vector<Vector3f> &dst,vector<Vector3f> &nor){
    assert(src.size()==dst.size() && src.size()==nor.size());
    Matrix<float,6,6> C; C.setZero();
    Matrix<float,6,1> d; d.setZero();

    for(uint i=0;i<src.size();++i){
        Vector3f cro = src[i].cross(nor[i]);
        C.block<3,3>(0,0) += cro*cro.transpose();
        C.block<3,3>(0,3) += nor[i]*cro.transpose();
        C.block<3,3>(3,3) += nor[i]*nor[i].transpose();
        float sum = (src[i]-dst[i]).dot(nor[i]);
        d.head(3) -= cro*sum;
        d.tail(3) -= nor[i]*sum;
{'fraction_non_alphanumeric': 0.1896853146853147, 'fraction_numerical': 0.029283216783216784, 'mean_word_length': 3.577, 'pattern_counts': {'":': 0, '<': 41, '<?xml version=': 0, '>': 35, 'https://': 2, 'lorem ipsum': 0, 'www.': 4, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2622106', 'n_tokens_mistral': 1826, 'n_tokens_neox': 1754, 'n_words': 306}