12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

change naming for local_point & world_point

release/4.3a0
zhaoyang 2015-06-12 16:55:38 -04:00
parent 8440e3c3b2
commit 3d18d70d69
1 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/geometry/StereoCamera.cpp
View File

@ -91,8 +91,8 @@ namespace gtsam {
double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
double X = Z * (measured[0] - K_->px()) / K_->fx(); double X = Z * (measured[0] - K_->px()) / K_->fx();
double Y = Z * (measured[2] - K_->py()) / K_->fy(); double Y = Z * (measured[2] - K_->py()) / K_->fy();
Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); Point3 world = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point; return world;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -105,28 +105,28 @@ namespace gtsam {
double disparity = uL - uR; double disparity = uL - uR;
double local_z = b * fx / disparity; double local_z = b * fx / disparity;
const Point3 local_point(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z); const Point3 local(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z);
if(H1 || H2) { if(H1 || H2) {
double z_partial_uR = local_z/disparity; double z_partial_uR = local_z/disparity;
double x_partial_uR = local_point.x()/disparity; double x_partial_uR = local.x()/disparity;
double y_partial_uR = local_point.y()/disparity; double y_partial_uR = local.y()/disparity;
Matrix3 D_local_z; Matrix3 D_local_z;
D_local_z << -x_partial_uR + local_point.z()/fx, x_partial_uR, 0, D_local_z << -x_partial_uR + local.z()/fx, x_partial_uR, 0,
-y_partial_uR, y_partial_uR, local_point.z() / fy, -y_partial_uR, y_partial_uR, local.z() / fy,
-z_partial_uR, z_partial_uR, 0; -z_partial_uR, z_partial_uR, 0;
Matrix3 D_point_local; Matrix3 D_point_local;
const Point3 world_point = leftCamPose_.transform_from(local_point, H1, D_point_local); const Point3 world = leftCamPose_.transform_from(local, H1, D_point_local);
if(H2) { if(H2) {
*H2 = D_point_local * D_local_z; *H2 = D_point_local * D_local_z;
} }
return world_point; return world;
} }
return leftCamPose_.transform_from(local_point); return leftCamPose_.transform_from(local);
} }
} }