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 X = Z * (measured[0] - K_->px()) / K_->fx();
double Y = Z * (measured[2] - K_->py()) / K_->fy();
Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point;
Point3 world = leftCamPose_.transform_from(Point3(X, Y, Z));
return world;
}
/* ************************************************************************* */
@ -105,28 +105,28 @@ namespace gtsam {
double disparity = uL - uR;
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) {
double z_partial_uR = local_z/disparity;
double x_partial_uR = local_point.x()/disparity;
double y_partial_uR = local_point.y()/disparity;
double x_partial_uR = local.x()/disparity;
double y_partial_uR = local.y()/disparity;
Matrix3 D_local_z;
D_local_z << -x_partial_uR + local_point.z()/fx, x_partial_uR, 0,
-y_partial_uR, y_partial_uR, local_point.z() / fy,
D_local_z << -x_partial_uR + local.z()/fx, x_partial_uR, 0,
-y_partial_uR, y_partial_uR, local.z() / fy,
-z_partial_uR, z_partial_uR, 0;
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) {
*H2 = D_point_local * D_local_z;
}
return world_point;
return world;
}
return leftCamPose_.transform_from(local_point);
return leftCamPose_.transform_from(local);
}
}