simplify camera projection sensor
PiperOrigin-RevId: 936571266 Change-Id: I5647e61fcc81ba6eb482dd95d3c40438f478bb18
This commit is contained in:
Taylor Howell
committed by
Copybara-Service
Copybara-Service
parent
da2f0ebd7b
commit
027cfd1201
+10
-64
@@ -284,29 +284,7 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
|
||||
const float cam_intrinsic[4], const float cam_sensorsize[2]) {
|
||||
mjtNum fx, fy;
|
||||
|
||||
// translation matrix (4x4)
|
||||
mjtNum translation[4][4] = {0};
|
||||
translation[0][0] = 1;
|
||||
translation[1][1] = 1;
|
||||
translation[2][2] = 1;
|
||||
translation[3][3] = 1;
|
||||
translation[0][3] = -cam_xpos[0];
|
||||
translation[1][3] = -cam_xpos[1];
|
||||
translation[2][3] = -cam_xpos[2];
|
||||
|
||||
// rotation matrix (4x4)
|
||||
mjtNum rotation[4][4] = {0};
|
||||
rotation[0][0] = 1;
|
||||
rotation[1][1] = 1;
|
||||
rotation[2][2] = 1;
|
||||
rotation[3][3] = 1;
|
||||
for (int i=0; i < 3; i++) {
|
||||
for (int j=0; j < 3; j++) {
|
||||
rotation[i][j] = cam_xmat[j*3+i];
|
||||
}
|
||||
}
|
||||
|
||||
// focal transformation matrix (3x4)
|
||||
// focal transformation
|
||||
if (cam_sensorsize[0] && cam_sensorsize[1]) {
|
||||
fx = cam_intrinsic[0] / cam_sensorsize[0] * cam_res[0];
|
||||
fy = cam_intrinsic[1] / cam_sensorsize[1] * cam_res[1];
|
||||
@@ -314,48 +292,16 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
|
||||
fx = fy = .5 / mju_tan(cam_fovy * mjPI / 360.) * cam_res[1];
|
||||
}
|
||||
|
||||
mjtNum focal[3][4] = {0};
|
||||
focal[0][0] = -fx;
|
||||
focal[1][1] = fy;
|
||||
focal[2][2] = 1.0;
|
||||
// relative position in world frame
|
||||
mjtNum relative_pos[3];
|
||||
mju_sub3(relative_pos, target_xpos, cam_xpos);
|
||||
|
||||
// image matrix (3x3)
|
||||
mjtNum image[3][3] = {0};
|
||||
image[0][0] = 1;
|
||||
image[1][1] = 1;
|
||||
image[2][2] = 1;
|
||||
image[0][2] = (mjtNum)cam_res[0] / 2.0;
|
||||
image[1][2] = (mjtNum)cam_res[1] / 2.0;
|
||||
|
||||
// projection matrix (3x4): product of all 4 matrices
|
||||
mjtNum proj[3][4] = {0};
|
||||
for (int i=0; i < 3; i++) {
|
||||
for (int j=0; j < 3; j++) {
|
||||
for (int k=0; k < 4; k++) {
|
||||
for (int l=0; l < 4; l++) {
|
||||
for (int n=0; n < 4; n++) {
|
||||
proj[i][n] += image[i][j] * focal[j][k] * rotation[k][l] * translation[l][n];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// projection matrix multiplies homogenous [x, y, z, 1] vectors
|
||||
mjtNum pos_hom[4] = {0, 0, 0, 1};
|
||||
mju_copy3(pos_hom, target_xpos);
|
||||
|
||||
// project world coordinates into pixel space, see:
|
||||
// https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula
|
||||
mjtNum pixel_coord_hom[3] = {0};
|
||||
for (int i=0; i < 3; i++) {
|
||||
for (int j=0; j < 4; j++) {
|
||||
pixel_coord_hom[i] += proj[i][j] * pos_hom[j];
|
||||
}
|
||||
}
|
||||
// project to camera frame: cam_pos = cam_xmat^T * relative_pos
|
||||
mjtNum cam_pos[3];
|
||||
mju_mulMatTVec(cam_pos, cam_xmat, relative_pos, 3, 3);
|
||||
|
||||
// avoid dividing by tiny numbers
|
||||
mjtNum denom = pixel_coord_hom[2];
|
||||
mjtNum denom = cam_pos[2];
|
||||
if (mju_abs(denom) < mjMINVAL) {
|
||||
if (denom < 0) {
|
||||
denom = mju_min(denom, -mjMINVAL);
|
||||
@@ -365,8 +311,8 @@ static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
|
||||
}
|
||||
|
||||
// compute projection
|
||||
sensordata[0] = pixel_coord_hom[0] / denom;
|
||||
sensordata[1] = pixel_coord_hom[1] / denom;
|
||||
sensordata[0] = -fx * (cam_pos[0] / denom) + 0.5 * (mjtNum)cam_res[0];
|
||||
sensordata[1] = fy * (cam_pos[1] / denom) + 0.5 * (mjtNum)cam_res[1];
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -982,9 +982,9 @@ TEST_F(SensorTest, CameraProjection) {
|
||||
MjModelPtr model = LoadModelFromString(xml);
|
||||
MjDataPtr data = MakeData(model);
|
||||
|
||||
// call step to update sensors
|
||||
mj_step(model.get(), data.get());
|
||||
mj_step1(model.get(), data.get()); // update values of position-based sensors
|
||||
// update positions and sensors
|
||||
mj_fwdPosition(model.get(), data.get());
|
||||
mj_sensorPos(model.get(), data.get());
|
||||
EXPECT_THAT(model->cam_resolution[0], 1920);
|
||||
EXPECT_THAT(model->cam_resolution[1], 1200);
|
||||
mjtNum eps = 1e-4;
|
||||
@@ -996,6 +996,165 @@ TEST_F(SensorTest, CameraProjection) {
|
||||
EXPECT_NEAR(data->sensordata[5], 600, eps);
|
||||
}
|
||||
|
||||
// previous implementation of cam_project to verify the new one
|
||||
static void cam_project_old(
|
||||
mjtNum sensordata[2], const mjtNum target_xpos[3],
|
||||
const mjtNum cam_xpos[3], const mjtNum cam_xmat[9],
|
||||
const int cam_res[2], mjtNum cam_fovy,
|
||||
const float cam_intrinsic[4], const float cam_sensorsize[2]) {
|
||||
mjtNum fx, fy;
|
||||
|
||||
// translation matrix (4x4)
|
||||
mjtNum translation[4][4] = {};
|
||||
translation[0][0] = 1;
|
||||
translation[1][1] = 1;
|
||||
translation[2][2] = 1;
|
||||
translation[3][3] = 1;
|
||||
translation[0][3] = -cam_xpos[0];
|
||||
translation[1][3] = -cam_xpos[1];
|
||||
translation[2][3] = -cam_xpos[2];
|
||||
|
||||
// rotation matrix (4x4)
|
||||
mjtNum rotation[4][4] = {};
|
||||
rotation[0][0] = 1;
|
||||
rotation[1][1] = 1;
|
||||
rotation[2][2] = 1;
|
||||
rotation[3][3] = 1;
|
||||
for (int i=0; i < 3; i++) {
|
||||
for (int j=0; j < 3; j++) {
|
||||
rotation[i][j] = cam_xmat[j*3+i];
|
||||
}
|
||||
}
|
||||
|
||||
// focal transformation matrix (3x4)
|
||||
if (cam_sensorsize[0] && cam_sensorsize[1]) {
|
||||
fx = cam_intrinsic[0] / cam_sensorsize[0] * cam_res[0];
|
||||
fy = cam_intrinsic[1] / cam_sensorsize[1] * cam_res[1];
|
||||
} else {
|
||||
fx = fy = .5 / mju_tan(cam_fovy * mjPI / 360.) * cam_res[1];
|
||||
}
|
||||
|
||||
mjtNum focal[3][4] = {};
|
||||
focal[0][0] = -fx;
|
||||
focal[1][1] = fy;
|
||||
focal[2][2] = 1.0;
|
||||
|
||||
// image matrix (3x3)
|
||||
mjtNum image[3][3] = {};
|
||||
image[0][0] = 1;
|
||||
image[1][1] = 1;
|
||||
image[2][2] = 1;
|
||||
image[0][2] = (mjtNum)cam_res[0] / 2.0;
|
||||
image[1][2] = (mjtNum)cam_res[1] / 2.0;
|
||||
|
||||
// projection matrix (3x4): product of all 4 matrices
|
||||
mjtNum proj[3][4] = {};
|
||||
for (int i=0; i < 3; i++) {
|
||||
for (int j=0; j < 3; j++) {
|
||||
for (int k=0; k < 4; k++) {
|
||||
for (int l=0; l < 4; l++) {
|
||||
for (int n=0; n < 4; n++) {
|
||||
proj[i][n] += image[i][j] * focal[j][k] *
|
||||
rotation[k][l] * translation[l][n];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// projection matrix multiplies homogenous [x, y, z, 1] vectors
|
||||
mjtNum pos_hom[4] = {0, 0, 0, 1};
|
||||
mju_copy3(pos_hom, target_xpos);
|
||||
|
||||
// project world coordinates into pixel space, see:
|
||||
// https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula
|
||||
mjtNum pixel_coord_hom[3] = {0};
|
||||
for (int i=0; i < 3; i++) {
|
||||
for (int j=0; j < 4; j++) {
|
||||
pixel_coord_hom[i] += proj[i][j] * pos_hom[j];
|
||||
}
|
||||
}
|
||||
|
||||
// avoid dividing by tiny numbers
|
||||
mjtNum denom = pixel_coord_hom[2];
|
||||
if (mju_abs(denom) < mjMINVAL) {
|
||||
if (denom < 0) {
|
||||
denom = mju_min(denom, -mjMINVAL);
|
||||
} else {
|
||||
denom = mju_max(denom, mjMINVAL);
|
||||
}
|
||||
}
|
||||
|
||||
// compute projection
|
||||
sensordata[0] = pixel_coord_hom[0] / denom;
|
||||
sensordata[1] = pixel_coord_hom[1] / denom;
|
||||
}
|
||||
|
||||
TEST_F(SensorTest, CameraProjectionComparison) {
|
||||
constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body pos="1.5 0.5 1.2">
|
||||
<site name="target1" pos="0 0 0"/>
|
||||
</body>
|
||||
<body pos="-0.8 -1.2 0.5">
|
||||
<site name="target2" pos="0 0 0"/>
|
||||
</body>
|
||||
<body pos="-0.5 0.5 0.5">
|
||||
<site name="target3" pos="0 0 0"/>
|
||||
</body>
|
||||
|
||||
<camera name="cam1" pos="0 0 1" xyaxes="0 -1 0 0 0 1" fovy="45" resolution="800 600"/>
|
||||
<camera name="cam2" pos="1 -1 2" euler="10 20 30" fovy="60" resolution="1024 768"/>
|
||||
<camera name="cam3" pos="-1 1 1.5" euler="-10 -20 30"
|
||||
resolution="640 480" sensorsize="0.032 0.024"
|
||||
focalpixel="500 500"/>
|
||||
</worldbody>
|
||||
<sensor>
|
||||
<camprojection site="target1" camera="cam1"/>
|
||||
<camprojection site="target2" camera="cam1"/>
|
||||
<camprojection site="target3" camera="cam1"/>
|
||||
|
||||
<camprojection site="target1" camera="cam2"/>
|
||||
<camprojection site="target2" camera="cam2"/>
|
||||
<camprojection site="target3" camera="cam2"/>
|
||||
|
||||
<camprojection site="target1" camera="cam3"/>
|
||||
<camprojection site="target2" camera="cam3"/>
|
||||
<camprojection site="target3" camera="cam3"/>
|
||||
</sensor>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||
MjDataPtr data = MakeData(model);
|
||||
|
||||
mj_fwdPosition(model.get(), data.get());
|
||||
mj_sensorPos(model.get(), data.get());
|
||||
|
||||
for (int i = 0; i < model->nsensor; ++i) {
|
||||
if (model->sensor_type[i] == mjSENS_CAMPROJECTION) {
|
||||
int objid = model->sensor_objid[i];
|
||||
int refid = model->sensor_refid[i];
|
||||
|
||||
mjtNum expected[2];
|
||||
cam_project_old(expected,
|
||||
data->site_xpos + 3*objid,
|
||||
data->cam_xpos + 3*refid,
|
||||
data->cam_xmat + 9*refid,
|
||||
model->cam_resolution + 2*refid,
|
||||
model->cam_fovy[refid],
|
||||
model->cam_intrinsic + 4*refid,
|
||||
model->cam_sensorsize + 2*refid);
|
||||
|
||||
int adr = model->sensor_adr[i];
|
||||
EXPECT_NEAR(data->sensordata[adr], expected[0], MjTol(1e-10, 1e-3));
|
||||
EXPECT_NEAR(data->sensordata[adr + 1], expected[1], MjTol(1e-10, 1e-3));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(SensorTest, InsideSite) {
|
||||
constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
|
||||
Reference in New Issue
Block a user