6D姿态检测

python算法工程师

已于 2023-04-14 13:09:40 修改

阅读量2.9k

点赞数 10

分类专栏：随笔文章标签：计算机视觉人工智能 opencv

于 2023-04-09 19:25:44 首次发布

本文链接：https://blog.csdn.net/qq_44089890/article/details/130045928

版权

随笔专栏收录该内容

82 篇文章 15 订阅

订阅专栏

在这里插入图片描述

3D相机—结构光相机

结构光，英文叫做 Structured light，其原理是基本原理是，通过近红外激光器，将具有一定结构特征的光线投射到被拍摄物体上，再由专门的红外摄像头进行采集。这种具备一定结构的光线，会因被摄物体的不同深度区域，而采集不同的图像相位信息，然后通过运算单元将这种结构的变化换算成深度信息，以此来获得三维结构。简单来说就是，通过光学手段获取被拍摄物体的三维结构，再将获取到的信息进行更深入的应用。通常采用特定波长的不可见的红外激光作为光源，它发射出来的光经过一定的编码投影在物体上，通过一定算法来计算返回的编码图案的畸变来得到物体的位置和深度信息。

在这里插入图片描述
结构光（散斑）的优点主要有：

1）方案成熟，相机基线可以做的比较小，方便小型化。

2）资源消耗较低，单帧 IR 图就可计算出深度图，功耗低。

3）主动光源，夜晚也可使用。

4）在一定范围内精度高，分辨率高，分辨率可达 1280x1024,帧率可达 60FPS。

3D相机—双目

双目立体视觉深度相机的深度测量过程，如下：

1、首先需要对双目相机进行标定，得到两个相机的内参数、外参矩阵。

2、根据标定结果对原始图像校正，校正后的两张图像位于同一平面且互相平行。

3、对校正后的两张图像进行像素点匹配。

4、根据匹配结果计算每个像素的深度，从而获得深度图。
在这里插入图片描述双目优点：
1.成本低；

缺点：
1）对环境光照非常敏感。光线变化导致图像偏差大，进而会导致匹配失败或精度低
2）不适用单调缺乏纹理的场景。双目视觉根据视觉特征进行图像匹配，没有特征会导致匹配失败。
3）计算复杂度高。该方法是纯视觉的方法，对算法要求高，计算量较大。

在这里插入图片描述

3D相机—TOF

TOF（Time of flight）直译为“飞行时间”。其测距原理是通过给目标连续发送光脉冲，然后用传感器接收从物体返回的光，通过探测光脉冲的飞行（往返）时间来得到目标物距离

3D激光扫描仪；
在这里插入图片描述
实际去做相机选型的时候，会考虑成本，精度，大小和重量；

常用去噪算法–最小二乘法

在这里插入图片描述用所有的观测值去拟合模型；

仅适用于噪声占比比较小的情况

常用去噪算法–RANSAC算法

只要噪声是杂乱无章的，哪怕噪声占比比较大，也能够很好地去噪；
在这里插入图片描述

RANSAC更新迭代次数的代码

在这里插入图片描述
如何提取图片的特征

在这里插入图片描述

在这里插入图片描述生成高斯差分金字塔（DOG金字塔），尺度空间构建

在这里插入图片描述

分配给关键点的方向并不直接是关键点的梯度方向，而是按照一种梯度方向直方图的方式给出的。

具体的方法是：计算以关键点为中心的邻域内所有点的梯度方向，当然梯度方向一定是在0~360°范围内，对这些梯度方向归一化到36个方向内，每个方向代表了10°的范围。然后累计落到每个方向内的关键点个数，以此生成梯度方向直方图。

在这里插入图片描述

将梯度方向直方图中纵坐标最大的项代表的方向分配给当前关键点作为主方向.

关键点描述
对关键点的描述是后续实现匹配的关键步骤，描述其实就是一种以数学方式定义关键的过程。描述子不但包含关键点，也包括关键点周围对其有贡献的邻域点。
描述的思路是：对关键点周围像素区域分块，计算快内梯度直方图，生成具有独特性的向量，这个向量是该区域图像信息的一种抽象表述。
如下图，对于22块，每块的所有像素点的荼毒做高斯加权，每块最终取8个方向，即可以生成228维度的向量，以这22*8维向量作为中心关键点的数学描述。

在这里插入图片描述

在这里插入图片描述
David G.Lowed的实验结果表明：对每个关键点，采用448共128维向量的
描述子进项关键点表征，综合效果最佳：

安装PCL库
安装Cloudcompare
跑一下opencv官方特征点检测匹配的算法

什么是6D姿态

6D姿态（6D pose）是指物体在三维空间中的位置和方向，通常用6个自由度来描述。这6个自由度包括物体在三维空间中的三个平移自由度和三个旋转自由度。

在计算机视觉和机器人领域，6D姿态通常用于物体识别、跟踪和姿态估计等任务。例如，在自动化物流中，机器人需要识别和抓取不同形状和大小的物体，这就需要准确估计物体的6D姿态。

6D姿态的估计通常通过使用传感器数据（如摄像头、激光雷达等）和计算机视觉算法来实现。其中，计算机视觉算法可以通过检测物体的关键点或边缘等特征，进而对物体的6D姿态进行估计和跟踪。

6D姿态的准确估计对于许多实际应用非常重要，例如机器人操作、虚拟现实和增强现实等领域。随着深度学习和计算机视觉技术的不断发展，6D姿态的估计能力也在不断提高。

6D是指6个自由度，代表了3个自由度的位移(也叫平移(Translation))，以及3个自由度的空间旋转(Rotation)，合起来就叫位姿(Pose)。位姿是一个相对的概念，指的是两个坐标系之间的位移和旋转变换。

在这里插入图片描述
6D姿态检测视频展示：
https://www.bilibili.com/s/video/BV18a411w7TR

霍夫变换求直线

霍夫变换（Hough Transform）是一种图像处理算法，用于检测几何形状（例如直线、圆和椭圆等）在图像中的位置。

霍夫变换求解直线的过程如下：

边缘检测：首先对图像进行边缘检测，提取出图像中的边缘。
构建霍夫空间：对于每一个边缘点，计算其与图像中所有其他边缘点之间的直线方程（即斜率和截距），并将这些直线方程映射到霍夫空间中。霍夫空间是一个二维平面，其中横轴表示截距，纵轴表示斜率。
求解直线：在霍夫空间中，可以通过查找在同一直线上的点的数量来确定直线。具体来说，对于每一个点，计算其对应的直线方程，并统计在该直线上的点的数量。如果该数量超过了阈值，则可以认为在图像中存在该直线。
从霍夫空间到图像空间的转换：确定直线后，可以将其从霍夫空间映射回图像空间，得到在图像中的直线。

霍夫变换对于直线检测具有很好的鲁棒性，可以处理图像中存在的噪声和部分遮挡等问题。但是，它的计算复杂度较高，对于大规模图像处理需要进行优化。
霍夫变换（Hough Transform）是一种图像处理算法，用于检测几何形状（例如直线、圆和椭圆等）在图像中的位置。

霍夫变换求解直线的过程如下：

边缘检测：首先对图像进行边缘检测，提取出图像中的边缘。

构建霍夫空间：对于每一个边缘点，计算其与图像中所有其他边缘点之间的直线方程（即斜率和截距），并将这些直线方程映射到霍夫空间中。霍夫空间是一个二维平面，其中横轴表示截距，纵轴表示斜率。

求解直线：在霍夫空间中，可以通过查找在同一直线上的点的数量来确定直线。具体来说，对于每一个点，计算其对应的直线方程，并统计在该直线上的点的数量。如果该数量超过了阈值，则可以认为在图像中存在该直线。

从霍夫空间到图像空间的转换：确定直线后，可以将其从霍夫空间映射回图像空间，得到在图像中的直线。

霍夫变换对于直线检测具有很好的鲁棒性，可以处理图像中存在的噪声和部分遮挡等问题。但是，它的计算复杂度较高，对于大规模图像处理需要进行优化。
在这里插入图片描述

1、首先边缘检测，得到许多点
2、遍历第一个点的各个\theta方向的\rho，得到第一个点的曲线
3、遍历每一个点，得到多条曲线
4、多曲线相交，就是最终的\rho和\theta，通过它可以确定最终直线

霍夫圆检测

霍夫圆检测（Hough Circle Detection）是一种图像处理算法，用于检测图像中的圆形。

霍夫圆检测的基本思路如下：

边缘检测：首先对图像进行边缘检测，提取出图像中的边缘。
构建霍夫空间：对于每一个边缘点，计算其与图像中所有其他边缘点之间的圆心和半径，并将这些圆心和半径映射到霍夫空间中。霍夫空间是一个三维空间，其中三个维度分别表示圆心的横坐标、纵坐标和半径。
求解圆：在霍夫空间中，可以通过查找在同一圆上的点的数量来确定圆。具体来说，对于每一个点，计算其对应的圆心和半径，并统计在该圆上的点的数量。如果该数量超过了阈值，则可以认为在图像中存在该圆。
从霍夫空间到图像空间的转换：确定圆后，可以将其从霍夫空间映射回图像空间，得到在图像中的圆。

霍夫圆检测对于圆形检测具有很好的鲁棒性，可以处理图像中存在的噪声和部分遮挡等问题。但是，它的计算复杂度较高，对于大规模图像处理需要进行优化。此外，由于霍夫圆检测需要预先定义半径范围，因此对于不同大小的圆需要设置不同的参数，这也是其应用上的一些限制。
在这里插入图片描述

2D模板匹配—广义霍夫模板匹配

2D模板匹配是指在一张大的图像中寻找与一个小的模板图像最相似的区域。广义霍夫模板匹配（Generalized Hough Transform）是一种经典的2D模板匹配算法，它可以处理任意形状的模板，并且对于旋转、缩放、平移等变换具有很好的鲁棒性。

广义霍夫模板匹配的基本思路如下：

预处理：对于模板图像，首先需要提取其边缘信息，并计算出每个边缘点的梯度方向和梯度幅值。
构建霍夫空间：对于每个边缘点，根据其梯度方向和梯度幅值，在霍夫空间中投票。霍夫空间是一个参数空间，其中每个点表示一个可能的匹配位置，每个维度表示一个参数（例如平移、旋转或缩放等）。
求解最优匹配：在霍夫空间中，可以通过查找投票数量最多的点来确定最优匹配位置和参数。具体来说，可以设置一个阈值，只有投票数量超过阈值的点才被认为是有效的匹配点。在找到最优匹配点之后，就可以得到模板在图像中的位置、旋转角度、缩放比例等信息。

广义霍夫模板匹配对于模板匹配具有很好的鲁棒性和准确性，但是需要预处理模板图像，计算复杂度较高。此外，对于大规模图像处理，需要进行优化以提高效率。
在这里插入图片描述
推荐视频：
https://www.youtube.com/watch?v=_mGxmZWs9Zw

在这里插入图片描述
霍夫变换的特点：
1.哪怕模板有些不连续的边缘，霍夫变换也是work的；
因为我们无需固定的参数方式对其进行表达；
2.对于遮挡和噪声有很强的容忍性；
3.对于简单的形状是非常高效的（比如直线，圆等等，因为参数空间的参数量小）4.对于复杂的形状我们可以用广义霍夫变换，但是参数空间比较多的时候，效率
会很低；（除非我们可以根据先验缩小参数空间的搜索范围；）

'''
Created on May 19, 2013

@author: vinnie
'''

import os
import cv2
import numpy as np
import matplotlib.pyplot as plt
from collections import defaultdict
from skimage.feature import canny
from scipy.ndimage.filters import sobel

# Good for the b/w test images used
MIN_CANNY_THRESHOLD = 10
MAX_CANNY_THRESHOLD = 50
#计算梯度的水平夹角
def gradient_orientation(image):
    '''
    Calculate the gradient orientation for edge point in the image
    '''
    dx = sobel(image, axis=0, mode='constant')
    dy = sobel(image, axis=1, mode='constant')
    gradient = np.arctan2(dy,dx) * 180 / np.pi
    
    return gradient
#根据点的水平梯度的夹角建立r向量表
def build_r_table(image, origin):
    '''
    Build the R-table from the given shape image and a reference point
    '''
    edges = cv2.Canny(image, MIN_CANNY_THRESHOLD,
                  MAX_CANNY_THRESHOLD)
    gradient = gradient_orientation(edges)
    
    r_table = defaultdict(list)
    for (i,j),value in np.ndenumerate(edges):
        if value:
            r_table[gradient[i,j]].append((origin[0]-i, origin[1]-j))

    return r_table
#进行霍夫检测，对累加器进行投票
def accumulate_gradients(r_table, grayImage):
    '''
    Perform a General Hough Transform with the given image and R-table
    '''
    edges = cv2.Canny(grayImage, MIN_CANNY_THRESHOLD,
                  MAX_CANNY_THRESHOLD)
    gradient = gradient_orientation(edges)
    
    accumulator = np.zeros(grayImage.shape)
    for (i,j),value in np.ndenumerate(edges):
        if value:#value不等于0意味着该点是有梯度的
            for r in r_table[gradient[i,j]]:#gradient[i,j]是待检图片该梯度点的角度值
                accum_i, accum_j = int(i+r[0]), int(j+r[1])
                if accum_i < accumulator.shape[0] and accum_j < accumulator.shape[1]:
                    accumulator[accum_i, accum_j] += 1
                    
    return accumulator
#根据参考图片生成参考点和参考特征
def general_hough_closure(reference_image):
    '''
    Generator function to create a closure with the reference image and origin
    at the center of the reference image
    
    Returns a function f, which takes a query image and returns the accumulator
    '''
    referencePoint = (reference_image.shape[0]/2, reference_image.shape[1]/2)#参考点取图片的中点
    r_table = build_r_table(reference_image, referencePoint)
    
    def f(query_image):
        return accumulate_gradients(r_table, query_image)
        
    return f

def n_max(a, n):
    '''
    Return the N max elements and indices in a
    '''
    indices = a.ravel().argsort()[-n:]
    indices = (np.unravel_index(i, a.shape) for i in indices)
    return [(a[i], i) for i in indices]

def test_general_hough(gh, reference_image, query):
    '''
    Uses a GH closure to detect shapes in an image and create nice output
    '''
    query_image = cv2.imread(query,0)
    accumulator = gh(query_image)

    plt.clf()
    plt.gray()
     
    fig = plt.figure()
    fig.add_subplot(2,2,1)
    plt.title('Reference image')
    plt.imshow(reference_image)#参考图片也就是模板图片
    
    fig.add_subplot(2,2,2)
    plt.title('Query image')
    plt.imshow(query_image)#query_image就是待检图片
    
    fig.add_subplot(2,2,3)
    plt.title('Accumulator')
    plt.imshow(accumulator)
    
    fig.add_subplot(2,2,4)
    plt.title('Detection')
    plt.imshow(query_image)
    
    # top 5 results in red
    m = n_max(accumulator, 2)
    y_points = [pt[1][0] for pt in m]
    x_points = [pt[1][1] for pt in m] 
    plt.scatter(x_points, y_points, marker='o', color='r')

    # top result in yellow
    i,j = np.unravel_index(accumulator.argmax(), accumulator.shape)
    plt.scatter([j], [i], marker='x', color='y')
    
    d,f = os.path.split(query)[0], os.path.splitext(os.path.split(query)[1])[0]
    plt.savefig(os.path.join(d, f + '_output.png'))
    
    return

def test():
    reference_image = cv2.imread(r"..\\images\\s.png",0)
    detect_s = general_hough_closure(reference_image)
    test_general_hough(detect_s, reference_image, "../images/s_test.png")
    
    # reference_image = cv2.imread("../images/diamond.png",0)
    # detect_s = general_hough_closure(reference_image)
    # test_general_hough(detect_s, reference_image, "../images/diamond_test1.png")
    # test_general_hough(detect_s, reference_image, "../images/diamond_test2.png")
    # test_general_hough(detect_s, reference_image, "../images/diamond_test3.png")
    # test_general_hough(detect_s, reference_image, "../images/diamond_test4.png")
    # test_general_hough(detect_s, reference_image, "../images/diamond_test5.png")
    # test_general_hough(detect_s, reference_image, "../images/diamond_test6.png")

if __name__ == '__main__':
    test()

util.hpp

/*
 * util.hpp
 *
 *  Created on: 15 f�vr. 2014
 *      Author: J�r�my
 */

#ifndef UTIL_HPP_
#define UTIL_HPP_

const double PI = 4.0*atan(1.0);

cv::Mat gradientY(const cv::Mat &src);
cv::Mat gradientX(const cv::Mat &src);
cv::Mat gradientDirection(const cv::Mat& src);
void invertIntensities(const cv::Mat& src, cv::Mat& dst);
float gradientDirection(const cv::Mat& src, int x, int y);
float fastsqrt(float val);
int rad2SliceIndex(double angle, int nSlices);

#endif /* UTIL_HPP_ */

util.cpp

/*
 * util.cpp
 *
 *  Created on: 15 f�vr. 2014
 *      Author: J�r�my
 */

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <math.h>
#include <iostream>
#include <iostream>

#include "util.hpp"

using namespace cv;
using namespace std;

int rad2SliceIndex(double angle, int nSlices) {
	double a = (angle > 0) ? (fmodf(angle,2*PI)) : (fmodf(angle+2*PI,2*PI));
	return floor(a / (2 * PI / nSlices));// +0.00000001) );
}

Mat gradientY(const Mat &src) {
	return gradientX(src.t()).t();
}

Mat gradientX(const Mat &src) {
	Mat dst(src.rows,src.cols,CV_32F);
	for (int y=0 ; y<src.rows ; y++) {
		const uchar *srcRow = src.ptr<uchar>(y);
		float *dstRow = dst.ptr<float>(y);
		dstRow[0] = srcRow[1] - srcRow[0];
		for(int x=1 ; x<src.cols-1 ; x++)
			dstRow[x] = srcRow[x+1] - srcRow[x-1];
		dstRow[src.cols-1] = srcRow[src.cols-1] - srcRow[src.cols-2];
	}
	return dst;
}

float gradientDirection(const Mat& src, int x, int y) {
	int gx,gy;
	if(x==0)				gx = src.at<uchar>(y,x+1) - src.at<uchar>(y,x);
	else if(x==src.cols-1)	gx = src.at<uchar>(y,x) - src.at<uchar>(y,x-1);
	else					gx = src.at<uchar>(y,x+1) - src.at<uchar>(y,x-1);
	if(y==0)				gy = src.at<uchar>(y+1,x) - src.at<uchar>(y,x);
	else if(y==src.rows-1)	gy = src.at<uchar>(y,x) - src.at<uchar>(y-1,x);
	if(y==0)				gy = src.at<uchar>(y+1,x) - src.at<uchar>(y-1,x);
	return atan2(gx,gy);
}
//对于图片计算梯度方向
Mat gradientDirection(const Mat& src) {
	Mat dst(src.size(),CV_64F);
//	Mat gradX = gradientX(src);
//	Mat gradY = gradientY(src);
	Mat gradX(src.size(), CV_64F);
	Sobel(src, gradX, CV_64F, 1,0,5);//kernel_size=5
	Mat gradY(src.size(), CV_64F);
	Sobel(src, gradY, CV_64F, 0,1,5);
	double t;
	for(int y=0 ; y<gradX.rows ; y++)
		for(int x=0 ; x<gradX.cols ; x++) {
			t = atan2(gradY.at<double>(y,x), gradX.at<double>(y,x));
			//dst.at<double>(y,x) = (t == 180) ? 0 : t;  
			dst.at<double>(y, x) =  t;
		}
	return dst;
}

void invertIntensities(const Mat& src, Mat& dst) {
	for(int i=0 ; i<src.rows ; i++)
		for(int j=0 ; j<src.cols ; j++)
			dst.at<uchar>(i,j) = 255 - src.at<uchar>(i,j);
}

float fastsqrt(float val) {
	int tmp = *(int *)&val;
	tmp -= 1<<23;
	tmp = tmp >> 1;
	tmp += 1<<29;
	return *(float *)&tmp;
}

main.cpp

/*
 * main.cpp
 *
 *  Created on: 15 f�vr. 2014
 *      Author: J�r�my
 */

#include <iostream>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "GeneralHoughTransform.hpp"

using namespace std;
using namespace cv;

int main() {
	int i = 1;
	Mat tpl = cv::imread("D:\\BaiduNetdiskDownload\\6Dcode\\generalized-hough-tranform-master\\res\\tpl.jpg");
	Mat src = imread("D:\\BaiduNetdiskDownload\\6Dcode\\generalized-hough-tranform-master\\res\\image.jpg");
	GeneralHoughTransform ght(tpl);//构建模板；

 	Size s( src.size().width / 4, src.size().height / 4);//把待检测图片压缩为原图的1/4
 	resize( src, src, s, 0, 0, cv::INTER_AREA);

 	imshow("debug - image", src);

 	ght.accumulate(src);//执行检测

	return 0;
}

GeneralHoughTransform.hpp

/*
 * GeneralHoughTransform.hpp
 *
 *  Created on: 14 févr. 2014
 *      Author: jguillon
 */

#ifndef GENERALHOUGHTRANSFORM_HPP_
#define GENERALHOUGHTRANSFORM_HPP_

struct GHTPoint {
	double phi;
	double s;
	cv::Point y;
	double hits;
};

class GeneralHoughTransform {

public:
	std::vector< std::vector<cv::Vec2f> > m_RTable;
	cv::Vec2f m_origin;
	cv::Mat m_templateImage;
	cv::Mat m_grayTemplateImage;
	cv::Mat m_template;

	int m_cannyThreshold1=150;
	int m_cannyThreshold2=200;
	double m_minPositivesDistance;
	double m_deltaScaleRatio = 0.01;
	double m_minScaleRatio = 0.9;
	double m_maxScaleRatio = 1.1;
	double m_deltaRotationAngle = 3.1415 / 512.0/64.0;
	double m_minRotationAngle = 0.8 * 3.1415;
	double m_maxRotationAngle = 1.2 * 3.1415;
	int m_nScales;
	int m_nRotations;
	int m_nSlices = 64*16*64;
private:
	void createRTable();
	void findOrigin();
	std::vector< std::vector<cv::Vec2f> > scaleRTable(const std::vector< std::vector<cv::Vec2f> >& RTable, double ratio);
	std::vector< std::vector<cv::Vec2f> > rotateRTable(const std::vector< std::vector<cv::Vec2f> >& RTable, double angle);
	void showRTable(std::vector< std::vector<cv::Vec2f> > RTable);

public:
	GeneralHoughTransform(const cv::Mat& templateImage);
	void accumulate(const cv::Mat& image);
	void drawTemplate(cv::Mat& image, GHTPoint params);
	std::vector<GHTPoint> findTemplates(std::vector< std::vector< cv::Mat > >& accum, int threshold);
	void setTemplate(const cv::Mat& templateImage);

};

#endif /* GENERALHOUGHTRANSFORM_HPP_ */

GeneralHoughTransform.cpp

/*
 * GeneralHoughTransform.cpp
 *
 *  Created on: 14 févr. 2014
 *      Author: jguillon
 */

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream> // For debugging
#include <opencv2\imgproc\types_c.h>
#include "GeneralHoughTransform.hpp"
#include "util.hpp"

using namespace cv;
using namespace std;

GeneralHoughTransform::GeneralHoughTransform(const Mat& templateImage) {
	/* Parameters to set */

	/* Computed attributes */
	m_nRotations = (m_maxRotationAngle - m_minRotationAngle) / m_deltaRotationAngle + 1;//待遍历的角度区间的个数
	m_nSlices = 64;//角度划分的bins
	m_nScales = (m_maxScaleRatio - m_minScaleRatio) / m_deltaScaleRatio + 1;//待遍历的尺度

	setTemplate(templateImage);
}

void GeneralHoughTransform::setTemplate(const Mat& templateImage) {
	templateImage.copyTo(m_templateImage);
 	resize( m_templateImage, m_templateImage, Size(templateImage.size().width / 4, templateImage.size().height / 4));
	findOrigin();
	cvtColor(m_templateImage, m_grayTemplateImage, CV_BGR2GRAY);
	m_grayTemplateImage.convertTo(m_grayTemplateImage, CV_8UC1);
	m_template = Mat(m_grayTemplateImage.size(), CV_8UC1);
	blur(m_grayTemplateImage, m_template, Size(3,3));
	Canny(m_template, m_template, m_cannyThreshold1, m_cannyThreshold2);
	createRTable();
}
//找参考点，原始的参考点是图片的中点，？？现在找一个红色的点作为替代点？？
void GeneralHoughTransform::findOrigin() {
	m_origin = Vec2f(m_templateImage.cols/2,m_templateImage.rows/2); // By default, the origin is at the center
	/*
	for(int j=0 ; j<m_templateImage.rows ; j++) {
		Vec3b* data= (Vec3b*)(m_templateImage.data + m_templateImage.step.p[0]*j);
		for(int i=0 ; i<m_templateImage.cols ; i++)
			if(data[i]==Vec3b(0,0,255)) { // If it's a red pixel...
				m_origin = Vec2f(i,j); // ...then it's the template's origin
			}
	}
	*/
}

void GeneralHoughTransform::createRTable() {
	int iSlice;
	double phi;

	Mat direction = gradientDirection(m_template);
	imshow("debug - template", m_template);
	imshow("debug - positive directions", direction);

	m_RTable.clear();
	m_RTable.resize(m_nSlices);//m_nSlices待遍历的角度
	for(int y=0 ; y<m_template.rows ; y++) {
		uchar *templateRow = m_template.ptr<uchar>(y);
		double *directionRow = direction.ptr<double>(y);
		for(int x=0 ; x<m_template.cols ; x++) {
			if(templateRow[x] == 255) {//如果是边缘点的话
				phi = directionRow[x]; // gradient direction in radians in [-PI;PI]
				iSlice = rad2SliceIndex(phi,m_nSlices);
				m_RTable[iSlice].push_back(Vec2f(m_origin[0]-x, m_origin[1]-y));
			}
		}
	}
}

vector< vector<Vec2f> > GeneralHoughTransform::scaleRTable(const vector< vector<Vec2f> >& RTable, double ratio) {
	vector< vector<Vec2f> > RTableScaled(RTable.size());
	for(vector< vector<Vec2f> >::size_type iSlice=0 ; iSlice<RTable.size() ; iSlice++) {
		for(vector<Vec2f>::size_type ir=0 ; ir<RTable[iSlice].size() ; ir++) {
			RTableScaled[iSlice].push_back(Vec2f(ratio*RTable[iSlice][ir][0], ratio*RTable[iSlice][ir][1]));
		}
	}
	return RTableScaled;
}

vector< vector<Vec2f> > GeneralHoughTransform::rotateRTable(const vector< vector<Vec2f> >& RTable, double angle) {
	vector< vector<Vec2f> > RTableRotated(RTable.size());
	double c = cos(angle);
	double s = sin(angle);
	int iSliceRotated;
	for(vector< vector<Vec2f> >::size_type iSlice = 0 ; iSlice<RTable.size() ; iSlice++) {
		iSliceRotated = rad2SliceIndex(iSlice*m_deltaRotationAngle + angle, m_nSlices);
		for(vector<Vec2f>::size_type ir=0 ; ir<RTable[iSlice].size(); ir++) {
			RTableRotated[iSliceRotated].push_back(Vec2f(c*RTable[iSlice][ir][0] - s*RTable[iSlice][ir][1], s*RTable[iSlice][ir][0] + c*RTable[iSlice][ir][1]));
		}
	}
	return RTableRotated;
}

void GeneralHoughTransform::showRTable(vector< vector<Vec2f> > RTable) {
	int N(0);
	cout << "--------" << endl;
	for(vector< vector<Vec2f> >::size_type i=0 ; i<RTable.size() ; i++) {
		for(vector<Vec2f>::size_type j=0 ; j<RTable[i].size() ; j++) {
			cout << RTable[i][j];
			N++;
		}
		cout << endl;
	}
	cout << N << " elements" << endl;
}

void GeneralHoughTransform::accumulate(const Mat& image) {
	/* Image preprocessing */
	Mat grayImage(image.size(), CV_8UC1), edges(image.size(), CV_8UC1);
	cvtColor(image, edges, CV_BGR2GRAY);
	blur(edges, edges, Size(3,3));
	Canny(edges, edges, m_cannyThreshold1, m_cannyThreshold2);
	Mat direction = gradientDirection(edges);

	/* Debug */
	imshow("debug - src edges", edges);
	imshow("debug - src edges gradient direction", direction);
	//waitKey(0);

	/* Accum size setting */
	int X = image.cols;
	int Y = image.rows;
	int S = ceil((m_maxScaleRatio - m_minScaleRatio) / m_deltaScaleRatio) + 1; // Scale Slices Number
	int R = ceil((m_maxRotationAngle - m_minRotationAngle) / m_deltaRotationAngle) + 1; // Rotation Slices Number

	/* Usefull variablaration */
	vector< vector< Mat > > accum(R,vector<Mat>(S, Mat::zeros(Size(X,Y),CV_64F)));
	Mat totalAccum = Mat::zeros(Size(X,Y),CV_32S);
	int iSlice(0), iScaleSlice(0), iRotationSlice(0), ix(0), iy(0);
	double max(0.0), phi(0.0);
	vector< vector<Vec2f> > RTableRotated(m_RTable.size()), RTableScaled(m_RTable.size());
	Mat showAccum(Size(X,Y),CV_8UC1);
	vector<GHTPoint> points;
	GHTPoint point;
	max = 0;
	/* Main loop */
	for(double angle=m_minRotationAngle ; angle<=m_maxRotationAngle+0.0001 ; angle+=m_deltaRotationAngle) { // For each rotation (0.0001 double comparison)
		iRotationSlice = round((angle-m_minRotationAngle)/m_deltaRotationAngle);
		cout << "Rotation Angle\t: " << angle/PI*180 << "°" << endl;
		RTableRotated = rotateRTable(m_RTable,angle);
		for(double ratio=m_minScaleRatio ; ratio<=m_maxScaleRatio+0.0001 ; ratio+=m_deltaScaleRatio) { // For each scaling (0.0001 double comparison)
 			iScaleSlice = round((ratio-m_minScaleRatio)/m_deltaScaleRatio);
			cout << "|- Scale Ratio\t: " << ratio*100 << "%" << endl;
			RTableScaled = scaleRTable(RTableRotated,ratio);
			accum[iRotationSlice][iScaleSlice] = Mat::zeros(Size(X,Y),CV_64F);
			
			for(int y=0 ; y<image.rows ; y++) {
				for(int x=0 ; x<image.cols ; x++) {
					phi = direction.at<double>(y,x);
					if(phi != 0.0) {
						iSlice = rad2SliceIndex(phi,m_nSlices);
						for(vector<Vec2f>::size_type ir=0 ; ir<RTableScaled[iSlice].size() ; ir++) { // For each r related to this angle-slice
							ix = x + round(RTableScaled[iSlice][ir][0]);	// We compute x+r, the supposed template origin position
							iy = y + round(RTableScaled[iSlice][ir][1]);
							if(ix>=0 && ix<image.cols && iy>=0 && iy<image.rows) { // If it's between the image boundaries
								totalAccum.at<int>(iy,ix)++;
								if(++accum[iRotationSlice][iScaleSlice].at<double>(iy,ix) > max) { // Icrement the accum
									max = accum[iRotationSlice][iScaleSlice].at<double>(iy,ix);
									point.phi = angle;
									point.s = ratio;
									point.y.y = iy;
									point.y.x = ix;
									point.hits = accum[iRotationSlice][iScaleSlice].at<double>(iy,ix);
								}
								/* To see the step-by-step accumulation uncomment these lines */
								// normalize(accum[iRotationSlice][iScaleSlice], showAccum, 0, 255, NORM_MINMAX, CV_8UC1);
								// imshow("debug - subaccum", showAccum);	waitKey(1);
							}
						}
					}
				}
			}
			/* Pushing back the best point for each transformation (uncomment line 159 : "max = 0") */
			points.push_back(point);
			/* Transformation accumulation visualisation */
			normalize(accum[iRotationSlice][iScaleSlice], showAccum, 0, 255, NORM_MINMAX, CV_8UC1); // To see each transformation accumulation (uncomment line 159 : "max = 0")
			// normalize(totalAccum, showAccum, 0, 255, NORM_MINMAX, CV_8UC1); // To see the cumulated accumulation (comment line 159 : "max = 0")
			imshow("debug - accum", showAccum);	//waitKey(1);
			// blur(accum[iRotationSlice][iScaleSlice], accum[iRotationSlice][iScaleSlice], Size(3,3)); // To harmonize the local maxima
		}
	}
	/* Pushing back the best point for cumulated transformations (comment line 159 : "max = 0") */
	// points.push_back(point);

	/* Drawing templates on best points */
	Mat out(image.size(), image.type());
	image.copyTo(out);
	//for(vector<GHTPoint>::size_type i=0 ; i<points.size() ; i++) {
	cv::circle(out,point.y,3,cv::Scalar(0,0,255));
		drawTemplate(out, point);

		//int msk = 1;
	//}
	imshow("debug - output", out);
	waitKey(0);
}

vector<GHTPoint> GeneralHoughTransform::findTemplates(vector< vector< Mat > >& accum, int threshold) {
	vector<GHTPoint> newPoints;

	//TODO

	return newPoints;
}

void GeneralHoughTransform::drawTemplate(Mat& image, GHTPoint params) {
	cout << params.y << " avec un rapport de grandeur de " << params.s << " et une rotation de " << params.phi/PI*180 << "° et avec " << params.hits << " !" << endl;
	double c = cos(params.phi);
	double s = sin(params.phi);
	int x(0), y(0), relx(0), rely(0);
	for(vector< vector<Vec2f> >::size_type iSlice = 0 ; iSlice<m_RTable.size() ; iSlice++)
		for(vector<Vec2f>::size_type ir=0 ; ir<m_RTable[iSlice].size() ; ir++) {
			relx = params.s * (c*m_RTable[iSlice][ir][0] - s*m_RTable[iSlice][ir][1]); // X-Coordinate of the template's point after transformation (relative to the origin)
			rely = params.s * (s*m_RTable[iSlice][ir][0] + c*m_RTable[iSlice][ir][1]); // Y-Coordinate of the template's point after transformation (relative to the origin)
			x = params.y.x + relx; // X-Coordinate of the template's point in the image
			y = params.y.y + rely; // Y-Coordinate of the template's point in the image
			if(x>=0 && x<image.cols && y>=0 && y<image.rows)
				image.at<Vec3b>(y,x) = Vec3b(0,255,0); // Put the pixel in green
		}
}

GHT3_code

main.cpp

#include"GHT.h"
int main()
{

	GeneralizedHoughGuilImpl1 GHT1;
	cv::Mat mode = cv::imread("D:\\BaiduNetdiskDownload\\6Dcode\\generalized-hough-tranform-master\\res\\template.jpg",0);
	cv::Mat image = cv::imread("D:\\BaiduNetdiskDownload\\6Dcode\\generalized-hough-tranform-master\\res\\image.jpg",0);
	cv::Mat mode_positions;
	cv::Mat mode_votes;
	GHT1.setTemplate(mode);
	GHT1.detect(image, mode_positions, mode_votes);
	return 1;
}

GHT.h

#include<opencv2/opencv.hpp>
#include <functional>
#include <limits>

using namespace cv;

// common


double toRad(double a)
{
    return a * 3.1415 / 180.0;
}

bool notNull(float v)
{
    return fabs(v) > std::numeric_limits<float>::epsilon();
}

 class GeneralizedHoughBase1
{
protected:
    GeneralizedHoughBase1();
   // virtual ~GeneralizedHoughBase1() {}

public:
    void detectImpl(cv::Mat image, cv::Mat positions, cv::Mat votes);
    void detectImpl(cv::Mat edges, cv::Mat dx, cv::Mat dy, cv::Mat positions, cv::Mat votes);

    virtual void processTempl() = 0;
    virtual void processImage() = 0;

    int cannyLowThresh_;
    int cannyHighThresh_;
    double minDist_;
    double dp_;

    Size templSize_;
    Point templCenter_;
    cv::Mat templEdges_;
    cv::Mat templDx_;
    cv::Mat templDy_;

    Size imageSize_;
    cv::Mat imageEdges_;
    cv::Mat imageDx_;
    cv::Mat imageDy_;

    std::vector<Vec4f> posOutBuf_;
    std::vector<Vec3i> voteOutBuf_;

//private:
//protected:
    void calcEdges(cv::Mat src, cv::Mat& edges, cv::Mat& dx, cv::Mat& dy);
    void filterMinDist();
    void convertTo(cv::Mat positions, cv::Mat votes);
};

GeneralizedHoughBase1::GeneralizedHoughBase1()
{
    cannyLowThresh_ = 50;
    cannyHighThresh_ = 100;
    minDist_ = 1.0;
    dp_ = 1.0;
}
//�����Ե
void GeneralizedHoughBase1::calcEdges(cv::Mat src, cv::Mat& edges, cv::Mat& dx, cv::Mat& dy)
{


    CV_Assert(src.type() == CV_8UC1);
    CV_Assert(cannyLowThresh_ > 0 && cannyLowThresh_ < cannyHighThresh_);

    Canny(src, edges, cannyLowThresh_, cannyHighThresh_);
    Sobel(src, dx, CV_32F, 1, 0);
    Sobel(src, dy, CV_32F, 0, 1);
}


void GeneralizedHoughBase1::detectImpl(cv::Mat image, cv::Mat positions, cv::Mat votes)
{
    calcEdges(image, imageEdges_, imageDx_, imageDy_);

    imageSize_ = imageEdges_.size();

    posOutBuf_.clear();
    voteOutBuf_.clear();

    processImage();

    if (!posOutBuf_.empty())
    {
        if (minDist_ > 1)
            filterMinDist();
        convertTo(positions, votes);
    }
    else
    {
        positions.release();
        //if (votes.needed())
        //    votes.release();
    }
}

void GeneralizedHoughBase1::detectImpl(cv::Mat edges, cv::Mat dx, cv::Mat dy, cv::Mat positions, cv::Mat votes)
{
    edges.copyTo(imageEdges_);
    dx.copyTo(imageDx_);
    dy.copyTo(imageDy_);

    CV_Assert(imageEdges_.type() == CV_8UC1);
    CV_Assert(imageDx_.type() == CV_32FC1 && imageDx_.size() == imageEdges_.size());
    CV_Assert(imageDy_.type() == imageDx_.type() && imageDy_.size() == imageEdges_.size());

    imageSize_ = imageEdges_.size();

    posOutBuf_.clear();
    voteOutBuf_.clear();

    processImage();

    if (!posOutBuf_.empty())
    {
        if (minDist_ > 1)
            filterMinDist();
        convertTo(positions, votes);
    }
    else
    {
        positions.release();
        //  if (votes.needed())
         //     votes.release();
    }
}

class Vec3iGreaterThanIdx
{
public:
    Vec3iGreaterThanIdx(const Vec3i* _arr) : arr(_arr) {}
    bool operator()(size_t a, size_t b) const { return arr[a][0] > arr[b][0]; }
    const Vec3i* arr;
};

void GeneralizedHoughBase1::filterMinDist()
{
    size_t oldSize = posOutBuf_.size();
    const bool hasVotes = !voteOutBuf_.empty();

    CV_Assert(!hasVotes || voteOutBuf_.size() == oldSize);

    std::vector<Vec4f> oldPosBuf;// = posOutBuf_;
    std::vector<Vec3i> oldVoteBuf;// (voteOutBuf_);

    std::vector<size_t> indexies(oldSize);
    for (size_t i = 0; i < oldSize; ++i)
        indexies[i] = i;
    std::sort(indexies.begin(), indexies.end(), Vec3iGreaterThanIdx(&oldVoteBuf[0]));

    posOutBuf_.clear();
    voteOutBuf_.clear();

    const int cellSize = cvRound(minDist_);
    const int gridWidth = (imageSize_.width + cellSize - 1) / cellSize;
    const int gridHeight = (imageSize_.height + cellSize - 1) / cellSize;

    std::vector< std::vector<Point2f> > grid(gridWidth * gridHeight);

    const double minDist2 = minDist_ * minDist_;

    for (size_t i = 0; i < oldSize; ++i)
    {
        const size_t ind = indexies[i];

        Point2f p(oldPosBuf[ind][0], oldPosBuf[ind][1]);

        bool good = true;

        const int xCell = static_cast<int>(p.x / cellSize);
        const int yCell = static_cast<int>(p.y / cellSize);

        int x1 = xCell - 1;
        int y1 = yCell - 1;
        int x2 = xCell + 1;
        int y2 = yCell + 1;

        // boundary check
        x1 = std::max(0, x1);
        y1 = std::max(0, y1);
        x2 = std::min(gridWidth - 1, x2);
        y2 = std::min(gridHeight - 1, y2);

        for (int yy = y1; yy <= y2; ++yy)
        {
            for (int xx = x1; xx <= x2; ++xx)
            {
                const std::vector<Point2f>& m = grid[yy * gridWidth + xx];

                for (size_t j = 0; j < m.size(); ++j)
                {
                    const Point2f d = p - m[j];

                    if (d.ddot(d) < minDist2)
                    {
                        good = false;
                        goto break_out;
                    }
                }
            }
        }

    break_out:

        if (good)
        {
            grid[yCell * gridWidth + xCell].push_back(p);

            posOutBuf_.push_back(oldPosBuf[ind]);
            if (hasVotes)
                voteOutBuf_.push_back(oldVoteBuf[ind]);
        }
    }
}

void GeneralizedHoughBase1::convertTo(cv::Mat _positions, cv::Mat _votes)
{
    const int total = static_cast<int>(posOutBuf_.size());
    const bool hasVotes = !voteOutBuf_.empty();

    CV_Assert(!hasVotes || voteOutBuf_.size() == posOutBuf_.size());

    _positions.create(1, total, CV_32FC4);
    cv::Mat positions = _positions;// .getMat();
    cv::Mat(1, total, CV_32FC4, &posOutBuf_[0]).copyTo(positions);
    /*
     if (_votes.needed())
     {
         if (!hasVotes)
         {
             _votes.release();
         }
         else
         {
             _votes.create(1, total, CV_32SC3);
            cv::Mat votes = _votes.getMat();
            cv::Mat(1, total, CV_32SC3, &voteOutBuf_[0]).copyTo(votes);
         }
     }*/
}
//}
class GeneralizedHoughGuilImpl1 : public GeneralizedHoughGuil, private GeneralizedHoughBase1
{
public:
    GeneralizedHoughGuilImpl1();

    void setTemplate(cv::InputArray templ, Point templCenter);// CV_OVERRIDE{ setTemplateImpl(templ, templCenter); }
    void setTemplate(cv::InputArray edges, cv::InputArray dx, cv::InputArray dy, Point templCenter);// CV_OVERRIDE{ setTemplateImpl(edges, dx, dy, templCenter); }

    void detect(cv::InputArray image, cv::OutputArray positions, cv::OutputArray votes);// CV_OVERRIDE{ detectImpl(image, positions, votes); }
    void detect(cv::InputArray edges, cv::InputArray dx, cv::InputArray dy, cv::OutputArray positions, cv::OutputArray votes);// CV_OVERRIDE{ detectImpl(edges, dx, dy, positions, votes); }
    void setTemplate(cv::Mat templ, cv::Point templCenter=cv::Point(-1,-1));
    void setCannyLowThresh(int cannyLowThresh) CV_OVERRIDE { cannyLowThresh_ = cannyLowThresh; }
    int getCannyLowThresh() const CV_OVERRIDE { return cannyLowThresh_; }
    void detect(cv::Mat image, cv::Mat positions, cv::Mat votes);
    void setCannyHighThresh(int cannyHighThresh) CV_OVERRIDE { cannyHighThresh_ = cannyHighThresh; }
    int getCannyHighThresh() const CV_OVERRIDE { return cannyHighThresh_; }

    void setMinDist(double minDist) CV_OVERRIDE { minDist_ = minDist; }
    double getMinDist() const CV_OVERRIDE { return minDist_; }

    void setDp(double dp) CV_OVERRIDE { dp_ = dp; }
    double getDp() const CV_OVERRIDE { return dp_; }

    void setMaxBufferSize(int maxBufferSize) CV_OVERRIDE { maxBufferSize_ = maxBufferSize; }
    int getMaxBufferSize() const CV_OVERRIDE { return maxBufferSize_; }

    void setXi(double xi) CV_OVERRIDE { xi_ = xi; }
    double getXi() const CV_OVERRIDE { return xi_; }

    void setLevels(int levels) CV_OVERRIDE { levels_ = levels; }
    int getLevels() const CV_OVERRIDE { return levels_; }

    void setAngleEpsilon(double angleEpsilon) CV_OVERRIDE { angleEpsilon_ = angleEpsilon; }
    double getAngleEpsilon() const CV_OVERRIDE { return angleEpsilon_; }

    void setMinAngle(double minAngle) CV_OVERRIDE { minAngle_ = minAngle; }
    double getMinAngle() const CV_OVERRIDE { return minAngle_; }

    void setMaxAngle(double maxAngle) CV_OVERRIDE { maxAngle_ = maxAngle; }
    double getMaxAngle() const CV_OVERRIDE { return maxAngle_; }

    void setAngleStep(double angleStep) CV_OVERRIDE { angleStep_ = angleStep; }
    double getAngleStep() const CV_OVERRIDE { return angleStep_; }

    void setAngleThresh(int angleThresh) CV_OVERRIDE { angleThresh_ = angleThresh; }
    int getAngleThresh() const CV_OVERRIDE { return angleThresh_; }

    void setMinScale(double minScale) CV_OVERRIDE { minScale_ = minScale; }
    double getMinScale() const CV_OVERRIDE { return minScale_; }

    void setMaxScale(double maxScale) CV_OVERRIDE { maxScale_ = maxScale; }
    double getMaxScale() const CV_OVERRIDE { return maxScale_; }

    void setScaleStep(double scaleStep) CV_OVERRIDE { scaleStep_ = scaleStep; }
    double getScaleStep() const CV_OVERRIDE { return scaleStep_; }

    void setScaleThresh(int scaleThresh) CV_OVERRIDE { scaleThresh_ = scaleThresh; }
    int getScaleThresh() const CV_OVERRIDE { return scaleThresh_; }

    void setPosThresh(int posThresh) CV_OVERRIDE { posThresh_ = posThresh; }
    int getPosThresh() const CV_OVERRIDE { return posThresh_; }

private:
    void processTempl();
    void processImage();

    int maxBufferSize_;
    double xi_;
    int levels_;
    double angleEpsilon_;

    double minAngle_;
    double maxAngle_;
    double angleStep_;
    int angleThresh_;

    double minScale_;
    double maxScale_;
    double scaleStep_;
    int scaleThresh_;

    int posThresh_;

    struct ContourPoint
    {
        Point2d pos;
        double theta;
    };

    struct Feature
    {
        ContourPoint p1;
        ContourPoint p2;

        double alpha12;
        double d12;

        Point2d r1;
        Point2d r2;
    };

    void buildFeatureList(const cv::Mat& edges, const cv::Mat& dx, const cv::Mat& dy, std::vector< std::vector<Feature> >& features, cv::Point2d center = cv::Point2d());
    void getContourPoints(const cv::Mat& edges, const cv::Mat& dx, const cv::Mat& dy, std::vector<ContourPoint>& points);

    void calcOrientation();
    void calcScale(double angle);
    void calcPosition(double angle, int angleVotes, double scale, int scaleVotes);

    std::vector< std::vector<Feature> > templFeatures_;
    std::vector< std::vector<Feature> > imageFeatures_;

    std::vector< std::pair<double, int> > angles_;
    std::vector< std::pair<double, int> > scales_;
};

void GeneralizedHoughGuilImpl1::setTemplate(cv::Mat templ, cv::Point templCenter)
{

    calcEdges(templ, templEdges_, templDx_, templDy_);

    if (templCenter == Point(-1, -1))
        templCenter = Point(templEdges_.cols / 2, templEdges_.rows / 2);

    templSize_ = templEdges_.size();
    templCenter_ = templCenter;

    processTempl();
}
void GeneralizedHoughGuilImpl1::setTemplate(cv::InputArray templ, cv::Point templCenter)
{
cv:Mat templ_ = templ.getMat();
    calcEdges(templ_, templEdges_, templDx_, templDy_);

    if (templCenter == Point(-1, -1))
        templCenter = Point(templEdges_.cols / 2, templEdges_.rows / 2);

    templSize_ = templEdges_.size();
    templCenter_ = templCenter;

    processTempl();
}

void GeneralizedHoughGuilImpl1::setTemplate(cv::InputArray edges, cv::InputArray dx, cv::InputArray dy, cv::Point templCenter)
{
    edges.copyTo(templEdges_);
    dx.copyTo(templDx_);
    dy.copyTo(templDy_);

    CV_Assert(templEdges_.type() == CV_8UC1);
    CV_Assert(templDx_.type() == CV_32FC1 && templDx_.size() == templEdges_.size());
    CV_Assert(templDy_.type() == templDx_.type() && templDy_.size() == templEdges_.size());

    if (templCenter == Point(-1, -1))
        templCenter = Point(templEdges_.cols / 2, templEdges_.rows / 2);

    templSize_ = templEdges_.size();
    templCenter_ = templCenter;

    processTempl();
}
void GeneralizedHoughGuilImpl1::detect(cv::Mat image, cv::Mat positions, cv::Mat votes)
{

    calcEdges(image, imageEdges_, imageDx_, imageDy_);

    imageSize_ = imageEdges_.size();

    posOutBuf_.clear();
    voteOutBuf_.clear();

    processImage();

    if (!posOutBuf_.empty())
    {
        if (minDist_ > 1)
            filterMinDist();
        convertTo(positions, votes);
    }
    else
    {
        positions.release();
        // if (votes.needed())
         //    votes.release();
    }
}
void GeneralizedHoughGuilImpl1::detect(cv::InputArray image, cv::OutputArray positions, cv::OutputArray votes)
{
    cv::Mat image_ = image.getMat();
    cv::Mat positions_ = positions.getMat();
    cv::Mat votes_ = votes.getMat();
    calcEdges(image_, imageEdges_, imageDx_, imageDy_);

    imageSize_ = imageEdges_.size();

    posOutBuf_.clear();
    voteOutBuf_.clear();

    processImage();

    if (!posOutBuf_.empty())
    {
        if (minDist_ > 1)
            filterMinDist();
        convertTo(positions_, votes_);
    }
    else
    {
        positions_.release();
        // if (votes.needed())
         //    votes.release();
    }
}

void GeneralizedHoughGuilImpl1::detect(cv::InputArray edges, cv::InputArray dx, cv::InputArray dy, cv::OutputArray positions, cv::OutputArray votes)
{
    edges.getMat().copyTo(imageEdges_);
    dx.getMat().copyTo(imageDx_);
    dy.getMat().copyTo(imageDy_);
    cv::Mat positions_ = positions.getMat();
    cv::Mat votes_ = votes.getMat();
    CV_Assert(imageEdges_.type() == CV_8UC1);
    CV_Assert(imageDx_.type() == CV_32FC1 && imageDx_.size() == imageEdges_.size());
    CV_Assert(imageDy_.type() == imageDx_.type() && imageDy_.size() == imageEdges_.size());

    imageSize_ = imageEdges_.size();

    posOutBuf_.clear();
    voteOutBuf_.clear();

    processImage();

    if (!posOutBuf_.empty())
    {
        if (minDist_ > 1)
            filterMinDist();
        convertTo(positions_, votes_);
    }
    else
    {
        positions_.release();
        // if (votes.needed())
         //    votes.release();
    }
}

//namespace
//{

double clampAngle(double a)
{
    double res = a;

    while (res > 360.0)
        res -= 360.0;
    while (res < 0)
        res += 360.0;

    return res;
}

bool angleEq(double a, double b, double eps = 1.0)
{
    return (fabs(clampAngle(a - b)) <= eps);
}

GeneralizedHoughGuilImpl1::GeneralizedHoughGuilImpl1()
{
    maxBufferSize_ = 1000;
    xi_ = 90.0;
    levels_ = 360;
    angleEpsilon_ = 1.0;

    minAngle_ = 0.0;
    maxAngle_ = 360.0;
    angleStep_ = 1.0;
    angleThresh_ = 15000;

    minScale_ = 0.5;
    maxScale_ = 2.0;
    scaleStep_ = 0.05;
    scaleThresh_ = 1000;

    posThresh_ = 100;
}
//����ģ������
void GeneralizedHoughGuilImpl1::processTempl()
{
    buildFeatureList(templEdges_, templDx_, templDy_, templFeatures_, templCenter_);
}
//����Image����������ͶƱ
void GeneralizedHoughGuilImpl1::processImage()
{
    buildFeatureList(imageEdges_, imageDx_, imageDy_, imageFeatures_);
    //���ڽǶ�ͶƱ
    calcOrientation();

    for (size_t i = 0; i < angles_.size(); ++i)
    {
        const double angle = angles_[i].first;
        const int angleVotes = angles_[i].second;
        //���ڳ߶�ͶƱ
        calcScale(angle);

        for (size_t j = 0; j < scales_.size(); ++j)
        {
            const double scale = scales_[j].first;
            const int scaleVotes = scales_[j].second;
            //����λ��ͶƱ
            calcPosition(angle, angleVotes, scale, scaleVotes);
        }
    }
}
//������ȡ��Ե���ͼ��������
void GeneralizedHoughGuilImpl1::buildFeatureList(const cv::Mat& edges, const cv::Mat& dx, const cv::Mat& dy, std::vector< std::vector<Feature> >& features, cv::Point2d center)
{
    CV_Assert(levels_ > 0);
    //�������֮���������
    const double maxDist = sqrt((double)templSize_.width * templSize_.width + templSize_.height * templSize_.height) * maxScale_;
   //�ǶȲ���
    const double alphaScale = levels_ / 360.0;

    std::vector<ContourPoint> points;
    getContourPoints(edges, dx, dy, points);//��ȡ��Ե��

    features.resize(levels_ + 1);
    std::for_each(features.begin(), features.end(), [=](std::vector<Feature>& e) { e.clear(); e.reserve(maxBufferSize_); });

    for (size_t i = 0; i < points.size(); ++i)
    {
        ContourPoint p1 = points[i];

        for (size_t j = 0; j < points.size(); ++j)
        {
            ContourPoint p2 = points[j];

            if (angleEq(p1.theta - p2.theta, xi_, angleEpsilon_))
            {
                const Point2d d = p1.pos - p2.pos;

                Feature f;

                f.p1 = p1;
                f.p2 = p2;

                f.alpha12 = clampAngle(fastAtan2((float)d.y, (float)d.x) - p1.theta);
                f.d12 = norm(d);

                if (f.d12 > maxDist)
                    continue;
                //r1��r2�ֱ���p1,p2������е��λ��
                f.r1 = p1.pos - center;
                f.r2 = p2.pos - center;
                //alpha12����ת����ƽ�Ʋ�����������Դ���Ϊ�������ӵ��feature
                const int n = cvRound(f.alpha12 * alphaScale);

                if (features[n].size() < static_cast<size_t>(maxBufferSize_))
                //if (features[n].size() < 1000)
                    features[n].push_back(f);
                else
                   break;
            }
        }
    }
}

void GeneralizedHoughGuilImpl1::getContourPoints(const cv::Mat& edges, const cv::Mat& dx, const cv::Mat& dy, std::vector<ContourPoint>& points)
{
    CV_Assert(edges.type() == CV_8UC1);
    CV_Assert(dx.type() == CV_32FC1 && dx.size == edges.size);
    CV_Assert(dy.type() == dx.type() && dy.size == edges.size);

    points.clear();
    points.reserve(edges.size().area());

    for (int y = 0; y < edges.rows; ++y)
    {
        const uchar* edgesRow = edges.ptr(y);
        const float* dxRow = dx.ptr<float>(y);
        const float* dyRow = dy.ptr<float>(y);

        for (int x = 0; x < edges.cols; ++x)
        {
            if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))
            {
                ContourPoint p;

                p.pos = Point2d(x, y);
                p.theta = fastAtan2(dyRow[x], dxRow[x]);

                points.push_back(p);
            }
        }
    }
}

void GeneralizedHoughGuilImpl1::calcOrientation()
{
    CV_Assert(levels_ > 0);
    CV_Assert(templFeatures_.size() == static_cast<size_t>(levels_ + 1));
    CV_Assert(imageFeatures_.size() == templFeatures_.size());
    CV_Assert(minAngle_ >= 0.0 && minAngle_ < maxAngle_&& maxAngle_ <= 360.0);
    CV_Assert(angleStep_ > 0.0 && angleStep_ < 360.0);
    CV_Assert(angleThresh_ > 0);

    const double iAngleStep = 1.0 / angleStep_;
    const int angleRange = cvCeil((maxAngle_ - minAngle_) * iAngleStep);

    std::vector<int> OHist(angleRange + 1, 0);
    for (int i = 0; i <= levels_; ++i)
    {
        const std::vector<Feature>& templRow = templFeatures_[i];
        const std::vector<Feature>& imageRow = imageFeatures_[i];

        for (size_t j = 0; j < templRow.size(); ++j)
        {
            Feature templF = templRow[j];

            for (size_t k = 0; k < imageRow.size(); ++k)
            {
                Feature imF = imageRow[k];

                const double angle = clampAngle(imF.p1.theta - templF.p1.theta);
                if (angle >= minAngle_ && angle <= maxAngle_)
                {
                    const int n = cvRound((angle - minAngle_) * iAngleStep);
                    ++OHist[n];
                }
            }
        }
    }

    angles_.clear();

    for (int n = 0; n < angleRange; ++n)
    {
        if (OHist[n] >= angleThresh_)
        {
            const double angle = minAngle_ + n * angleStep_;
            angles_.push_back(std::make_pair(angle, OHist[n]));
        }
    }
}

void GeneralizedHoughGuilImpl1::calcScale(double angle)
{
    CV_Assert(levels_ > 0);
    CV_Assert(templFeatures_.size() == static_cast<size_t>(levels_ + 1));
    CV_Assert(imageFeatures_.size() == templFeatures_.size());
    CV_Assert(minScale_ > 0.0 && minScale_ < maxScale_);
    CV_Assert(scaleStep_ > 0.0);
    CV_Assert(scaleThresh_ > 0);

    const double iScaleStep = 1.0 / scaleStep_;
    const int scaleRange = cvCeil((maxScale_ - minScale_) * iScaleStep);

    std::vector<int> SHist(scaleRange + 1, 0);

    for (int i = 0; i <= levels_; ++i)
    {
        const std::vector<Feature>& templRow = templFeatures_[i];
        const std::vector<Feature>& imageRow = imageFeatures_[i];

        for (size_t j = 0; j < templRow.size(); ++j)
        {
            Feature templF = templRow[j];

            templF.p1.theta += angle;

            for (size_t k = 0; k < imageRow.size(); ++k)
            {
                Feature imF = imageRow[k];

                if (angleEq(imF.p1.theta, templF.p1.theta, angleEpsilon_))
                {
                    const double scale = imF.d12 / templF.d12;
                    if (scale >= minScale_ && scale <= maxScale_)
                    {
                        const int s = cvRound((scale - minScale_) * iScaleStep);
                        ++SHist[s];
                    }
                }
            }
        }
    }

    scales_.clear();

    for (int s = 0; s < scaleRange; ++s)
    {
        if (SHist[s] >= scaleThresh_)
        {
            const double scale = minScale_ + s * scaleStep_;
            scales_.push_back(std::make_pair(scale, SHist[s]));
        }
    }
}

void GeneralizedHoughGuilImpl1::calcPosition(double angle, int angleVotes, double scale, int scaleVotes)
{
    CV_Assert(levels_ > 0);
    CV_Assert(templFeatures_.size() == static_cast<size_t>(levels_ + 1));
    CV_Assert(imageFeatures_.size() == templFeatures_.size());
    CV_Assert(dp_ > 0.0);
    CV_Assert(posThresh_ > 0);

    const double sinVal = sin(toRad(angle));
    const double cosVal = cos(toRad(angle));
    const double idp = 1.0 / dp_;

    const int histRows = cvCeil(imageSize_.height * idp);
    const int histCols = cvCeil(imageSize_.width * idp);

    cv::Mat DHist(histRows + 2, histCols + 2, CV_32SC1, Scalar::all(0));

    for (int i = 0; i <= levels_; ++i)
    {
        const std::vector<Feature>& templRow = templFeatures_[i];
        const std::vector<Feature>& imageRow = imageFeatures_[i];

        for (size_t j = 0; j < templRow.size(); ++j)
        {
            Feature templF = templRow[j];

            templF.p1.theta += angle;

            templF.r1 *= scale;
            templF.r2 *= scale;

            templF.r1 = Point2d(cosVal * templF.r1.x - sinVal * templF.r1.y, sinVal * templF.r1.x + cosVal * templF.r1.y);
            templF.r2 = Point2d(cosVal * templF.r2.x - sinVal * templF.r2.y, sinVal * templF.r2.x + cosVal * templF.r2.y);

            for (size_t k = 0; k < imageRow.size(); ++k)
            {
                Feature imF = imageRow[k];

                if (angleEq(imF.p1.theta, templF.p1.theta, angleEpsilon_))
                {
                    Point2d c1, c2;

                    c1 = imF.p1.pos - templF.r1;
                    c1 *= idp;

                    c2 = imF.p2.pos - templF.r2;
                    c2 *= idp;

                    if (fabs(c1.x - c2.x) > 1 || fabs(c1.y - c2.y) > 1)
                        continue;

                    if (c1.y >= 0 && c1.y < histRows && c1.x >= 0 && c1.x < histCols)
                        ++DHist.at<int>(cvRound(c1.y) + 1, cvRound(c1.x) + 1);
                }
            }
        }
    }

    for (int y = 0; y < histRows; ++y)
    {
        const int* prevRow = DHist.ptr<int>(y);
        const int* curRow = DHist.ptr<int>(y + 1);
        const int* nextRow = DHist.ptr<int>(y + 2);

        for (int x = 0; x < histCols; ++x)
        {
            const int votes = curRow[x + 1];

            if (votes > posThresh_ && votes > curRow[x] && votes >= curRow[x + 2] && votes > prevRow[x + 1] && votes >= nextRow[x + 1])
            {
                posOutBuf_.push_back(Vec4f(static_cast<float>(x * dp_), static_cast<float>(y * dp_), static_cast<float>(scale), static_cast<float>(angle)));
                voteOutBuf_.push_back(Vec3i(votes, scaleVotes, angleVotes));
            }
        }
    }
}

/*
Ptr<GeneralizedHoughGuil> cv::createGeneralizedHoughGuil()
{
    return makePtr<GeneralizedHoughGuilImpl>();
}*/

GHT.cpp

//#include "precomp.hpp"
#include"GHT.h"

PPF：ModelGlobally,MatchLocally:EfﬁcientandRobust3DObjectRecognition（2010CVPR)

摘要：
文章主要解决点云中自由形态下三维目标的识别问题，提出了基于方向点对特征和局部模型匹配的快速投票架构，建立全局模型描述的方法，在噪声、杂波和遮挡的背景下具有较高的3D目标识别效率。
1.全局法在3D目标识别速度、精度和泛化性上表现不佳，基于局部不变特征的局部法效率高，但对目标表面分辨率和模型数据质量要求高；
2.全局模型描述法为相似特征的点对实现全局分布，以稀疏方向点对的表示形式，在保证识别效率的基础上提升计算和识别速度；
3.类似于广义霍夫变换的快速投票方案，用于在局部缩减的搜索空间中优化模型姿态，该搜索空间根据模型上的点和围绕表面法线的旋转进行参数化。
相关工作：
使用广义霍夫变换的变体检测对象，仅限于原始对象，因为具有6个自由度的完整3D姿势恢复计算太昂贵。局部法描述器：围绕特定点周围使用低维描述法，描述目标表面局部描述难以识别具有局部相似性的圆形、圆柱、圆面等目标在离线阶段，创建全局模型描述。在在线阶段，选择场景中的一组参考点。场景中的所有其他点都与参考点配对以创建点对特征。这些特征与全局模型描述中包含的模型特征相匹配，并且一组潜在匹配被检索。每个潜在的匹配通过使用有效的投票方案投票给对象姿势，其中姿势相对于参考点被参数化。该投票方案最终返回最佳对象姿势。
我们首先介绍点对特征，然后描述如何使用这些点对特征构建全局模型描述。然后我们详细描述投票方案。

在这里插入图片描述

DenseFusion

DenseFusion是一种用于3D物体检测和位姿估计的深度学习模型，它由普林斯顿大学的研究人员开发。DenseFusion使用RGB-D摄像头捕捉的图像和深度信息来实现准确的3D物体检测和位姿估计。

DenseFusion的核心思想是将2D图像中的物体信息与3D点云信息进行融合，从而提高物体检测和位姿估计的精度。具体来说，DenseFusion使用两个子网络来处理2D图像和3D点云信息。其中，2D子网络使用ResNet来提取图像的特征，3D子网络使用PointNet来提取点云的特征。接着，DenseFusion使用一个特殊的多阶段融合模块来将2D和3D特征进行融合，并生成物体的3D位姿信息。

在训练DenseFusion模型时，需要准备大量的RGB-D图像和点云数据，并使用适当的算法进行数据增强和预处理。同时，需要根据任务需求选择合适的超参数和模型架构，并进行训练和调优。在训练过程中，可以使用平均重投影误差（Average Reprojection Error）作为损失函数来计算模型的训练误差，并使用反向传播算法来更新模型的参数。

DenseFusion在许多3D物体检测和位姿估计的任务中都取得了很好的效果，并且已经被广泛应用于机器人视觉、虚拟现实、增强现实等领域。虽然DenseFusion的性能很高，但它也存在一些局限性，例如对噪声和遮挡敏感等。因此，在应用DenseFusion模型时，需要根据具体场景进行调整和优化。
在这里插入图片描述 DenseFusion的算法原理可以分为以下几个步骤：

数据预处理：DenseFusion使用RGB-D摄像头捕捉的图像和深度信息作为输入数据，并将它们进行预处理，包括对图像进行缩放、归一化和中心化，以及对深度信息进行处理和滤波。
特征提取：DenseFusion使用两个子网络来处理2D图像和3D点云信息。其中，2D子网络使用ResNet来提取图像的特征，3D子网络使用PointNet来提取点云的特征。ResNet可以很好地提取2D图像中的物体特征，而PointNet则可以很好地提取3D点云中的物体特征。
特征融合：DenseFusion使用一个特殊的多阶段融合模块来将2D和3D特征进行融合，并生成物体的3D位姿信息。该融合模块包括多个阶段，每个阶段都使用不同的方法来融合2D和3D特征，包括将2D和3D特征进行拼接、加权平均和卷积等操作。
位姿估计：最后，DenseFusion使用生成的3D位姿信息来进行物体的位姿估计。具体来说，DenseFusion使用一种迭代优化方法来计算物体的位姿，该方法基于3D-3D位姿估计和2D-3D位姿估计。

总的来说，DenseFusion的算法原理是通过将2D图像和3D点云信息进行融合，从而提高物体检测和位姿估计的精度。该算法结合了深度学习和几何计算的方法，可以在许多实际应用中取得很好的效果。

6D姿态面临的挑战

6D姿态估计是指确定物体在3D空间中的位置和旋转信息，通常用欧拉角或四元数来表示。在实际应用中，6D姿态估计面临以下挑战：

遮挡问题：当物体被其他物体或遮挡物遮挡时，其6D姿态很难准确估计。
噪声问题：传感器测量数据的噪声会导致6D姿态估计的误差增加。
多样性问题：同一物体的6D姿态可能因其形状、材料、光照等因素而有所不同，因此需要考虑这种多样性。
处理速度问题：6D姿态估计需要在实时或近实时的时间内完成，因此需要考虑算法的处理速度和计算复杂度。
数据量和质量问题：为了获得准确的6D姿态估计结果，需要大量的训练数据，并且这些数据必须具有高质量和丰富的变化。
对称性问题：一些物体具有对称性，例如圆柱体或立方体，这会导致6D姿态估计的难度增加。

为了解决这些挑战，研究人员正在开发新的6D姿态估计算法和技术，并探索使用深度学习、几何计算、传感器融合等方法来提高6D姿态估计的精度和鲁棒性。

1.重复纹理
重复纹理可能会导致不同角度的物体具有相似性,无法很好地区别;
2.形状对称
形状对称也可能会导致不同角度的物体具有相似性,无法很好地区别;
3.纹理少
因为在检测姿态从2D图片上分割出该目标,需要其具有丰富的区别性的特征才好做分割;
4.遮挡
遮挡的影响不言而喻
此外如果是方案中使用到了深度相机,则还需要考虑深度相机的精度;
5.表面光滑\反光的物体
当物体表面超过一定的光滑程度时，深度相机测量精度会急剧下降，甚至测量失败（没有深度值）。6.黑色物体
基于红外的结构光深度相机对深色（尤其是黑色）物体的测量一般不准确，甚至测量失败（没有深
度值）。
7.透明物体
我们知道红外光是可以轻松穿透普通透明玻璃的。所以除了前面提到的反射的影响，红外光在一定
条件下还可能产生透射现象，这会给红外结构光深度相机带来新的问题：我称之为深度值的歧义性。我们想象深度相机投射出一束红外光到一块普通的玻璃上，这束红外光不会发生镜面反射，它会穿透玻璃继续前行，如果透过玻璃后能够在有效测量范围内遇到其他物体并反射回红外光，那么此时
深度相机测量的深度值其实是玻璃后面物体距离相机的距离，并不是玻璃表面距离相机的距离。更悲剧的是，很多时候光线透过玻璃后是一个开阔空间，透射过玻璃的红外光线就变成了“肉包子打狗，有去无回”，因此没有对应的深度值。

6D姿态数据集

以下是一些常用的6D姿态数据集：

YCB-Video数据集：由斯坦福大学主持的一个大规模6D姿态数据集，包含21个物体，每个物体有多个实例和多个姿态，共有133万个图像和38万个3D点云数据。
LINEMOD数据集：包含13个不同的物体，每个物体有15种不同的姿态，共有39,123个RGB-D图像和点云数据。
OccludedLINEMOD数据集：是LINEMOD数据集的扩展版本，包含与LINEMOD相同的物体和姿态，但是在不同的场景中添加了遮挡和干扰，共有120,000个RGB-D图像和点云数据。
ITODD数据集：包括20个物体，每个物体有多个实例和多个姿态，共有10,000个RGB-D图像和点云数据。
T-LESS数据集：包含30个不同的物体，每个物体有10种不同的姿态，共有约10万个RGB-D图像和点云数据。
BOP数据集：包括两个子数据集，BOP2019和BOP2020，分别包含在自然环境下拍摄的物体和在工业环境下拍摄的物体，共有约180个物体和数百万个RGB-D图像和点云数据。

这些数据集提供了广泛的6D姿态估计测试场景和基准，可以用于评估不同的6D姿态估计算法和技术的性能。同时，这些数据集也可以用于训练深度学习模型和其他机器学习算法，提高6D姿态估计的精度和鲁棒性。
在这里插入图片描述在制作数据集和进行3D重建时，常用的二进制编码哈希（BCH）码包括以下几种：

SHOT-BCH：是在SHOT特征描述符的基础上使用BCH编码生成的二进制编码哈希。使用SHOT-BCH能够提高点云的匹配速度和精度，同时减小存储数据的大小。
FPFH-BCH：是在FPFH特征描述符的基础上使用BCH编码生成的二进制编码哈希。使用FPFH-BCH能够提高点云的匹配速度和精度，同时减小存储数据的大小。
3D-SIFT-BCH：是在3D-SIFT特征描述符的基础上使用BCH编码生成的二进制编码哈希。使用3D-SIFT-BCH能够提高点云的匹配速度和精度，同时减小存储数据的大小。

这些二进制编码哈希能够有效地减小点云数据的存储空间，提高点云数据的匹配速度和精度。在实际应用中，不同的哈希编码可以根据具体应用场景进行选择和组合，以提高点云数据的分类、识别和重建精度。
在这里插入图片描述这是一个描述制作数据集和进行6D姿态估计的流程，其中包括以下几个步骤：

使用BCH编码生成二进制编码哈希，减小点云数据的存储空间，提高点云数据的匹配速度和精度。
移动相机位置进行RGBD图片采集，获取不同视角下的点云数据和RGBD图像数据。
根据角点检测与匹配计算出两帧之间的相机外参变化，即相机位置和姿态的变化。
计算出某一帧中待检物体从模型坐标系到相机坐标系的变换，即物体的位姿信息，可以使用ICP等算法进行计算。
根据步骤3和4的结果，计算出每一帧图片中物体的横坐标系到相机坐标系的变换，即待求解的6D姿态信息。
使用计算出的6D姿态信息进行物体识别、跟踪和重建等应用，实现对物体的精确定位和姿态估计。

以上是一个基本的流程，具体实现过程可能会根据具体应用场景和算法不同而有所差异。
在这里插入图片描述
DenseFusion是一种用于6D姿态估计的深度学习方法。其整体流程如下：

数据预处理：使用RGB-D相机采集物体的图像和深度图像，将其转化为点云数据。在训练前，对点云数据进行预处理，包括点云采样、归一化和旋转等操作，以提高模型的鲁棒性和泛化能力。
网络结构：DenseFusion采用了一个两阶段的网络结构，第一阶段是提取物体特征的CNN网络，第二阶段是基于特征的位姿估计网络。其中，CNN网络采用ResNet-18作为骨干网络，用于提取物体的局部特征。位姿估计网络采用了一个迭代的姿态回归算法，通过多次迭代来逐步优化位姿估计的精度。
损失函数：DenseFusion采用了一个综合的损失函数，包括了姿态误差损失、置信度损失和遮挡损失等。姿态误差损失用于衡量位姿估计的精度，置信度损失用于衡量位姿估计的可靠性，遮挡损失用于处理遮挡情况下的位姿估计问题。
训练过程：DenseFusion采用了一个端到端的训练过程，通过最小化损失函数来优化模型参数。在训练过程中，使用真实的位姿信息作为监督信号，对模型进行训练。
测试过程：在测试过程中，将输入的RGB-D图像转换为点云数据，并通过CNN网络提取物体的局部特征。然后，使用位姿估计网络对物体的位姿进行估计，并输出置信度信息。最终，根据置信度信息选择最优的位姿估计结果。
应用场景：DenseFusion可以应用于物体识别、物体跟踪和物体重建等场景，具有很好的鲁棒性和泛化能力。
DenseFusion和NeRF是两种不同的深度学习方法，主要有以下区别：
任务类型：DenseFusion主要应用于6D姿态估计和物体识别等计算机视觉任务，而NeRF主要应用于渲染和重建等计算机图形学任务。
数据类型：DenseFusion主要使用RGB-D图像和点云数据进行训练和测试，而NeRF主要使用多角度的2D图像和深度图像进行训练和测试。
网络结构：DenseFusion采用了一个两阶段的网络结构，包括一个CNN网络和一个位姿估计网络。而NeRF采用了一个深度神经网络，用于从2D图像和深度图像中重建3D场景。
损失函数：DenseFusion采用了一个综合的损失函数，包括姿态误差损失、置信度损失和遮挡损失等，用于优化位姿估计和物体识别的精度和鲁棒性。而NeRF采用了一个渲染误差损失函数，用于优化重建场景的精度和真实度。
应用场景：DenseFusion主要应用于物体识别、物体跟踪和物体重建等计算机视觉任务，适用于需要进行物体姿态估计和识别的场景。而NeRF主要应用于渲染和重建等计算机图形学任务，适用于需要从2D图像和深度图像中重建真实的3D场景的场景。

虽然DenseFusion和NeRF是两种不同的深度学习方法，但它们都具有很好的应用前景，并可以互相借鉴和结合以解决更加复杂和实际的问题。

重投影误差

重投影误差是计算机视觉中一种常用的误差指标，用于衡量图像中的像素点在三维空间中对应点的投影与其在图像中位置的差异。重投影误差通常用于视觉SLAM、图像配准、三维重建等场景中。

具体而言，假设在相机坐标系下，一个三维点 $P$ 的坐标为 $(X, Y, Z)$ ，其在图像中的像素坐标为 $(u, v)$ 。假设相机的内参矩阵为 $K$ ，外参矩阵为 $[R ∣ t]$ ，其中 $R$ 为旋转矩阵， $t$ 为平移矩阵。那么， $P$ 在图像平面上的投影坐标为 $\hat{p}=K[R|t]P$ 。则该点在图像上的重投影误差为：

$\frac{1}{n}\sum_{i=1}^{n}\|\hat{p}_i - p_i\|$

其中， $n$ 为图像中的像素点数， $\hat{p}_i$ 为 $P$ 在图像平面上的投影坐标， $p_i$ 为图像中的像素坐标。重投影误差越小，则说明三维点的位置估计越准确。

在实际应用中，重投影误差可以用于优化相机位姿或三维点的位置，以提高模型的精度和鲁棒性。例如，在视觉SLAM中，可以通过优化相机位姿和三维点的位置，最小化重投影误差来提高SLAM的精度和鲁棒性。

重投影误差：指的真实三维空间点在图像平面上的投影（也就是图像上的像素点）和重投影（其实是用我们的计算值得到的虚拟的像素点）的差值。因为种种原因计算得到的值和实际情况不会完全相符，也就是这个差值不可能恰好为0，此时也就需要将这些差值的和最小化获取最优的相机位姿参数及三维空间点的坐标。
在这里插入图片描述

实例分割VS语义分割

实例分割和语义分割是计算机视觉中两种常见的分割方法，主要有以下区别：

分割对象：实例分割旨在将图像中的每个像素分配到不同的对象实例中，每个实例都有一个唯一的标识符。而语义分割旨在将图像中的每个像素分配到不同的语义类别中，同一类别的像素具有相同的标识符。
算法思想：实例分割通常采用基于区域的方法，如基于图的分割、分水岭变换等，以分离不同的对象实例。而语义分割通常采用基于像素的方法，如卷积神经网络等，以识别不同的语义类别。
精度要求：实例分割通常需要较高的精度和鲁棒性，以分离不同的对象实例。而语义分割通常需要较高的泛化能力和效率，以识别不同的语义类别。
应用场景：实例分割适用于需要对图像中的不同对象实例进行分析和处理的场景，如目标检测、多目标跟踪等。而语义分割适用于需要对图像中的不同语义类别进行分析和处理的场景，如自动驾驶、遥感图像分析等。

总之，实例分割和语义分割是两种不同的分割方法，各自适用于不同的场景和任务。在实际应用中，可以根据具体需求选择合适的分割方法，以实现更好的效果和性能。

实例分割和语义分割在计算机视觉领域有不同的应用场景，下面分别举例说明：

实例分割的应用场景

（1）目标检测：实例分割可以将图像中的每个目标分配一个唯一的标识符，从而实现目标检测和识别等任务。例如，在自动驾驶中，实例分割可以用于检测道路上的不同车辆、行人等目标。

（2）多目标跟踪：实例分割可以对图像中的不同目标进行跟踪和重识别，从而实现多目标跟踪和分析等任务。例如，在视频监控中，实例分割可以用于跟踪场景中的多个人物、车辆等目标。

（3）医学图像分析：实例分割可以用于医学图像的分割和分析，从而实现病灶检测、器官分割等任务。例如，在医学影像中，实例分割可以用于检测肿瘤、血管等目标。

语义分割的应用场景

（1）自动驾驶：语义分割可以将图像中的不同区域分配到不同的语义类别中，从而实现自动驾驶中的路面分割、障碍物检测等任务。

（2）遥感图像分析：语义分割可以用于遥感图像的分割和分析，从而实现土地利用、环境监测等任务。例如，在农业领域，语义分割可以用于识别不同的作物、土地类型等。

（3）机器人导航：语义分割可以用于机器人的感知和导航，从而实现智能导航、环境建模等任务。例如，在室内导航中，语义分割可以用于检测门、墙壁等障碍物。

总之，实例分割和语义分割在不同的应用场景中发挥着不同的作用，通过对图像进行分割和分析，可以实现更加智能化和高效的计算机视觉应用。

实例分割和语义分割都有许多不同的算法，下面列举一些常见的算法，供参考：

实例分割的算法

（1）Mask R-CNN：基于 Faster R-CNN 算法，通过添加一个分支网络来预测每个目标实例的掩码。

（2）FCIS：基于全卷积网络，通过密集预测每个像素属于哪个目标实例。

（3）YOLACT：基于单阶段检测器，通过引入实例分割头来直接预测每个目标实例的掩码。

（4）Panoptic FPN：将实例分割和语义分割结合起来，实现同时分割目标实例和场景中的语义类别。

语义分割的算法

（1）FCN：将全连接层转化为卷积层，实现端到端的像素级别的预测。

（2）U-Net：基于编码器-解码器结构，通过跳跃连接来融合不同层级的特征。

（3）DeepLab：基于空洞卷积和多尺度池化，实现精细的语义分割。

（4）PSPNet：通过金字塔池化来捕捉不同尺度的上下文信息，实现精确的语义分割。

除了上述算法，还有许多其他的实例分割和语义分割算法，例如：InstanceCut、SOLO、PANet等实例分割算法，以及SegNet、ENet、ICNet等语义分割算法。在具体应用中，可以根据实际需求选择合适的算法，以实现更好的效果和性能。

在这里插入图片描述

maskRCNN

Mask R-CNN是一种常用的实例分割算法，是在Faster R-CNN框架基础上扩展而来，由Facebook AI Research团队于2018年提出。Mask R-CNN不仅可以进行目标检测、分类和边界框回归，还可以生成目标实例的掩码，从而实现像素级别的实例分割。

Mask R-CNN的主要思路是在Faster R-CNN的基础上增加一个分支网络，用于预测每个目标实例的掩码。具体来说，Mask R-CNN将Faster R-CNN中的ROI Pooling层替换为ROI Align层，这样可以避免信息损失和空间偏差，并将RoI特征图输入到分支网络中。分支网络包括一个卷积层，一个上采样层和一个卷积层，用于生成每个目标实例的二值掩码。

在训练阶段，Mask R-CNN使用多任务损失函数，包括目标分类损失、边界框回归损失和掩码预测损失。其中，掩码预测损失使用二元交叉熵损失函数。在推理阶段，Mask R-CNN可以对图像中的每个目标实例生成对应的掩码，并将其与目标边界框进行融合，得到最终的实例分割结果。

Mask R-CNN在COCO数据集上取得了很好的实验结果，不仅可以实现高质量的目标检测和分类，还可以实现精确的实例分割。因此，Mask R-CNN被广泛应用于计算机视觉领域，例如目标检测、自动驾驶、医学图像分析等。

Mask R-CNN是最流行和有效的实例分割算法之一，但也有其他算法可以执行实例分割。以下是Mask R-CNN与其他实例分割算法的比较：

FCIS（全卷积实例分割）

FCIS是另一种可以执行实例分割而无需依赖区域建议网络的算法。相反，FCIS使用完全卷积网络直接从图像预测实例掩码。与Mask R-CNN相比，FCIS具有更简单的结构，计算效率更高，但在检测小物体方面可能不够准确。

PANet（路径聚合网络）

PANet是一种用于实例分割和目标检测的多任务学习框架。它使用底层路径生成高分辨率特征图，并使用顶层路径在不同尺度上融合特征。与Mask R-CNN相比，PANet可以更有效地处理不同尺度的物体，并产生更准确的分割结果，但可能需要更多的计算资源。

SOLO（基于位置的分割）

SOLO是一种最近的实例分割算法，它使用完全卷积网络在不同的位置和尺度上预测对象掩码。与Mask R-CNN不同，SOLO不需要区域建议，可以更高效地预测对象掩码。SOLO在几个基准数据集上实现了最先进的准确度，但由于需要更多的网络参数，可能不适合实时应用。

总的来说，不同的实例分割算法都有其优点和局限性，选择算法取决于具体的应用要求，例如准确性、效率和实时性能。

虽然Mask R-CNN是一种功能强大且广泛使用的实例分割算法，但它也存在一些限制。以下是一些限制：

计算量大：Mask R-CNN需要大量的计算资源，尤其是在训练时。这可能使其在资源受限的环境或处理能力有限的设备上难以使用。
推理速度较慢：与其他目标检测算法相比，Mask R-CNN的推理速度可能较慢。这可能在需要快速处理的实时应用中成为问题。
对小物体敏感：Mask R-CNN可能难以检测和分割小物体，特别是当它们被遮挡或与背景对比度较低时。
对超参数敏感：Mask R-CNN的性能可能对超参数的选择敏感，例如学习率、批量大小和训练迭代次数。这可能需要进行大量的调整才能实现最佳性能。
有限的泛化能力：与其他深度学习模型一样，Mask R-CNN可能难以泛化到新的和未见过的数据，特别是当训练数据有限或存在偏差时。

总的来说，虽然Mask R-CNN是一种功能强大且有效的实例分割算法，但它也有其限制。在考虑在特定应用中使用Mask R-CNN时，必须考虑到这些限制。

以下是提高Mask R-CNN性能的一些方法：

使用更大更多样化的训练数据集：Mask R-CNN对训练数据的质量和数量非常敏感，因此使用更大更多样化的数据集可以提高其性能。这包括使用随机裁剪、翻转和旋转等技术增强训练数据。
对预训练模型进行微调：在特定数据集上微调预训练的Mask R-CNN模型可以提高其性能，特别是当新的数据集与模型最初训练的数据集相似时。
调整超参数：调整超参数如学习率、批量大小和训练迭代次数可以提高Mask R-CNN的性能。不同的数据集和应用程序可能需要不同的超参数设置，因此需要尝试不同的值。
使用更强大的硬件：Mask R-CNN需要大量的计算资源，因此使用更强大的硬件，如GPU或TPU，可以显著提高其性能。
使用多模型集成：组合多个Mask R-CNN模型的预测结果可以提高其性能，特别是当这些模型在不同的数据集或使用不同的超参数进行训练时。
使用后处理技术：后处理技术如非极大值抑制或形态学操作可以提高预测边界框和掩码的准确性。

总的来说，提高Mask R-CNN性能需要结合多种技术，包括数据增强、超参数调整和硬件优化。需要尝试不同的方法，并在验证集上评估性能，以确定特定应用程序的最佳技术组合。

Mask R-CNN算法流程如下：

输入原始图像。
使用卷积神经网络（CNN）提取特征图。
使用Region Proposal Network（RPN）生成候选区域（region proposals）。
使用ROI Align将每个区域映射到特征图上，并提取相应的RoI特征。
对每个RoI特征执行分类和边界框回归，以确定每个区域内是否存在目标以及其位置。
对于每个RoI特征，使用Mask Head分支生成一个二值掩码，以表示目标的像素级分割。
计算三个损失函数：目标分类损失、边界框回归损失和掩码预测损失。
在训练期间，使用反向传播和梯度下降优化器来更新模型的权重。
在推理时，对于每个检测到的目标，使用与其关联的掩码来生成像素级分割结果。
对于每个目标，将其边界框和掩码绘制在原始图像上，以显示检测和分割结果。

总的来说，Mask R-CNN是一种基于CNN和RPN的多任务学习算法，可以同时执行目标检测和像素级分割任务。它的流程包括特征提取、区域提议、RoI对齐、分类和回归、掩码预测和损失计算等步骤。

在这里插入图片描述

solov1

SOLOv1是一种基于Mask R-CNN的目标检测算法，它可以同时实现目标检测和实例分割。相比于传统的目标检测算法，SOLOv1能够直接生成每个实例的像素级分割结果，从而更加准确地定位和检测目标。

SOLOv1的主要思想是将实例分割任务转化为像素级点分割任务。具体来说，它首先将卷积特征图分解为若干个密集预测子网格，然后对每个子网格生成一个二值掩码，用于表示该子网格中的目标像素。SOLOv1通过一个特殊的解码器来将预测的二值掩码映射回原始图像中，从而得到每个目标的像素级分割结果。

SOLOv1的网络结构包含两个主要部分：一个特征提取网络和一个密集预测分支。特征提取网络通常是一个预训练的卷积神经网络（如ResNet），用于从输入图像中提取高层次的特征表示。密集预测分支由若干个密集预测子网格组成，每个子网格负责预测其对应的像素掩码。SOLOv1使用了一个多任务损失函数，同时优化目标检测和实例分割的损失。其中目标检测损失和传统的目标检测算法相似，而实例分割损失则是基于像素级点分割的交叉熵损失。

SOLOv1算法在多个基准数据集上进行了实验，并取得了优秀的结果。它不仅可以实现准确的目标检测，还可以直接生成每个目标的像素级分割结果，为图像分析和计算机视觉领域提供了一个新的研究方向。

首先思考一个问题，能否像语义分割一样进行实例分割？实例分割和语义分割在算法处理上最大的不同就是，实例
分割需要处理同类别的实例重叠或粘连的问题。那么如果将不同的实例分配到不同的输出channel上，不就可以解决这个问题了吗？本文作者正是这种思路，不过这样也面临两个问题:一是通道分配顺序的问题，语义分割是根据类别进行通道分配的。而对于实例分割，相同类别的不同实例需要分配到不同通道上，需要解决按照什么样的规则分配。二是尺度问题，不同尺度的物体利用相同大小的输出来预测会导致正负样本不平衡，以及小目标分割边缘不够精细
的问题。对于这两个问题，本文作者给出了解答。作者首先对MSCOCO数据集进行统计，发现在验证集中，绝大
多数（约98.9%）的实例要么中心位置不同，要么大小不同。因此可以通过中心位置和对象大小直接区分实例，既
location和sizes。所以作者利用位置来分配实例应该落入哪一个通道，利用FPN来解决尺度问题。

您的描述比较接近SOLOv2算法，它的确是通过按照位置和大小将不同实例分配到不同的输出通道上来解决实例分割中的重叠和粘连问题。SOLOv2在SOLOv1的基础上进行了改进，提出了SOLOv2++的算法。SOLOv2++算法采用了一个新的实例分割头部，即SOLOv2++多层次实例分割（Multi-Level
Instance Segmentation，MLIS）头部，用于解决SOLOv2算法中存在的尺度问题和实例重叠问题。

在MLIS头部中，SOLOv2++算法采用了一个多层级的分割模块，用于处理不同尺度的物体和实例之间的关系。具体来说，它首先将输入特征图分解为多个不同尺度的子特征图，然后在每个子特征图上执行实例分割操作，最后将不同尺度的实例分割结果融合起来，得到最终的像素级分割结果。MLIS头部还采用了一种基于形态学的后处理方法，用于去除分割结果中的小噪声和不规则边缘，从而得到更加精细的分割结果。

总之，SOLOv2++算法通过设计新的实例分割头部和后处理方法，成功解决了SOLOv2算法中存在的尺度问题和实例重叠问题，从而实现了更加准确的实例分割效果。

在这里插入图片描述

solov2

v1采用的是Decoupledhead的方式，分别预测X分支和Y分支，使用的时候再进行element-wise相乘。v2继续进行了优化，提出了dynamichead。如下图，mask预测分为了kernel分支和feature分支。

SOLOv1采用了Decoupled SOLO Head的方式进行实例分割，而SOLOv2则进一步优化了这个头部结构，并提出了Dynamic SOLO Head。

在SOLOv1中，Decoupled SOLO Head由两个分支组成：一个用于预测对象的中心点、宽度和高度，另一个用于生成对象的像素掩码。这两个分支可以独立地进行训练和推断，并且它们的输出可以通过元素级相乘进行组合，从而得到最终的实例分割结果。

在SOLOv2中，Dynamic SOLO Head采用了一种新的头部结构，将实例分割任务分解为两个子任务：kernel分支和feature分支。kernel分支负责生成每个对象的内核（也称为掩膜），而feature分支则负责生成每个对象的特征表示。这两个子任务可以独立地进行训练和推断，并且它们的输出可以通过卷积操作进行组合，从而得到最终的实例分割结果。

Dynamic SOLO Head的一个主要优点是可以动态地调整内核和特征的尺寸和形状，以适应不同大小和形状的对象。此外，Dynamic SOLO Head还采用了一种新的多级调制方法，用于在不同尺度下生成更加准确的分割结果。

Dynamic SOLO Head是SOLOv2中的实例分割头部结构，它由两个子分支组成：kernel分支和feature分支。kernel分支负责生成每个对象的内核（也称为掩膜），而feature分支则负责生成每个对象的特征表示。这两个子任务可以独立地进行训练和推断，并且它们的输出可以通过卷积操作进行组合，从而得到最终的实例分割结果。

具体来说，kernel分支生成的内核是一种特殊的掩膜，用于表示对象的形状和位置。通过调整内核的形状和大小，可以适应不同大小和形状的对象。而feature分支生成的特征表示则用于描述对象的视觉特征，以便在像素级别对其进行分类和分割。

Dynamic SOLO Head采用了一种新的多级调制方法，用于在不同尺度下生成更加准确的分割结果。具体来说，它将内核和特征分别调整到多个不同的尺度，并使用不同的卷积核对它们进行处理。这种多级调制方法可以提高实例分割的准确性和效率。

另外，Dynamic SOLO Head还使用了一种特殊的损失函数，称为Mask IoU Loss。该损失函数基于像素级的交叉熵损失，并将其与掩膜的IoU（Intersection over Union）进行结合，用于优化实例分割的准确性和稳定性。
在这里插入图片描述 SOLOv1和SOLOv2的网络结构都是基于Mask R-CNN的框架，但在实例分割头部结构上进行了改进，使得网络可以同时实现目标检测和实例分割。

SOLOv1的网络结构包含两个主要部分：一个特征提取网络和一个密集预测分支。特征提取网络通常是一个预训练的卷积神经网络（如ResNet），用于从输入图像中提取高层次的特征表示。密集预测分支由若干个密集预测子网格组成，每个子网格负责预测其对应的像素掩码。

SOLOv2在SOLOv1的基础上进一步优化了实例分割头部结构，采用了Dynamic SOLO Head，将实例分割任务分解为内核和特征两个子任务，并使用多级调制和Mask IoU Loss等技术进行优化。与SOLOv1相比，SOLOv2的网络结构更加复杂，具有更高的准确性和效率。

总之，SOLOv1和SOLOv2都是基于Mask R-CNN的网络结构，但在实例分割头部结构上进行了改进，使得网络可以同时实现目标检测和实例分割，并取得了在多个基准数据集上领先的实验结果。
在这里插入图片描述 YOLOv5是一种基于目标检测的算法，其主要目的是对输入图像中的目标进行检测和定位。虽然YOLOv5在目标检测任务中表现出了很好的性能，但是它并不能直接用于实例分割任务。

实例分割和目标检测虽然都是计算机视觉领域中的重要任务，但是它们的目标和输出不同。目标检测的目标是检测和定位目标的位置和类别，而实例分割的目标是从图像中分割出每个对象的像素级掩码。

虽然YOLOv5不能直接用于实例分割任务，但是可以使用一些基于YOLOv5的算法来实现实例分割。例如，通过将YOLOv5与分割网络（如U-Net或Mask R-CNN）结合起来，可以实现同时目标检测和实例分割的任务。

另外，也有一些基于YOLOv5的改进算法专门用于实例分割任务。例如，YOLOv5-Panoptic是一种基于YOLOv5的实例分割算法，它使用了一种新的分割头部结构和多级特征融合方法，可以同时输出实例分割和语义分割结果。

总之，虽然YOLOv5不能直接用于实例分割任务，但是可以通过结合分割网络或使用基于YOLOv5的改进算法来实现实例分割任务。

yolact

YOLACT是一种基于实例分割的目标检测方法，由美国加州大学圣地亚哥分校的Bolya Zhou等人提出。与传统的目标检测方法不同，YOLACT可以直接输出每个对象的像素掩码，从而实现更加精细的目标检测和分割。

YOLACT的核心思想是将目标检测和实例分割任务相结合，并使用全卷积网络来输出每个对象的像素掩码。具体来说，YOLACT使用了一个基于ResNet的特征提取网络和一个由若干个YOLACT头部组成的全卷积网络。每个YOLACT头部由两个子分支组成：一个用于预测对象的类别、边界框和掩码，另一个用于生成对象的特征表示。

与传统的目标检测方法不同，YOLACT可以直接输出每个对象的像素掩码，而不需要通过区域提取和像素点分类等过程。这种像素级别的分割结果可以更加准确地描述对象的形状和位置，从而进一步提高目标检测的准确性和效率。

另外，YOLACT还使用了一种新的损失函数，称为Mask IOU Loss。该损失函数基于像素级的二元交叉熵损失，并将其与掩膜的IoU（Intersection over Union）进行结合，用于优化实例分割的准确性和稳定性。

YOLACT的网络结构

YOLACT的网络结构可以分为两个主要部分：一个特征提取网络和一个YOLACT头部网络。

特征提取网络通常是一个预训练的卷积神经网络（如ResNet），用于从输入图像中提取高层次的特征表示。在YOLACT中，特征提取网络输出的特征图被称为“backbone features”。

YOLACT头部网络由若干个YOLACT头部组成，每个头部用于检测和分割一个目标。每个YOLACT头部由两个子分支组成：一个用于预测对象的类别、边界框和掩码，另一个用于生成对象的特征表示。具体来说，每个头部包括以下三个部分：

对象类别和边界框预测分支：该分支用于预测目标的类别和位置。它由两个全连接层和一个线性层组成，其中线性层用于预测目标的位置和类别。
掩码预测分支：该分支用于生成目标的像素掩码。它包括一个全卷积网络和一个线性层，其中全卷积网络用于生成精细的目标掩码，线性层用于将目标掩码缩放到正确的大小。
特征生成分支：该分支用于生成目标的特征表示。它由一个ROI Pooling层和若干个卷积层组成，其中ROI Pooling层用于从backbone features中提取与目标对应的特征表示，卷积层用于将特征表示转换为适合生成目标掩码的形式。

为了提高实例分割的准确性和稳定性，YOLACT还使用了一种新的损失函数，称为Mask IOU Loss。该损失函数基于像素级的二元交叉熵损失，并将其与掩膜的IoU（Intersection over Union）进行结合，用于优化实例分割的准确性和稳定性。

总之，YOLACT的网络结构包括一个特征提取网络和一个YOLACT头部网络，通过直接输出每个对象的像素掩码，实现了更加精细的目标检测和分割。

在这里插入图片描述

SOLO和YOALCT都是用于实例分割任务的算法，它们的主要区别在于网络结构和实现方式上。

网络结构：SOLO采用了一种新的分割头部结构，称为SOLO head，用于同时预测目标的类别、边界框和像素级分割掩码。SOLO head由两个分支组成：一个用于预测目标的类别和边界框，另一个用于生成目标的像素级分割掩码。而YOLOACT则采用了一种基于YOLACT头部的结构，用于直接生成每个实例的像素级分割掩码。
实现方式：SOLO采用的是基于Mask R-CNN的实现方式，首先检测出目标的位置和类别，然后再生成目标的像素级分割掩码。而YOLOACT则采用的是直接生成像素级分割掩码的方式，因此可以更加高效地实现实例分割任务。

总之，SOLO和YOLOACT都是用于实例分割任务的算法，它们的主要区别在于网络结构和实现方式上。SOLO采用了一种新的分割头部结构，而YOLOACT则采用了一种基于YOLACT头部的结构，并且直接生成像素级分割掩码，因此可以更加高效地实现实例分割任务。在这里插入图片描述

depth_and_rgb_create_points.py

import torch.utils.data as data
from PIL import Image
import os
import os.path
import errno
import torch
import json
import codecs
import numpy as np
import sys
import torchvision.transforms as transforms
import argparse
import json
import time
import random
import numpy.ma as ma
import copy
import scipy.misc
import scipy.io as scio
import yaml
import cv2
def to_str(var):
    return str(list(np.reshape(np.asarray(var), (1, np.size(var)))[0]))[1:-1]
if __name__=='__main__':
    rgb_path=r"D:\BaiduNetdiskDownload\6Ddataset\Linemod_preprocessed\Linemod_preprocessed\data\01\rgb\0000.png"
    depth_path=r"D:\BaiduNetdiskDownload\6Ddataset\Linemod_preprocessed\Linemod_preprocessed\data\01\depth\0000.png"

    cam_cx = 325.26110
    cam_cy = 242.04899
    cam_fx = 572.41140
    cam_fy = 573.57043
    img =cv2.imread(rgb_path)# Image.open(rgb_path)
    ori_img = np.array(img)
    depth = np.array(Image.open(depth_path))
    xmap = np.array([[j for i in range(640)] for j in range(480)])
    ymap = np.array([[i for i in range(640)] for j in range(480)])
    img = np.array(img)
    B=img[:,:,0]
    G = img[:, :, 1]
    R = img[:, :, 2]
    depth_masked = depth.flatten()[:, np.newaxis].astype(np.float32)
    xmap_masked = xmap.flatten()[:, np.newaxis].astype(np.float32)
    ymap_masked = ymap.flatten()[:, np.newaxis].astype(np.float32)
    B=B.flatten()[:, np.newaxis].astype(np.uint8)
    G=G.flatten()[:, np.newaxis].astype(np.uint8)
    R=R.flatten()[:, np.newaxis].astype(np.uint8)
    cam_scale = 1.0
    pt2 = depth_masked / cam_scale
    pt0 = (ymap_masked - cam_cx) * pt2 / cam_fx
    pt1 = (xmap_masked - cam_cy) * pt2 / cam_fy
    number=len(pt2)
    cloud = np.concatenate((pt0, pt1, pt2,R,G,B), axis=1)
    #cloud = cloud / 1000.0  # 变成以米为单位
    fname="test_cloud2.ply"
    f=open(fname,'w')
    f.writelines('ply'+'\n')
    f.writelines('format ascii 1.0' + '\n')
    f.writelines('comment VCGLIB generated' + '\n')
    f.writelines('element vertex '+str(number) + '\n')
    f.writelines('property float x' + '\n')
    f.writelines('property float y' + '\n')
    f.writelines('property float z' + '\n')
    f.writelines('property uchar red' + '\n')
    f.writelines('property uchar green' + '\n')
    f.writelines('property uchar blue'+ '\n')
    f.writelines('end_header' + '\n')

    for every_array in cloud:
        str_every_array=str(every_array[0])+' '+str(every_array[1])+' '+str(every_array[2])+' '+ \
                str(int(every_array[3])) + ' ' + str(int(every_array[4])) + ' ' + str(int(every_array[5]))+'\n'
        f.writelines(str_every_array)
    f.close()

generate_3D_models.py

#3D重建全流程
import pcl
import cv2
import open3d as o3d
import os,glob,yaml
from PIL import Image
import numpy as np
import matplotlib.pyplot as plt

def rigid_transform_3D(A, B):
    assert len(A) == len(B)

    N = A.shape[0]  # total points
    centroid_A = np.mean(A, axis=0)
    centroid_B = np.mean(B, axis=0)

    # centre the points
    AA = A - np.tile(centroid_A, (N, 1))
    BB = B - np.tile(centroid_B, (N, 1))

    H = np.matmul(np.transpose(AA), BB)
    U, S, Vt = np.linalg.svd(H)
    R = np.matmul(Vt.T, U.T)

    # special reflection case
    if np.linalg.det(R) < 0:
        print("Reflection detected")
        Vt[2, :] *= -1
        R = np.matmul(Vt.T, U.T)

    t = -np.matmul(R, centroid_A) + centroid_B
    t=t.reshape(3,1)
    # print("R:",R)
    # print("t:", t)
    transform_matrix=np.hstack((R,t))
    print(transform_matrix)
    return transform_matrix

    # err = B - np.matmul(A,R.T) - t.reshape([1, 3])
def save_points_to_ply(cloud,BGR,file_name):
    number=cloud.shape[0]
    fname = file_name
    f = open(fname, 'w')
    f.writelines('ply' + '\n')
    f.writelines('format ascii 1.0' + '\n')
    f.writelines('comment VCGLIB generated' + '\n')
    f.writelines('element vertex ' + str(number) + '\n')
    f.writelines('property float x' + '\n')
    f.writelines('property float y' + '\n')
    f.writelines('property float z' + '\n')
    f.writelines('property uchar red' + '\n')
    f.writelines('property uchar green' + '\n')
    f.writelines('property uchar blue' + '\n')
    f.writelines('end_header' + '\n')

    for every_array,every_color in zip(cloud,BGR):
        str_every_array = str(every_array[0]) + ' ' + str(every_array[1]) + ' ' + str(every_array[2]) + ' ' + \
                          str(int(every_color[2])) + ' ' + str(int(every_color[1])) + ' ' + str(
            int(every_color[0])) + '\n'
        f.writelines(str_every_array)
    f.close()
def all_pixel_to_3d(img,depth,file_name):
    cam_cx = 325.26110
    cam_cy = 242.04899
    cam_fx = 572.41140
    cam_fy = 573.57043
    #img = cv2.imread(rgb_path)  # Image.open(rgb_path)
    #depth = np.array(Image.open(depth_path))
    xmap = np.array([[j for i in range(640)] for j in range(480)])
    ymap = np.array([[i for i in range(640)] for j in range(480)])
    img = np.array(img)
    B = img[:, :, 0]
    G = img[:, :, 1]
    R = img[:, :, 2]
    depth_masked = depth.flatten()[:, np.newaxis].astype(np.float32)
    xmap_masked = xmap.flatten()[:, np.newaxis].astype(np.float32)
    ymap_masked = ymap.flatten()[:, np.newaxis].astype(np.float32)
    B = B.flatten()[:, np.newaxis].astype(np.uint8)
    G = G.flatten()[:, np.newaxis].astype(np.uint8)
    R = R.flatten()[:, np.newaxis].astype(np.uint8)
    cam_scale = 1.0
    pt2 = depth_masked / cam_scale
    pt0 = (ymap_masked - cam_cx) * pt2 / cam_fx
    pt1 = (xmap_masked - cam_cy) * pt2 / cam_fy
    number = len(pt2)
    return_cloud=np.concatenate((pt0, pt1, pt2), axis=1)
    cloud = np.concatenate((pt0, pt1, pt2, R, G, B), axis=1)
    # cloud = cloud / 1000.0  # 变成以米为单位
    fname = file_name
    f = open(fname, 'w')
    f.writelines('ply' + '\n')
    f.writelines('format ascii 1.0' + '\n')
    f.writelines('comment VCGLIB generated' + '\n')
    f.writelines('element vertex ' + str(number) + '\n')
    f.writelines('property float x' + '\n')
    f.writelines('property float y' + '\n')
    f.writelines('property float z' + '\n')
    f.writelines('property uchar red' + '\n')
    f.writelines('property uchar green' + '\n')
    f.writelines('property uchar blue' + '\n')
    f.writelines('end_header' + '\n')

    for every_array in cloud:
        str_every_array = str(every_array[0]) + ' ' + str(every_array[1]) + ' ' + str(every_array[2]) + ' ' + \
                          str(int(every_array[3])) + ' ' + str(int(every_array[4])) + ' ' + str(
            int(every_array[5])) + '\n'
        f.writelines(str_every_array)
    f.close()
    return return_cloud
def transform(cloud,transform_matrix):
    number=cloud.shape[0]
    temp=np.ones(number)
    temp=temp[:,np.newaxis]
    cloud=np.hstack((cloud,temp))
    cloud=cloud.transpose()
    return_cloud=np.dot(transform_matrix, cloud)
    return return_cloud.transpose()
def circle_keypoints(rgb,pt):
    for id,every_pt in enumerate(pt):
        #cv2.circle(rgb, (int(every_pt[0]), int(every_pt[1])), 3, (0, 0, 255), -1)
        #font = cv2.FONT_ITALIC
        cv2.putText(rgb, str(id), (int(every_pt[0]), int(every_pt[1])), cv2.FONT_ITALIC, 0.5, (0, 255, 0), 3)
        #cv2.putText(rgb, (int(every_pt[0]), int(every_pt[1])), 3, (0, 0, 255), -1)
    return rgb
def pixel_coor_to_camera_coor(pt_list,depth_mask):
    cam_cx = 325.26110
    cam_cy = 242.04899
    cam_fx = 572.41140
    cam_fy = 573.57043
    cam_scale = 1.0
    points=[]
    for every_tuple in pt_list:
        x=every_tuple[0]
        y=every_tuple[1]
        depth=depth_mask[int(y)][int(x)]
        pt2 = depth / cam_scale
        pt0 = (x - cam_cx) * pt2 / cam_fx
        pt1 = (y - cam_cy) * pt2 / cam_fy
        points.append([pt0,pt1,pt2])
    return points
def keypoints_match(img1,img2):
    sift = cv2.SIFT_create()
    # find the keypoints and descriptors with SIFT
    kp1, des1 = sift.detectAndCompute(img1, None)
    kp2, des2 = sift.detectAndCompute(img2, None)
    # BFMatcher with default params
    bf = cv2.BFMatcher()
    matches = bf.knnMatch(des1, des2, k=2)
    # Apply ratio test
    good = []
    return_pt1=[]
    return_pt2=[]
    for m, n in matches:
        if m.distance < 0.6 * n.distance:
            good.append([m])
            pt1=  kp1[m.queryIdx].pt
            pt2 = kp2[m.trainIdx].pt
            return_pt1.append(pt1)
            return_pt2.append(pt2)
    #cv.drawMatchesKnn expects list of lists as matches.
    img3 = cv2.drawMatchesKnn(img1, kp1, img2, kp2, good, None, flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
    plt.imshow(img3), plt.show()
    return return_pt1,return_pt2
#挑选三张图来进行模型重建
if __name__=='__main__':
    data_root=r"D:\BaiduNetdiskDownload\6Ddataset\Linemod_preprocessed\Linemod_preprocessed\data\01"
    rgb_path1=os.path.join(data_root,'rgb\\0000.png')
    rgb_path2=os.path.join(data_root,'rgb\\0600.png')
    rgb_path3=os.path.join(data_root,'rgb\\1200.png')
    depth_path1=os.path.join(data_root,'depth\\0000.png')
    depth_path2=os.path.join(data_root,'depth\\0006.png')
    depth_path3=os.path.join(data_root,'depth\\1200.png')

    rgb1=cv2.imread(rgb_path1)
    rgb2=cv2.imread(rgb_path2)
    rgb3=cv2.imread(rgb_path3)
    depth1=np.array(Image.open(depth_path1))
    depth2=np.array(Image.open(depth_path2))
    depth3=np.array(Image.open(depth_path3))

    #先在二维上进行特征点匹配
    return_pt11,return_pt12=keypoints_match(rgb1,rgb2)
    rgb1_show=circle_keypoints(rgb2.copy(),return_pt11)
    cv2.imwrite("rgb1_show.jpg",rgb1_show)
    rgb2_show=circle_keypoints(rgb1.copy(),return_pt12)
    cv2.imwrite("rgb2_show.jpg",rgb2_show)
    return_pt21,return_pt22=keypoints_match(rgb1,rgb3)
    return_pt11_3d=np.array(pixel_coor_to_camera_coor(return_pt11,depth1))
    return_pt12_3d=np.array(pixel_coor_to_camera_coor(return_pt12,depth2))
    return_pt21_3d=np.array(pixel_coor_to_camera_coor(return_pt21,depth1))
    return_pt22_3d=np.array(pixel_coor_to_camera_coor(return_pt22,depth3))
    #计算相机外参变换
    transform_matrix2to1=rigid_transform_3D(return_pt12_3d,return_pt11_3d)#return_pt11_3d
    transform_matrix3to1=rigid_transform_3D(return_pt22_3d,return_pt21_3d)
    # transform_matrix2to1=createTransformFunc(return_pt12_3d,return_pt11_3d)
    # transform_matrix3to1=createTransformFunc(return_pt22_3d,return_pt21_3d)
    #获取相机坐标系下的全部点云
    cloud1=all_pixel_to_3d(rgb1,depth1,'image1.ply')
    cloud2=all_pixel_to_3d(rgb2,depth2,'image2.ply')
    cloud3=all_pixel_to_3d(rgb3,depth3,'image3.ply')
    #将点云经过旋转平移，同时保存一下中间结果
    cloud2to1=transform(cloud2,transform_matrix2to1)
    #cloud2to1=cloud2to1
    cloud3to1=transform(cloud3,transform_matrix3to1)
    #cloud3to1=cloud3to1
    save_points_to_ply(cloud2to1,rgb2.reshape(640*480,3),'cloud2to1.ply')
    save_points_to_ply(cloud3to1,rgb3.reshape(640*480,3),'cloud3to1.ply')
    #将点云进行icp配准，得到最终结果
    #加载GT文件
    #gt=yaml.load(os.path.join(data_root,'gt.yaml'), Loader=yaml.Loader)
    #将匹配的二维点映射到三维，计算相机外参的变换

    #将三个相机坐标系的点云旋转平移到同一坐标系下
    #仅保留前景的点云

python算法工程师

关注

10
点赞
踩
40

收藏

觉得还不错? 一键收藏
1
评论
6D姿态检测

什么是6D姿态6D姿态（6D pose）是指物体在三维空间中的位置和方向，通常用6个自由度来描述。这6个自由度包括物体在三维空间中的三个平移自由度和三个旋转自由度。在计算机视觉和机器人领域，6D姿态通常用于物体识别、跟踪和姿态估计等任务。例如，在自动化物流中，机器人需要识别和抓取不同形状和大小的物体，这就需要准确估计物体的6D姿态。6D姿态的估计通常通过使用传感器数据（如摄像头、激光雷达等）和计算机视觉算法来实现。其中，。
复制链接

扫一扫

专栏目录