GTSA中的 gtsam::StereoPoint2 类注释解读

1. 类作用

class GTSAM_EXPORT StereoPoint2

StereoPoint2 是 GTSAM 中用于表示 双目相机图像上的三元组像素坐标 的类。

• 左图像 u 坐标：uL_
• 右图像 u 坐标：uR_
• 垂直 v 坐标：v_

双目几何中常用 (uL, uR, v) 来表示一个特征点，其中视差 d = uL - uR 可以直接用来计算深度：
$$[Z=\frac{f\cdot B}{d}]$$
其中：

• (f) 是焦距
• (B) 是双目基线长度
• (d = u_L - u_R) 是像素视差

---

2. 成员变量

private:double uL_, uR_, v_;

• uL_：左相机的像素 x 坐标
• uR_：右相机的像素 x 坐标
• v_：像素 y 坐标

public:enum { dimension = 3 };

• 表示这个类在优化/Lie群中的维度是 3

---

3. 构造函数

StereoPoint2() : uL_(0), uR_(0), v_(0) {}
StereoPoint2(double uL, double uR, double v) : uL_(uL), uR_(uR), v_(v) {}
explicit StereoPoint2(const Vector3& v) : uL_(v(0)), uR_(v(1)), v_(v(2)) {}

• 默认构造函数初始化为零
• 可以直接指定 (uL, uR, v)
• 支持从 Vector3 构造

---

4. 打印与比较

void print(const std::string& s = "") const;
bool equals(const StereoPoint2& q, double tol = 1e-9) const;
bool operator==(const StereoPoint2& q) const;

• print：打印调试信息
• equals/operator==：判断两点是否相等，可设置容差

---

5. Lie群/向量操作（Group 接口）

inline static StereoPoint2 identity() { return StereoPoint2(); }
StereoPoint2 operator-() const { return StereoPoint2(-uL_, -uR_, -v_); }
inline StereoPoint2 operator +(const Vector3& v) const;
inline StereoPoint2 operator +(const StereoPoint2& b) const;
inline StereoPoint2 operator -(const StereoPoint2& b) const;

• 提供了类似 Lie 群的接口，方便在优化中进行增量操作
• identity() 返回零向量
• 支持加法、减法、逆操作

---

6. 标准接口

inline double uL() const { return uL_; }
inline double uR() const { return uR_; }
inline double v() const { return v_; }
Vector3 vector() const;
Point2 point2() const;
Point2 right() const;

• vector()：返回三维向量 (uL, uR, v)
• point2()：返回左图像二维点 (uL, v)
• right()：返回右图像二维点 (uR, v)

这在 SLAM/双目视觉里非常常用，比如用于投影误差计算或视觉重投影优化。

---

7. 过时接口（Deprecated）

inline StereoPoint2 inverse() const;
inline StereoPoint2 compose(const StereoPoint2& p1) const;
inline StereoPoint2 between(const StereoPoint2& p2) const;
inline Vector localCoordinates(const StereoPoint2& t2) const;
inline StereoPoint2 retract(const Vector& v) const;
static inline Vector Logmap(const StereoPoint2& p);
static inline StereoPoint2 Expmap(const Vector& d);

• 用于 GTSAM 的优化框架中
• 支持 Expmap/Logmap 等 Lie 群相关操作
• compose、between 可实现增量更新和误差计算

---

8. 序列化

template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {ar & BOOST_SERIALIZATION_NVP(uL_);ar & BOOST_SERIALIZATION_NVP(uR_);ar & BOOST_SERIALIZATION_NVP(v_);
}

• 支持 Boost Serialization
• 方便保存/加载 SLAM 点、关键帧数据

---

9. 总结

StereoPoint2 提供了一个 轻量级的双目图像点封装：

功能	描述	SLAM 使用场景
坐标存储	(uL, uR, v)	双目深度计算、重投影误差
Lie群接口	+, -, Expmap, Logmap	优化器增量更新
序列化	Boost 序列化支持	保存关键点、轨迹
转换接口	point2(), right()	投影到单目图像，视觉误差计算

---

10.典型使用示例

1️.双目三角化计算深度

#include <gtsam/geometry/StereoPoint2.h>
#include <iostream>int main() {// 左右相机像素坐标和相机参数double uL = 150, uR = 120, v = 200;double f = 525.0;     // 焦距double B = 0.1;       // 基线 0.1m// 使用 StereoPoint2 封装点gtsam::StereoPoint2 sp(uL, uR, v);// 计算视差double disparity = sp.uL() - sp.uR();// 双目深度计算double Z = f * B / disparity;std::cout << "Depth Z: " << Z << std::endl;return 0;
}

解释：

• StereoPoint2 封装了 (uL, uR, v)，直接用于计算深度
• SLAM 中双目前端可以用它来生成 3D 点云

---

2️.重投影误差计算

在视觉-里程计或图优化中，常需要计算三维点投影到左/右图的误差：

#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/geometry/Pose3.h>
#include <iostream>int main() {gtsam::StereoPoint2 observed(150, 120, 200);// 假设由估计的相机位姿得到的投影点gtsam::StereoPoint2 predicted(152, 122, 201);// 重投影误差向量gtsam::Vector3 error = predicted.vector() - observed.vector();std::cout << "Reprojection error: " << error.transpose() << std::endl;return 0;
}

解释：

• vector() 提供三维 (uL, uR, v) 向量
• 可直接用于 GTSAM 优化器构建误差项

---

3️.与优化器结合（增量更新）

在 Lie群优化中，StereoPoint2 可与 Expmap/Logmap 做增量更新：

#include <gtsam/geometry/StereoPoint2.h>
#include <iostream>int main() {gtsam::StereoPoint2 sp(150, 120, 200);// 模拟优化器的增量更新gtsam::Vector3 delta;delta << 1.0, -0.5, 0.2;// 通过 Expmap 增量更新gtsam::StereoPoint2 sp_updated = sp.retract(delta);std::cout << "Updated StereoPoint2: " << sp_updated.uL() << ", " << sp_updated.uR() << ", " << sp_updated.v() << std::endl;return 0;
}

解释：

• retract(Vector) = compose(Expmap(delta))
• 用于非线性优化中迭代更新像素点

---

4️.左右图投影与特征匹配

gtsam::StereoPoint2 sp(150, 120, 200);// 获取左图点
gtsam::Point2 left_pt = sp.point2();
std::cout << "Left image point: " << left_pt.transpose() << std::endl;// 获取右图点
gtsam::Point2 right_pt = sp.right();
std::cout << "Right image point: " << right_pt.transpose() << std::endl;

解释：

• 左右点可以直接用于匹配与重投影残差
• 常用于构建双目三角化约束

---