使用python語言來實現二維點雲的icp演算法
二維點雲icp演算法原理及推導，請見我的另外一篇部落格
二維點雲icp原理推導
特點說明：
icp演算法中的loss計算方式，可以根據自己實際需要來調整。我這裡使用的是，目標點雲a中的某個點 a，從源點雲 b 中找到距離點 a 最近的點 b，總的loss就是這些a-b距離之和
正常的loss計算方式，應該是從源點雲b中的每個點 b，去目標點雲a裡尋找最近點
loss的定義方式一定要好好思考，結合自己的實際需要，不要只會把***的**搬過去，因為可能不work
import numpy as np
import math
import matplotlib.pyplot as plt
# 求出兩個點之間的向量角度，向量方向由點1指向點2
defgetthetaoftwopoints
(x1, y1, x2, y2)
:return math.atan2(y2-y1, x2-x1)
# 求出兩個點的距離
defgetdistoftwopoints
(x1, y1, x2, y2)
:return math.sqrt(math.
pow(x2-x1,2)
+ math.
pow(y2-y1,2)
)# 在pt_set點集中找到距(p_x, p_y)最近點的id
defgetclosestid
(p_x, p_y, pt_set):id
=0min=
10000000
for i in
range
(pt_set.shape[1]
):dist = getdistoftwopoints(p_x, p_y, pt_set[0]
[i], pt_set[1]
[i])
if dist <
min:
id= i
min= dist
return
id# 求出兩個點集之間的平均點距
defdistoftwoset
(set1, set2)
: loss =0;
for i in
range
(set1.shape[1]
):id= getclosestid(set1[0]
[i], set1[1]
[i], set2)
dist = getdistoftwopoints(set1[0]
[i], set1[1]
[i], set2[0]
[id], set2[1]
[id])
loss = loss + dist
return loss/set1.shape[1]
# icp核心**
deficp
(sourcepoints, targetpoints)
: a = targetpoints
b = sourcepoints
iteration_times =
0 dist_now =
1 dist_improve =
1 dist_before = distoftwoset(a, b)
while iteration_times <
10and dist_improve >
0.001
: x_mean_target = a[0]
.mean(
) y_mean_target = a[1]
.mean(
) x_mean_source = b[0]
.mean(
) y_mean_source = b[1]
.mean(
) a_ = a - np.array(
[[x_mean_target]
,[y_mean_target]])
b_ = b - np.array(
[[x_mean_source]
,[y_mean_source]])
w_up =
0 w_down =
0for i in
range
(a_.shape[1]
):j = getclosestid(a_[0]
[i], a_[1]
[i], b_)
w_up_i = a_[0]
[i]*b_[1]
[j]- a_[1]
[i]*b_[0]
[j] w_down_i = a_[0]
[i]*b_[0]
[j]+ a_[1]
[i]*b_[1]
[j] w_up = w_up + w_up_i
w_down = w_down + w_down_i
theta = math.atan2(w_up, w_down)
x = x_mean_target - math.cos(theta)
*x_mean_source - math.sin(theta)
*y_mean_source
y = y_mean_target + math.sin(theta)
*x_mean_source - math.cos(theta)
*y_mean_source
r = np.array(
[[math.cos(theta)
, math.sin(theta)],
[-math.sin(theta)
, math.cos(theta)]]
) b = np.matmul(r, b)
+ np.array(
[[x]
,[y]])
iteration_times = iteration_times +
1 dist_now = distoftwoset(a, b)
dist_improve = dist_before - dist_now
print
("迭代第"
+str
(iteration_times)
+"次, 損失是"
+str
(dist_now)
+",提公升了"
+str
(dist_improve)
) dist_before = dist_now
return b
用webgl來繪製二維點雲吧
執行測試平台 小強ros機械人 做圖形化程式web是非常方便的。最近做了乙個專案就是用webgl來繪製二維點雲，執行效果還是不錯的。下面簡單介紹一下webgl的使用方法和二維點雲的繪製方法。首先什麼是webgl？opengl大家一定都知道，就是 open graphic library.webgl ...
點雲資料二維變換 「福」字變換
問題 通過二維變換實現圖形重合 分析 圖形要找到變換規則，即平移，旋轉，放縮的引數與次序，需要知道兩圖形的差距，這個差距，即位置 大小與方向。零階矩和一階矩可以描述乙個圖形的質心，二階矩可以描述乙個圖形的大小方向，即橢圓區域擬合，橢圓的確定需要圓心 長軸 短軸與旋轉角4個引數 具體公式為 其中的v是...
Python中max函式用於二維列表的例項
最近寫乙個和二維eqqgkuijxf列表有關的演算法時候發現的 當用max求二維列表中最大值時，輸出的結果是子列表首元素最大的那個列表 測試如下 c 1,2,1 0,5,6 a 0,3,1 1,4,6 print max c max a 結果是這樣的 1,2,程式設計客棧 1 1,4,6 以上這篇p...