29 #include "../core/factory.h" 30 #include "../stuff/macros.h" 57 for (
int i=0; i<7; i++)
67 for (
int i=0; i<7; i++)
68 os << cam2world[i] <<
" ";
77 for (
int i=0; i<2; i++){
80 for (
int i=0; i<2; i++)
81 for (
int j=i; j<2; j++) {
91 for (
int i=0; i<2; i++){
95 for (
int i=0; i<2; i++)
96 for (
int j=i; j<2; j++){
108 Vector3d xyz_trans = T.map(xyz);
110 double x = xyz_trans[0];
111 double y = xyz_trans[1];
112 double z = xyz_trans[2];
115 Matrix<double,2,3> tmp;
144 res[0] = proj[0]*
fx +
cx;
145 res[1] = proj[1]*
fy +
cy;
151 const float invz = 1.0f/trans_xyz[2];
153 res[0] = trans_xyz[0]*invz*
fx +
cx;
154 res[1] = trans_xyz[1]*invz*
fy +
cy;
155 res[2] = res[0] - bf*invz;
163 for (
int i=0; i<=3; i++){
166 for (
int i=0; i<=2; i++)
167 for (
int j=i; j<=2; j++) {
177 for (
int i=0; i<=3; i++){
181 for (
int i=0; i<=2; i++)
182 for (
int j=i; j<=2; j++){
193 Vector3d xyz_trans = T.map(xyz);
195 const Matrix3d R = T.rotation().toRotationMatrix();
197 double x = xyz_trans[0];
198 double y = xyz_trans[1];
199 double z = xyz_trans[2];
240 for (
int i=0; i<2; i++){
243 for (
int i=0; i<2; i++)
244 for (
int j=i; j<2; j++) {
254 for (
int i=0; i<2; i++){
258 for (
int i=0; i<2; i++)
259 for (
int j=i; j<2; j++){
270 double x = xyz_trans[0];
271 double y = xyz_trans[1];
272 double invz = 1.0/xyz_trans[2];
273 double invz_2 = invz*invz;
293 res[0] = proj[0]*
fx +
cx;
294 res[1] = proj[1]*
fy +
cy;
300 const float invz = 1.0f/trans_xyz[2];
302 res[0] = trans_xyz[0]*invz*
fx +
cx;
303 res[1] = trans_xyz[1]*invz*
fy +
cy;
304 res[2] = res[0] -
bf*invz;
310 for (
int i=0; i<=3; i++){
313 for (
int i=0; i<=2; i++)
314 for (
int j=i; j<=2; j++) {
324 for (
int i=0; i<=3; i++){
328 for (
int i=0; i<=2; i++)
329 for (
int j=i; j<=2; j++){
339 double x = xyz_trans[0];
340 double y = xyz_trans[1];
341 double invz = 1.0/xyz_trans[2];
342 double invz_2 = invz*invz;
const Measurement & measurement() const
accessor functions for the measurement represented by the edge
Vector2d project2d(const Vector3d &v)
Matrix< double, 7, 1 > Vector7d
bool write(std::ostream &os) const
write the vertex to a stream
virtual void linearizeOplus()
bool write(std::ostream &os) const
write the vertex to a stream
bool read(std::istream &is)
read the vertex from a stream, i.e., the internal state of the vertex
virtual void linearizeOplus()
const InformationType & information() const
information matrix of the constraint
TFSIMD_FORCE_INLINE const tfScalar & y() const
Vector3d cam_project(const Vector3d &trans_xyz, const float &bf) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VertexSE3Expmap()
JacobianXiOplusType _jacobianOplusXi
SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential ...
Vector3d cam_project(const Vector3d &trans_xyz) const
Vector3d unproject2d(const Vector2d &v)
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
virtual void linearizeOplus()
void fromVector(const Vector7d &v)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void setEstimate(const EstimateType &et)
set the estimate for the vertex also calls updateCache()
TFSIMD_FORCE_INLINE const tfScalar & z() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3ProjectXYZ()
virtual void linearizeOplus()
bool read(std::istream &is)
read the vertex from a stream, i.e., the internal state of the vertex
bool write(std::ostream &os) const
write the vertex to a stream
bool write(std::ostream &os) const
write the vertex to a stream
Vector2d cam_project(const Vector3d &trans_xyz) const
bool read(std::istream &is)
read the vertex from a stream, i.e., the internal state of the vertex
const EstimateType & estimate() const
return the current estimate of the vertex
Vector2d cam_project(const Vector3d &trans_xyz) const
JacobianXjOplusType _jacobianOplusXj
Vector3d map(const Vector3d &xyz) const
bool write(std::ostream &os) const
write the vertex to a stream
bool read(std::istream &is)
read the vertex from a stream, i.e., the internal state of the vertex
bool read(std::istream &is)
read the vertex from a stream, i.e., the internal state of the vertex
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeStereoSE3ProjectXYZ()
VertexContainer _vertices