结构光：LaserPlane类

于 2020-01-08 15:42:36 发布
阅读量291 收藏 1
点赞数
分类专栏： 算法
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_35299223/article/details/103892420
版权

plane.h

#ifndef  PLANE_H
#define  PLANE_H

#include <opencv2\imgproc.hpp>
#include <opencv2\calib3d.hpp>
#include <opencv2\highgui.hpp>
#include<iostream>
#include <vector>
#include<math.h>
using namespace cv;

class LaserPlane {
private:
	typedef struct {
		//Z=a0*X+a1*Y+a2;
		double a0;
		double a1;
		double a2;
	}PlaneCoeffs;
	typedef struct {
		double kx;
		double ky;
		double kz;
		Point3f PassPt;
	}Line;
	Line BaseLine;
	PlaneCoeffs PCoeffs;

public:
	LaserPlane();
	~LaserPlane();
	void PlaneFitting(vector<Point3f> &PointsCameraCoordinate);//利用三维点云集合进行拟合平面
	void CleanCoeffs();//清空激光平面的系数
	void ComputeImgPtOnPlane(Mat& M,vector<Point2f>& ImgPtSet,vector<Point3f>& PtsInCamera);//输入相机内参，像素坐标点集，输出他们在相机坐标系下的三维坐标
	void SetBaseLine(Mat& M, vector<Point2f>& BaseLineImgPtSet);//设定一条基准线作为参考坐标
	void SetBaseLineWith3DPts(vector<Point3f>& PtsInCamera);//利用空间中的三维点云坐标设置基线
	float PointToBaseLine(Point3f& Pt);//计算单点Pt到基线的距离
	void ComputePtDistToBaseLine(vector<Point3f>& PtSet, vector<float>& DistIn3D);//计算空间中的点集到直线的距离
	void DisplayCoeffs();//打印平面参数
	void DisplayBaseLine();//打印baseline的信息
	float NormPoint3f(Point3f& vec);//计算向量的距离
	void SetBaseLineParams(float _kx, float _ky, float _kz, Point3f& pt);//人为设定基线参数

};


#endif // ! PLANE_H

plane.cpp

#include"Plane.h"

LaserPlane::LaserPlane() {
	PCoeffs.a0 = 0;
	PCoeffs.a1 = 0;
	PCoeffs.a2 = 0;
	BaseLine.kx = 0;
	BaseLine.ky = 0;
	BaseLine.kz = 0;
	BaseLine.PassPt.x = 0;
	BaseLine.PassPt.y = 0;
	BaseLine.PassPt.z = 0;
}

LaserPlane::~LaserPlane() {}

// 利用三维点云集合进行拟合平面
// Z=a0*X+a1*Y+a2;
void LaserPlane::PlaneFitting(vector<Point3f> &PointsCameraCoordinate) {
	int NumOfPts = PointsCameraCoordinate.size();
	Mat A = Mat::ones(NumOfPts, 3, CV_64FC1);//[Xci,Yci,1]*[a0,a1,a2]'=[Zci]
	Mat b = Mat::zeros(NumOfPts, 1, CV_64FC1);

	for (int i = 0; i < NumOfPts; i++) {
		*A.ptr<double>(i, 0) = PointsCameraCoordinate[i].x;
		*A.ptr<double>(i, 1) = PointsCameraCoordinate[i].y;//A的第三列统一为1

		*b.ptr<double>(i, 0) = PointsCameraCoordinate[i].z;
	}
	Mat a_coefficient = (A.t()*A).inv()*(A.t()*b);
	for (int i = 0; i < 3; i++) {
		PCoeffs.a0 = a_coefficient.at<double>(0, 0);
		PCoeffs.a1 = a_coefficient.at<double>(1, 0);
		PCoeffs.a2 = a_coefficient.at<double>(2, 0);
	}
}

//清空激光平面的系数
void LaserPlane::CleanCoeffs() {
	PCoeffs.a0 = 0;
	PCoeffs.a1 = 0;
	PCoeffs.a2 = 0;
	BaseLine.kx = 0;
	BaseLine.ky = 0;
	BaseLine.kz = 0;
	BaseLine.PassPt.x = 0;
	BaseLine.PassPt.y = 0;
	BaseLine.PassPt.z = 0;
}

//输入相机内参，像素坐标点集，输出他们在相机坐标系下的三维坐标
/* 
	a0*Xc+a1*Yc+a2=Zc;
	Zc*(a0*Xn+a1*Yn-1)=-a2;
	Zc=-a2/(a0*Xn+a1*Yn-1);
*/
void LaserPlane::ComputeImgPtOnPlane(Mat& M, vector<Point2f>& ImgPtSet, vector<Point3f>& PtsInCamera)
{
	int NumOfPts = ImgPtSet.size();
	//构建uvPoints矩阵
	Mat uvPts = Mat::ones(3, NumOfPts, CV_64FC1);
	for (int i = 0; i < NumOfPts; i++) {
		*uvPts.ptr<double>(0, i) = ImgPtSet[i].x;
		*uvPts.ptr<double>(1, i) = ImgPtSet[i].y;
	}
	//构建齐次矩阵
	Mat XnYnMat = M.inv()*uvPts;
	
	// 构建结果矩阵
	vector<double> LineZc(NumOfPts, 0);
	vector<double> LineXc(NumOfPts, 0);
	vector<double> LineYc(NumOfPts, 0);
	double Xn;//临时变量
	double Yn;

	// 计算像素点在空间的坐标
	for (int i = 0; i < NumOfPts; i++) {
		Xn = *XnYnMat.ptr<double>(0, i);
		Yn = *XnYnMat.ptr<double>(1, i);
		LineZc[i] = (-1 * PCoeffs.a2) / (PCoeffs.a0*Xn + PCoeffs.a1*Yn - 1);
		LineXc[i] = Xn*LineZc[i];
		LineYc[i] = Yn*LineZc[i];
		Point3f tempPt;
		tempPt.x = LineXc[i];
		tempPt.y = LineYc[i];
		tempPt.z = LineZc[i];
		PtsInCamera.push_back(tempPt);
	}
}


//设定一条基准线作为参考坐标
void LaserPlane::SetBaseLine(Mat& M, vector<Point2f>& BaseLineImgPtSet) {
	//预设一个容器装baseline的point3f
	vector<Point3f> PtsInCamera;

	//计算baseline的point3f
	ComputeImgPtOnPlane(M, BaseLineImgPtSet, PtsInCamera);

	//利用空间点云进行设置基线
	SetBaseLineWith3DPts(PtsInCamera);
}


//利用空间中的点云进行设置基线
void LaserPlane::SetBaseLineWith3DPts(vector<Point3f>& PtsInCamera)
{
	int NumOfPts = PtsInCamera.size();
	Mat A1 = Mat::ones(NumOfPts, 2,CV_64FC1);
	Mat b1 = Mat::zeros(NumOfPts, 1, CV_64FC1);
	Mat A2 = Mat::ones(NumOfPts, 2, CV_64FC1);
	Mat b2 = Mat::zeros(NumOfPts, 1, CV_64FC1);

	// 设置A1，b1,A2,b2
	for (int i = 0; i < NumOfPts; i++) {
		*A1.ptr<double>(i, 0) = PtsInCamera[i].z;
		*A2.ptr<double>(i, 0) = PtsInCamera[i].z;
		*b1.ptr<double>(i, 0) = PtsInCamera[i].x;
		*b2.ptr<double>(i, 0) = PtsInCamera[i].y;
	}

	Mat k1b1 = (A1.t()*A1).inv()*(A1.t()*b1);
	Mat k2b2 = (A2.t()*A2).inv()*(A2.t()*b2);
	BaseLine.kx = *k1b1.ptr<double>(0, 0);
	BaseLine.ky = *k2b2.ptr<double>(0, 0);
	BaseLine.kz = 1;
	// 将方向向量单位化
	double VecLen = sqrt(BaseLine.kx*BaseLine.kx + BaseLine.ky*BaseLine.ky + BaseLine.kz*BaseLine.kz);
	BaseLine.kx = BaseLine.kx/VecLen;
	BaseLine.ky = BaseLine.ky/VecLen;
	BaseLine.kz = BaseLine.kz/VecLen;

	BaseLine.PassPt.x= *k1b1.ptr<double>(1, 0);
	BaseLine.PassPt.y = *k2b2.ptr<double>(1, 0);
	BaseLine.PassPt.z = 0;
}

// Pt为基线外一点M1，基线过M0，方向向量为[kx,ky,kz]
float LaserPlane::PointToBaseLine(Point3f & Pt)
{
	float SingleDist = 0;
	Point3f VecM0ToM1;

	Point3f DirUnitVec;
	DirUnitVec.x = BaseLine.kx;
	DirUnitVec.y = BaseLine.ky;
	DirUnitVec.z = BaseLine.kz;

	//设定基线外一点
	VecM0ToM1 = Pt - BaseLine.PassPt;
	
	//计算投影向量
	Point3f ProjVecM0ToM1 = VecM0ToM1.ddot(DirUnitVec)*DirUnitVec;

	//计算垂直向量
	Point3f PerpendicularVecOfM0ToM1 = VecM0ToM1 - ProjVecM0ToM1;

	//计算垂直距离
	SingleDist = NormPoint3f(PerpendicularVecOfM0ToM1);
	return SingleDist;
}

// 计算点集到基线坐标
// 假设要求的线外一点为M1,基线过M0,方向向量为[kx,ky,kz]
void LaserPlane::ComputePtDistToBaseLine(vector<Point3f>& PtSet, vector<float>& DistIn3D) {
	int NumOfPts = PtSet.size();
	float SingleDist=0;
	Point3f VecM0ToM1;

	Point3f DirUnitVec;
	DirUnitVec.x = BaseLine.kx;
	DirUnitVec.y = BaseLine.ky;
	DirUnitVec.z = BaseLine.kz;

	for (int i = 0; i < NumOfPts; i++) {
		VecM0ToM1 = PtSet[i] - BaseLine.PassPt;
		//计算投影向量
		Point3f ProjVecM0ToM1 = VecM0ToM1.ddot(DirUnitVec)*DirUnitVec;
		
		//计算垂直向量
		Point3f PerpendicularVecOfM0ToM1 = VecM0ToM1 - ProjVecM0ToM1;
		
		//计算垂直距离
		SingleDist = NormPoint3f(PerpendicularVecOfM0ToM1);
		DistIn3D.push_back(SingleDist);
	}
}

//打印平面参数
void LaserPlane::DisplayCoeffs()
{
	std::cout << "a0= "<<PCoeffs.a0 << std::endl;
	std::cout << "a1= " << PCoeffs.a1 << std::endl;
	std::cout << "a2= " << PCoeffs.a2 << std::endl;
}

//打印基线信息
void LaserPlane::DisplayBaseLine()
{
	std::cout << "法向量为" << std::endl;
	std::cout << "kx=" << BaseLine.kx << std::endl;
	std::cout << "ky=" << BaseLine.ky << std::endl;
	std::cout << "kz=" << BaseLine.kz << std::endl;
	std::cout << "通过的点坐标" << std::endl;
	std::cout << BaseLine.PassPt<< std::endl;
}

//三维向量的norm
float LaserPlane::NormPoint3f(Point3f & vec)
{
	double Norm = sqrt(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z);
	return Norm;
}

// 自己设定基准线的参数
void LaserPlane::SetBaseLineParams(float _kx, float _ky, float _kz, Point3f & pt)
{
	BaseLine.kx = _kx;
	BaseLine.ky = _ky;
	BaseLine.kz = _kz;
	BaseLine.PassPt = pt;
}

测试代码

#include <iostream>
#include "Plane.h"
#include <vector>
#include <fstream>
#include "MatIO.h"
using namespace std;
int main() {
	LaserPlane p;
	fstream fileA,fileb;
	fileA.open("PlaneFitA.txt");
	fileb.open("PlaneFitb.txt");
	vector<Point3f> CamPts;
	int numofpts = 10067;
	//将文件中的点读入向量中
	for (int i = 0; i < numofpts; i++) {
		Point3f temppt;
		float x;
		float y;
		float abandon;
		float z;
		fileA >> x;
		fileA >> y;
		fileA >> abandon;
		fileb >> z;
		temppt.x = x;
		temppt.y = y;
		temppt.z = z;
		CamPts.push_back(temppt);
	}
	p.PlaneFitting(CamPts);//平面系数拟合
	p.DisplayCoeffs();//打印参数

	// 待测量中心线准备
	fstream MeasureLineFile;
	MeasureLineFile.open("MidLineImgPts.txt");
	vector<Point2f> ImgPts;
	int NumOfLinePts=1671;
	for (int i = 0; i < NumOfLinePts; i++) {
		Point2f SingleImgPt;
		MeasureLineFile >> SingleImgPt.x;
		MeasureLineFile >> SingleImgPt.y;
		ImgPts.push_back(SingleImgPt);
	}
	// 内参矩阵准备
	Mat matlabM = Mat::zeros(3, 3, CV_64FC1);
	ReadMatFromFileName("matlabM.txt", matlabM, 1);
	matlabM = matlabM.t();//matlab矩阵要进行转置
	// 输出的结果向量
	vector<Point3f> MeasurePtsInC;
	p.ComputeImgPtOnPlane(matlabM, ImgPts, MeasurePtsInC);

	cout << "测量结果" << endl;
	cout << MeasurePtsInC << endl;

	// 直线拟合
	fstream FittingPtsFile;
	FittingPtsFile.open("matlabPts.txt");
	vector<Point3f> FitPtSet;
	int NumOfFitPtSet = 10;
	
	for (int i = 0; i < NumOfFitPtSet; i++) {
		Point3f TempFitPt;
		FittingPtsFile >> TempFitPt.x;
		FittingPtsFile >> TempFitPt.y;
		FittingPtsFile >> TempFitPt.z;
		FitPtSet.push_back(TempFitPt);
	}

	p.SetBaseLineWith3DPts(FitPtSet);
	p.DisplayBaseLine();

	// 点集合使用直线拟合的点集，计算他们到直线的距离
	// Point3f是float类型,Point3d是double类型
	vector<float> DistSet;
	cout << "距离计算" << endl;
	p.ComputePtDistToBaseLine(FitPtSet, DistSet);
	for (int i = 0; i < DistSet.size(); i++) {
		cout << DistSet[i] << endl;
	}

	//测试距离计算是否合理
	cout << "输入已知点，测试是否合理" << endl;
	Point3f pt(0, 0, 0);
	p.SetBaseLineParams(1, 0, 0, pt);// 设定基线为过0点的，方向为(1,0,0)的线
	Point3f pt2(0, 1, 1);//被测试点
	vector<Point3f> ptset2;
	vector<float> DistSet2;
	ptset2.push_back(pt2);// 要测量的点是pt2,结果存储在DistSet2中
	p.ComputePtDistToBaseLine(ptset2, DistSet2);
	for (int i = 0; i < DistSet2.size(); i++) {
		cout << DistSet2[i] << endl;
	}

	//测试单点距离函数函数
	cout << "单点距离函数" << endl;
	Point3f testpt1(1, 1, 0);
	Point3f testpt2(0, 1, 1);	
	Point3f testpt3(0, 1, 2);
	
	cout << p.PointToBaseLine(testpt1) << endl;
	cout << p.PointToBaseLine(testpt2) << endl;
	cout << p.PointToBaseLine(testpt3) << endl;

	return 0;
}

文件下载

链接：https://pan.baidu.com/s/1_YsF1R4gWyQvtkNqnDnKbw
提取码：u21v

计算三维点云坐标函数保存

这个函数在使用的时候出现了报错，查找原因发现是因为M是CV_32FC1的，而我们构建的uvPts是CV_64FC1的。现在考虑在代码中修改，但是我不记得当时为什么要使用double了。似乎是因为原本标定后直接使用的M是64FC1的，但是我们读取获得的内参是32FC1的。因此我觉得这里使用一个转换函数更加合适。


//输入相机内参，像素坐标点集，输出他们在相机坐标系下的三维坐标
/*
    a0*Xc+a1*Yc+a2=Zc;
    Zc*(a0*Xn+a1*Yn-1)=-a2;
    Zc=-a2/(a0*Xn+a1*Yn-1);
*/
void LaserPlane::ComputeImgPtOnPlane(Mat& M, vector<Point2f>& ImgPtSet, vector<Point3f>& PtsInCamera)
{
    int NumOfPts = ImgPtSet.size();
    //构建uvPoints矩阵
    Mat uvPts = Mat::ones(3, NumOfPts, CV_64FC1);
    for (int i = 0; i < NumOfPts; i++) {
        *uvPts.ptr<double>(0, i) = ImgPtSet[i].x;
        *uvPts.ptr<double>(1, i) = ImgPtSet[i].y;
    }
	//debug
	Mat  testMat = M.inv();
	int MinvType=testMat.type();

    // 构建齐次矩阵
    Mat XnYnMat = M.inv()*uvPts;

    // 构建结果矩阵
    vector<double> LineZc(NumOfPts, 0);
    vector<double> LineXc(NumOfPts, 0);
    vector<double> LineYc(NumOfPts, 0);
    double Xn;//临时变量
    double Yn;

    // 计算像素点在空间的坐标
    for (int i = 0; i < NumOfPts; i++) {
        Xn = *XnYnMat.ptr<double>(0, i);
        Yn = *XnYnMat.ptr<double>(1, i);
        LineZc[i] = (-1 * PCoeffs.a2) / (PCoeffs.a0*Xn + PCoeffs.a1*Yn - 1);
        LineXc[i] = Xn*LineZc[i];
        LineYc[i] = Yn*LineZc[i];
        Point3f tempPt;
        tempPt.x = LineXc[i];
        tempPt.y = LineYc[i];
        tempPt.z = LineZc[i];
        PtsInCamera.push_back(tempPt);
    }
}
糖琴心
关注
  • 0
    点赞
  • 踩
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值