#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;
int main() {    Size boardSize(9,6);
    float squareSize=25.0f;
    vector<vector<Point2f>> imagePoints;
    vector<vector<Point3f>> objectPoints;    
    for(int i=0;i<20;i++) {        Mat img=imread(format("calib_%d.jpg",i));
        vector<Point2f> corners;        if(findChessboardCorners(img,boardSize,corners)) {            cornerSubPix(img,corners,Size(11,11),Size(-1,-1),                        TermCriteria(TermCriteria::EPS+TermCriteria::MAX_ITER,30,0.001));
            imagePoints.push_back(corners);
            
            vector<Point3f> obj;            for(int j=0;j<boardSize.height;j++)                for(int k=0;k<boardSize.width;k++)
                    obj.push_back(Point3f(k*squareSize,j*squareSize,0));
            objectPoints.push_back(obj);
        }
    }    
    Mat cameraMatrix, distCoeffs;
    vector<Mat> rvecs, tvecs;    calibrateCamera(objectPoints,imagePoints,Size(1280,1024),
                   cameraMatrix,distCoeffs,rvecs,tvecs);    
    FileStorage fs("calib_params.xml",FileStorage::WRITE);
    fs << "cameraMatrix" << cameraMatrix << "distCoeffs" << distCoeffs;
    fs.release();    return 0;
}

import numpy as npimport cv2
def calculate_phase(images):    I0,I1,I2,I3=images    return np.arctan2(I3-I1,I0-I2)
def unwrap_phase(wrapped_phase, gray_codes):
    k=np.zeros_like(wrapped_phase,dtype=np.int32)    for code in gray_codes:
        k=(k<<1)|code    return wrapped_phase+2*np.pi*k
phase_imgs=[cv2.imread(f"phase_{i}.png",0) for i in range(4)]
gray_imgs=[cv2.imread(f"gray_{i}.png",0)>128 for i in range(8)]
abs_phase=unwrap_phase(calculate_phase(phase_imgs),gray_imgs)

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/registration/icp.h>
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
int main() {    PointCloudT::Ptr cloud1(new PointCloudT), cloud2(new PointCloudT);    pcl::io::loadPCDFile("scan1.pcd",*cloud1);    pcl::io::loadPCDFile("scan2.pcd",*cloud2);    
    pcl::StatisticalOutlierRemoval<PointT> sor;
    sor.setInputCloud(cloud1);
    sor.setMeanK(50);
    sor.setStddevMulThresh(1.0);
    sor.filter(*cloud1);    
    pcl::IterativeClosestPoint<PointT,PointT> icp;
    icp.setInputSource(cloud1);
    icp.setInputTarget(cloud2);
    icp.setMaxCorrespondenceDistance(0.05);
    icp.setMaximumIterations(100);
    icp.setTransformationEpsilon(1e-8);    
    PointCloudT::Ptr aligned(new PointCloudT);
    icp.align(*aligned);    pcl::io::savePCDFile("aligned.pcd",*aligned);    return 0;
}