Skip to content
This repository was archived by the owner on Feb 3, 2025. It is now read-only.

Commit

Permalink
add test that actually catches current failure. Currently, translatio…
Browse files Browse the repository at this point in the history 
…n in <camera><pose> tag is not applied to the camera in multicamera.
  • Loading branch information
John Hsu committed Jan 21, 2014
1 parent 2fe3acf commit 077ebae
Show file tree
Hide file tree
Showing 3 changed files with 210 additions and 44 deletions.
1 change: 1 addition & 0 deletions gazebo/rendering/Camera.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -635,6 +635,7 @@ void Camera::SetWorldPosition(const math::Vector3 &_pos)
return;

this->sceneNode->setPosition(Ogre::Vector3(_pos.x, _pos.y, _pos.z));
this->sceneNode->needUpdate();
}

//////////////////////////////////////////////////
Expand Down
200 changes: 157 additions & 43 deletions test/integration/multicamera_sensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,12 +30,22 @@ class MultiCameraSensor : public ServerFixture
{
};

unsigned char* img = NULL;
unsigned char* img1 = NULL;
unsigned char* img2 = NULL;
int imageCount = 0;
int imageCount1 = 0;
int imageCount2 = 0;
unsigned char* img0Left = NULL;
unsigned char* imgtLeft = NULL;
unsigned char* img1Left = NULL;
unsigned char* img2Left = NULL;
unsigned char* img0Right = NULL;
unsigned char* imgtRight = NULL;
unsigned char* img1Right = NULL;
unsigned char* img2Right = NULL;
int imageCount0Left = 0;
int imageCounttLeft = 0;
int imageCount1Left = 0;
int imageCount2Left = 0;
int imageCount0Right = 0;
int imageCounttRight = 0;
int imageCount1Right = 0;
int imageCount2Right = 0;

void OnNewFrameTest(int* _imageCounter, unsigned char* _imageDest,
const unsigned char *_image,
Expand All @@ -62,13 +72,18 @@ TEST_F(MultiCameraSensor, CameraRotationTest)

// get two cameras, one with rotation and one without.
std::string cameraUnrotated = "multicamera_sensor_unrotated";
std::string cameraTranslated = "multicamera_sensor_translated";
std::string cameraRotated1 = "multicamera_sensor_rotated1";
std::string cameraRotated2 = "multicamera_sensor_rotated2";

sensors::SensorPtr sensor = sensors::get_sensor(cameraUnrotated);
sensors::MultiCameraSensorPtr camSensorUnrotated =
boost::dynamic_pointer_cast<sensors::MultiCameraSensor>(sensor);

sensor = sensors::get_sensor(cameraTranslated);
sensors::MultiCameraSensorPtr camSensorTranslated =
boost::dynamic_pointer_cast<sensors::MultiCameraSensor>(sensor);

sensor = sensors::get_sensor(cameraRotated1);
sensors::MultiCameraSensorPtr camSensorRotated1 =
boost::dynamic_pointer_cast<sensors::MultiCameraSensor>(sensor);
Expand All @@ -82,54 +97,138 @@ TEST_F(MultiCameraSensor, CameraRotationTest)
unsigned int depth = 3;

// initialize global variables
imageCount = 0;
imageCount1 = 0;
imageCount2 = 0;
img = new unsigned char[width * height * depth];
img1 = new unsigned char[width * height * depth];
img2 = new unsigned char[width * height * depth];

for (unsigned int i = 0; i < 2; ++i)
imageCount0Left = 0;
imageCounttLeft = 0;
imageCount1Left = 0;
imageCount2Left = 0;
imageCount0Right = 0;
imageCounttRight = 0;
imageCount1Right = 0;
imageCount2Right = 0;
img0Left = new unsigned char[width * height * depth];
imgtLeft = new unsigned char[width * height * depth];
img1Left = new unsigned char[width * height * depth];
img2Left = new unsigned char[width * height * depth];
img0Right = new unsigned char[width * height * depth];
imgtRight = new unsigned char[width * height * depth];
img1Right = new unsigned char[width * height * depth];
img2Right = new unsigned char[width * height * depth];

{
// connect to camera image updates
event::ConnectionPtr c =
camSensorUnrotated->GetCamera(i)->ConnectNewImageFrame(
boost::bind(&::OnNewFrameTest, &imageCount, img,
event::ConnectionPtr c0Left =
camSensorUnrotated->GetCamera(0)->ConnectNewImageFrame(
boost::bind(&::OnNewFrameTest, &imageCount0Left, img0Left,
_1, _2, _3, _4, _5));
event::ConnectionPtr ctLeft =
camSensorTranslated->GetCamera(0)->ConnectNewImageFrame(
boost::bind(&::OnNewFrameTest, &imageCounttLeft, imgtLeft,
_1, _2, _3, _4, _5));
event::ConnectionPtr c1Left =
camSensorRotated1->GetCamera(0)->ConnectNewImageFrame(
boost::bind(&::OnNewFrameTest, &imageCount1Left, img1Left,
_1, _2, _3, _4, _5));
event::ConnectionPtr c2Left =
camSensorRotated2->GetCamera(0)->ConnectNewImageFrame(
boost::bind(&::OnNewFrameTest, &imageCount2Left, img2Left,
_1, _2, _3, _4, _5));
event::ConnectionPtr c1 =
camSensorRotated1->GetCamera(i)->ConnectNewImageFrame(
boost::bind(&::OnNewFrameTest, &imageCount1, img1,

event::ConnectionPtr c0Right =
camSensorUnrotated->GetCamera(1)->ConnectNewImageFrame(
boost::bind(&::OnNewFrameTest, &imageCount0Right, img0Right,
_1, _2, _3, _4, _5));
event::ConnectionPtr ctRight =
camSensorTranslated->GetCamera(1)->ConnectNewImageFrame(
boost::bind(&::OnNewFrameTest, &imageCounttRight, imgtRight,
_1, _2, _3, _4, _5));
event::ConnectionPtr c1Right =
camSensorRotated1->GetCamera(1)->ConnectNewImageFrame(
boost::bind(&::OnNewFrameTest, &imageCount1Right, img1Right,
_1, _2, _3, _4, _5));
event::ConnectionPtr c2 =
camSensorRotated2->GetCamera(i)->ConnectNewImageFrame(
boost::bind(&::OnNewFrameTest, &imageCount2, img2,
event::ConnectionPtr c2Right =
camSensorRotated2->GetCamera(1)->ConnectNewImageFrame(
boost::bind(&::OnNewFrameTest, &imageCount2Right, img2Right,
_1, _2, _3, _4, _5));

// activate camera
camSensorUnrotated->SetActive(true);
camSensorTranslated->SetActive(true);
camSensorRotated1->SetActive(true);
camSensorRotated2->SetActive(true);

// Get at least 10 images from each camera
int waitCount = 0;
while (imageCount < 10 || imageCount1 < 10 || imageCount2 < 10)
while (imageCount0Left < 10 || imageCounttLeft < 10 ||
imageCount1Left < 10 || imageCount2Left < 10)
{
// wait at most 10 seconds sim time.
if (++waitCount >= 1000)
gzerr << "[" << imageCount
<< "/10, " << imageCount1
<< "/10, " << imageCount2
gzerr << "Err [" << imageCount0Left
<< "/10, " << imageCounttLeft
<< "/10, " << imageCount1Left
<< "/10, " << imageCount2Left
<< "/10, " << imageCount0Right
<< "/10, " << imageCounttRight
<< "/10, " << imageCount1Right
<< "/10, " << imageCount2Right
<< "/10] images received from cameras\n";
EXPECT_LT(waitCount, 1000);

common::Time::MSleep(10);
}

// compare unrotated left against translated left, both should be
// seeing the green block, if not, pose translation in <camera> tag
// is broken.
// Note, right camera of translated camera (imgtRight) should also
// see green block.
{
unsigned int diffMax = 0, diffSum = 0;
double diffAvg = 0.0;

// compare left images
this->ImageCompare(img0Left, imgtLeft, width, height, depth,
diffMax, diffSum, diffAvg);

// We expect that there will be some non-zero difference between the two
// images.
EXPECT_EQ(diffSum, 0u);

// We expect that the average difference will be well within 3-sigma.
EXPECT_NEAR(diffAvg/255., 0.0, 1e-16);

// compare right of translated camera to
// right of unrotated/untranslated camera images
// the result should differ as former sees green block and latter
// sees red block.
this->ImageCompare(img0Right, imgtRight, width, height, depth,
diffMax, diffSum, diffAvg);

// We expect that there will be some non-zero difference between the two
// images.
EXPECT_NE(diffSum, 0u);

// We expect that the average difference will be well within 3-sigma.
EXPECT_GT(fabs(diffAvg)/255., 1e-16);
}

// compare unrotated against rotated1
{
unsigned int diffMax = 0, diffSum = 0;
double diffAvg = 0.0;
this->ImageCompare(img, img1, width, height, depth,
// compare left images
this->ImageCompare(img0Left, img1Left, width, height, depth,
diffMax, diffSum, diffAvg);

// We expect that there will be some non-zero difference between the two
// images.
EXPECT_EQ(diffSum, 0u);

// We expect that the average difference will be well within 3-sigma.
EXPECT_NEAR(diffAvg/255., 0.0, 1e-16);

// compare right images
this->ImageCompare(img0Right, img1Right, width, height, depth,
diffMax, diffSum, diffAvg);

// We expect that there will be some non-zero difference between the two
Expand All @@ -144,17 +243,21 @@ TEST_F(MultiCameraSensor, CameraRotationTest)
{
unsigned int diffMax = 0, diffSum = 0;
double diffAvg = 0.0;
this->ImageCompare(img, img2, width, height, depth,

// compare left images
this->ImageCompare(img0Left, img2Left, width, height, depth,
diffMax, diffSum, diffAvg);

// for (unsigned j = 0; j < width * height * depth; ++j)
// {
// unsigned int p = static_cast<unsigned int>(img[j]);
// unsigned int p2 = static_cast<unsigned int>(img2[j]);
// // if (p != p2)
// if (p != p2)
// gzerr << j << " " << p << " " << p2 << "\n";
// }
// We expect that there will be some non-zero difference between the two
// images.
EXPECT_EQ(diffSum, 0u);

// We expect that the average difference will be well within 3-sigma.
EXPECT_NEAR(diffAvg/255., 0.0, 1e-16);

// compare right images
this->ImageCompare(img0Right, img2Right, width, height, depth,
diffMax, diffSum, diffAvg);

// We expect that there will be some non-zero difference between the two
// images.
Expand All @@ -166,19 +269,30 @@ TEST_F(MultiCameraSensor, CameraRotationTest)

// activate camera
camSensorUnrotated->SetActive(false);
camSensorTranslated->SetActive(false);
camSensorRotated1->SetActive(false);
camSensorRotated2->SetActive(false);

// disconnect callbacks
camSensorUnrotated->GetCamera(i)->DisconnectNewImageFrame(c);
camSensorRotated1->GetCamera(i)->DisconnectNewImageFrame(c1);
camSensorRotated2->GetCamera(i)->DisconnectNewImageFrame(c2);
camSensorUnrotated->GetCamera(0)->DisconnectNewImageFrame(c0Left);
camSensorTranslated->GetCamera(0)->DisconnectNewImageFrame(ctLeft);
camSensorRotated1->GetCamera(0)->DisconnectNewImageFrame(c1Left);
camSensorRotated2->GetCamera(0)->DisconnectNewImageFrame(c2Left);
camSensorUnrotated->GetCamera(1)->DisconnectNewImageFrame(c0Right);
camSensorTranslated->GetCamera(1)->DisconnectNewImageFrame(ctRight);
camSensorRotated1->GetCamera(1)->DisconnectNewImageFrame(c1Right);
camSensorRotated2->GetCamera(1)->DisconnectNewImageFrame(c2Right);
}

// cleanup
delete[] img;
delete[] img1;
delete[] img2;
delete[] img0Left;
delete[] imgtLeft;
delete[] img1Left;
delete[] img2Left;
delete[] img0Right;
delete[] imgtRight;
delete[] img1Right;
delete[] img2Right;
}

int main(int argc, char **argv)
Expand Down
53 changes: 52 additions & 1 deletion test/worlds/camera_rotation_test.world
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@
</visual>
</link>
</model>
<!-- unrotated cameras for ground truth -->
<!-- unrotated cameras for red block ground truth -->
<model name="cam_x_rot_test_unrotated_cameras_1">
<static>1</static>
<pose>0 10 1 0 0 0</pose>
Expand All @@ -60,6 +60,7 @@
<update_rate>15</update_rate>
<pose>0 0 0 0 0 0</pose>
<camera name="left">
<!-- should be in front of green block -->
<pose>0 1.0 0 0 0 0</pose>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
Expand All @@ -72,6 +73,55 @@
<far>100</far>
</clip>
</camera>
<camera name="right">
<!-- should be in front of red block -->
<pose>0 0 0 0 0 0</pose>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1024</width>
<height>544</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>100</far>
</clip>
</camera>
</sensor>
</link>
</model>
<!-- Unrotated but translated cameras for ground truth.
Both cameras should see the green block.
Use this camera for green block ground truth in testing.
-->
<model name="cam_x_rot_test_translated_cameras_1">
<static>1</static>
<pose>0 11 1 0 0 0</pose>
<link name="cam_x_rot_test_2_link">
<visual name="cam_x_rot_test_1_vis">
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.01 0.2 0.4</size>
</box>
</geometry>
</visual>
<sensor name="multicamera_sensor_translated" type="multicamera">
<update_rate>15</update_rate>
<pose>0 0 0 0 0 0</pose>
<camera name="left">
<pose>0 0 0 0 0 0</pose>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1024</width>
<height>544</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>100</far>
</clip>
</camera>
<camera name="right">
<pose>0 0 0 0 0 0</pose>
<horizontal_fov>1.3962634</horizontal_fov>
Expand Down Expand Up @@ -187,6 +237,7 @@
<sensor name="multicamera_sensor_rotated2" type="multicamera">
<update_rate>15</update_rate>
<camera name="left">
<!-- offset left cameras to put it in front of the green block -->
<pose>0 0.36235775447 0.93203908596 1.2 0 0</pose>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
Expand Down

0 comments on commit 077ebae

Please sign in to comment.