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 構建最小二乘問題，使誤...
《視覺SLAM十四講》第3講課後習題程式設計題
實現 include include include using namespace std int main int argc,char ar eigen quaterniond q1 0.35,0.2,0.3,0.1 定義兩個四元數 eigen quaterniond q2 0.5,0.4,0....
3D遊戲從頭編 第七回 燈光
金點時空 第七回 燈光可能秋天就要到了，最近天氣總是突冷忽熱的，一不小心就要著涼。雖然我已經很注意了，但還是沒能逃過這一劫，幸好吃藥吃的及時 到位，再加上 的悉心照料，終於是挺了過來。再說點題外話，就在前幾天，金點時空的網域名稱 www.gpgame.com 註冊一年到期了，我們正在進行續費，但是目...