3D 3D ICP估計姿態

1.icp

假設有一組配對好的3d點， \(p=, ..., p_}\) , \(p^=^, ..., p_^}\)。

有乙個歐式變換r,t，使得： \(p_ = rp^_ + t\)

該問題可以用迭代最近點(icp)來求解。注意考慮兩組3d點的變換時，和相機沒有關係。

icp求解線性代數的求解(svd)和非線性優化方式求解(類似於ba)

定義誤差項： \(e_ = p_ - ( rp^_ + t )\)

構建最小二乘問題，使誤差平方和達到極小的r,t

定義兩組點的質心：

\(p = \frac \sum^_ (p_), p^ = \frac \sum^_ (p_^),\)

步驟：

void pose_estimation_3d3d(const vector&pts1,

const vector&pts2,
mat &r, mat &t) 
p1 = point3f(vec3f(p1) / n);
p2 = point3f(vec3f(p2) / n);
vectorq1(n), q2(n); // remove the center
//去質心
for (int i = 0; i < n; i++) 
// compute q1*q2^t
eigen::matrix3d w = eigen::matrix3d::zero();
for (int i = 0; i < n; i++) 
cout << "w=" << w << endl;
// svd on w
eigen::jacobisvdsvd(w, eigen::computefullu | eigen::computefullv);
eigen::matrix3d u = svd.matrixu();
eigen::matrix3d v = svd.matrixv();
cout << "u=" << u << endl;
cout << "v=" << v << endl;
eigen::matrix3d r_ = u * (v.transpose());
if (r_.determinant() < 0) 
eigen::vector3d t_ = eigen::vector3d(p1.x, p1.y, p1.z) - r_ * eigen::vector3d(p2.x, p2.y, p2.z);
// convert to cv::mat
//推導是按第二張圖到第一張圖的變化，
//此處進行逆變換，即為第一張圖到第二張圖的變化
r = (mat_(3, 3) <<
r_(0, 0), r_(0, 1), r_(0, 2),
r_(1, 0), r_(1, 1), r_(1, 2),
r_(2, 0), r_(2, 1), r_(2, 2)
);t = (mat_(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

/// 節點，優化變數維度和資料型別

class vertexpose : public g2o::basevertex<6, sophus::se3d> 
//更新估計值
/// left multiplication on se3
virtual void oplusimpl(const double *update) override 
virtual bool read(istream &in) override {}
virtual bool write(ostream &out) const override {}
};/// 邊，誤差模型 觀測維度，觀測資料型別， 鏈結節點型別
class edgeprojectxyzrgbdposeonly : public g2o::baseunaryedge<3, eigen::vector3d, vertexpose> 
virtual void computeerror() override 
//計算雅可比矩陣
//雅可比矩陣為[i,-p'^]
virtual void linearizeoplus() override 
bool read(istream &in) {}
bool write(ostream &out) const {}
protected:
eigen::vector3d _point;
};//將頂點和邊加入g2o
oid bundleadjustment(
const vector&pts1,
const vector&pts2,
mat &r, mat &t) 
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeoptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::durationtime_used = chrono::duration_cast>(t2 - t1);
cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
cout << endl << "after optimization:" << endl;
cout << "t=\n" << pose->estimate().matrix() << endl;
// convert to cv::mat
eigen::matrix3d r_ = pose->estimate().rotationmatrix();
eigen::vector3d t_ = pose->estimate().translation();
r = (mat_(3, 3) <<
r_(0, 0), r_(0, 1), r_(0, 2),
r_(1, 0), r_(1, 1), r_(1, 2),
r_(2, 0), r_(2, 1), r_(2, 2)
);t = (mat_(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

視覺十四講 第七講 3D 3D ICP估計姿態

1.icp 假設有一組配對好的3d點，p p p p 有乙個歐式變換r,t，使得 p rp t 該問題可以用迭代最近點 icp 來求解。注意考慮兩組3d點的變換時，和相機沒有關係。icp求解線性代數的求解 svd 和非線性優化方式求解 類似於ba 定義誤差項 e p rp t 構建最小二乘問題，使誤...

3d在除錯區輸出座標 3D人體姿態估計筆記

英文版 3d human pose estimation notes 人體姿態估計 估計人的關節點座標 回歸問題 多人姿態估計 人體姿態跟蹤 先獲取2d資訊，然後再 提公升 到3d姿態 直接用預訓練好的2d姿態網路，將得到的2d座標輸入到3d姿態估計網路中 得益於2d姿態估計較為成熟 缺點 為什麼要...

6D姿態估計常見技術總結

pnp問題 根據影象中特徵點的二維畫素座標及其對應的三維空間座標，來估計相機在參考座標系中位姿的一類演算法 思想 常見的是 根據n個3d 2d匹配點對，利用最小化重投影誤差來求解相機外參的演算法。作用 在乙個包含異常資料 外點 的資料集中選取出最大一致集 選取最大有效樣本資料集 的演算法，同時，一些...