p81523470
/
ppovb5fc7
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
ppovb5fc7/gazebo/test/integration/multicamera_sensor.cc

587 lines
22 KiB
C++
Raw Blame History

/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include "gazebo/physics/physics.hh"
#include "gazebo/sensors/sensors.hh"
#include "gazebo/common/common.hh"
#include "gazebo/common/Timer.hh"
#include "gazebo/rendering/Camera.hh"
#include "gazebo/rendering/Conversions.hh"
#include "gazebo/sensors/MultiCameraSensor.hh"
#include "gazebo/test/ServerFixture.hh"
#include "scans_cmp.h"
using namespace gazebo;
class MultiCameraSensor : public ServerFixture
{
};
unsigned char* img0Left = NULL;
unsigned char* imgt = NULL;
unsigned char* img1Left = NULL;
unsigned char* img2Left = NULL;
unsigned char* img0Right = NULL;
unsigned char* img1Right = NULL;
unsigned char* img2Right = NULL;
int imageCount0Left = 0;
int imageCountt = 0;
int imageCount1Left = 0;
int imageCount2Left = 0;
int imageCount0Right = 0;
int imageCount1Right = 0;
int imageCount2Right = 0;
/////////////////////////////////////////////////
void OnNewFrameTest(int* _imageCounter, unsigned char* _imageDest,
const unsigned char *_image,
unsigned int _width, unsigned int _height,
unsigned int _depth,
const std::string &/*_format*/)
{
memcpy(_imageDest, _image, _width * _height * _depth);
*_imageCounter += 1;
}
/////////////////////////////////////////////////
TEST_F(MultiCameraSensor, CameraRotationTest)
{
Load("worlds/camera_rotation_test.world");
// Make sure the render engine is available.
if (rendering::RenderEngine::Instance()->GetRenderPathType() ==
rendering::RenderEngine::NONE)
{
gzerr << "No rendering engine, unable to run camera test\n";
return;
}
// get two cameras, one with rotation and one without.
std::string cameraUnrotated = "multicamera_sensor_unrotated";
std::string cameraTranslated = "camera_sensor_translated";
std::string cameraRotated1 = "multicamera_sensor_rotated1";
std::string cameraRotated2 = "multicamera_sensor_rotated2";
sensors::SensorPtr sensor = sensors::get_sensor(cameraUnrotated);
sensors::MultiCameraSensorPtr camSensorUnrotated =
std::dynamic_pointer_cast<sensors::MultiCameraSensor>(sensor);
sensor = sensors::get_sensor(cameraTranslated);
sensors::CameraSensorPtr camSensorTranslated =
std::dynamic_pointer_cast<sensors::CameraSensor>(sensor);
sensor = sensors::get_sensor(cameraRotated1);
sensors::MultiCameraSensorPtr camSensorRotated1 =
std::dynamic_pointer_cast<sensors::MultiCameraSensor>(sensor);
sensor = sensors::get_sensor(cameraRotated2);
sensors::MultiCameraSensorPtr camSensorRotated2 =
std::dynamic_pointer_cast<sensors::MultiCameraSensor>(sensor);
unsigned int width = 1024;
unsigned int height = 544;
unsigned int depth = 3;
// initialize global variables
imageCount0Left = 0;
imageCountt = 0;
imageCount1Left = 0;
imageCount2Left = 0;
imageCount0Right = 0;
imageCount1Right = 0;
imageCount2Right = 0;
img0Left = new unsigned char[width * height * depth];
imgt = 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];
img1Right = new unsigned char[width * height * depth];
img2Right = new unsigned char[width * height * depth];
{
// connect to camera image updates
event::ConnectionPtr c0Left =
camSensorUnrotated->Camera(0)->ConnectNewImageFrame(
std::bind(&::OnNewFrameTest, &imageCount0Left, img0Left,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_5));
event::ConnectionPtr ct =
camSensorTranslated->Camera()->ConnectNewImageFrame(
std::bind(&::OnNewFrameTest, &imageCountt, imgt,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_5));
event::ConnectionPtr c1Left =
camSensorRotated1->Camera(0)->ConnectNewImageFrame(
std::bind(&::OnNewFrameTest, &imageCount1Left, img1Left,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_5));
event::ConnectionPtr c2Left =
camSensorRotated2->Camera(0)->ConnectNewImageFrame(
std::bind(&::OnNewFrameTest, &imageCount2Left, img2Left,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_5));
event::ConnectionPtr c0Right =
camSensorUnrotated->Camera(1)->ConnectNewImageFrame(
std::bind(&::OnNewFrameTest, &imageCount0Right, img0Right,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_5));
event::ConnectionPtr c1Right =
camSensorRotated1->Camera(1)->ConnectNewImageFrame(
std::bind(&::OnNewFrameTest, &imageCount1Right, img1Right,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_5));
event::ConnectionPtr c2Right =
camSensorRotated2->Camera(1)->ConnectNewImageFrame(
std::bind(&::OnNewFrameTest, &imageCount2Right, img2Right,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_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 (imageCount0Left < 10 || imageCountt < 10 ||
imageCount1Left < 10 || imageCount2Left < 10 ||
imageCount0Right < 10 ||
imageCount1Right < 10 || imageCount2Right < 10)
{
// wait at most 10 seconds sim time.
if (++waitCount >= 1000)
gzerr << "Err [" << imageCount0Left
<< "/10, " << imageCountt
<< "/10, " << imageCount1Left
<< "/10, " << imageCount2Left
<< "/10, " << imageCount0Right
<< "/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, imgt, 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 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 = -1.0;
// 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, imgt, width, height, depth,
diffMax, diffSum, diffAvg);
// use below to construct test for rotated2 left camera offset
// math::Quaternion a(1.2, 1.3, 1.4);
// gzerr << "test: " << a.RotateVector(math::Vector3(0, 1, 0)) << "\n";
// We expect that there will be some non-zero difference between the two
// images.
EXPECT_GT(diffSum, 100u);
// We expect that the average difference will be well within 3-sigma.
EXPECT_GT(fabs(diffAvg)/255., 0.0549);
}
// compare unrotated against rotated1
{
unsigned int diffMax = 0, diffSum = 0;
double diffAvg = 0.0;
// 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 unrotated against rotated1
{
unsigned int diffMax = 0, diffSum = 0;
double diffAvg = 0.0;
// 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
// 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 unrotated against rotated2
{
unsigned int diffMax = 0, diffSum = 0;
double diffAvg = 0.0;
// compare left images
this->ImageCompare(img0Left, img2Left, 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 unrotated against rotated2
{
unsigned int diffMax = 0, diffSum = 0;
double diffAvg = 0.0;
// 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.
EXPECT_EQ(diffSum, 0u);
// We expect that the average difference will be well within 3-sigma.
EXPECT_NEAR(diffAvg/255., 0.0, 1e-16);
}
// activate camera
camSensorUnrotated->SetActive(false);
camSensorTranslated->SetActive(false);
camSensorRotated1->SetActive(false);
camSensorRotated2->SetActive(false);
// disconnect callbacks
camSensorUnrotated->Camera(0)->DisconnectNewImageFrame(c0Left);
camSensorTranslated->Camera()->DisconnectNewImageFrame(ct);
camSensorRotated1->Camera(0)->DisconnectNewImageFrame(c1Left);
camSensorRotated2->Camera(0)->DisconnectNewImageFrame(c2Left);
camSensorUnrotated->Camera(1)->DisconnectNewImageFrame(c0Right);
camSensorRotated1->Camera(1)->DisconnectNewImageFrame(c1Right);
camSensorRotated2->Camera(1)->DisconnectNewImageFrame(c2Right);
}
// cleanup
delete[] img0Left;
delete[] imgt;
delete[] img1Left;
delete[] img2Left;
delete[] img0Right;
delete[] img1Right;
delete[] img2Right;
}
/////////////////////////////////////////////////
TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest)
{
// this test checks Camera::GetWorldRotation and other
// world pose functions.
// motivated by issue #1087
Load("worlds/camera_rotation_test.world");
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
// Make sure the render engine is available.
if (rendering::RenderEngine::Instance()->GetRenderPathType() ==
rendering::RenderEngine::NONE)
{
gzerr << "No rendering engine, unable to run camera test\n";
return;
}
// get two cameras, one with rotation and one without.
std::string modelUnrotated = "cam_x_rot_test_unrotated_cameras_1";
std::string multicameraUnrotated = "multicamera_sensor_unrotated";
std::string modelTranslated = "cam_x_rot_test_translated_camera_1";
std::string cameraTranslated = "camera_sensor_translated";
std::string modelRotated1 = "cam_x_rot_test_rotated_cameras_1";
std::string multicameraRotated1 = "multicamera_sensor_rotated1";
std::string modelRotated2 = "cam_x_rot_test_rotated_cameras_2";
std::string multicameraRotated2 = "multicamera_sensor_rotated2";
physics::ModelPtr model1 = world->GetModel(modelUnrotated);
sensors::SensorPtr sensor1 = sensors::get_sensor(multicameraUnrotated);
sensors::MultiCameraSensorPtr multicamera1 =
std::dynamic_pointer_cast<sensors::MultiCameraSensor>(sensor1);
physics::ModelPtr model2 = world->GetModel(modelTranslated);
sensors::SensorPtr sensor2 = sensors::get_sensor(cameraTranslated);
sensors::CameraSensorPtr camera2 =
std::dynamic_pointer_cast<sensors::CameraSensor>(sensor2);
physics::ModelPtr model3 = world->GetModel(modelRotated1);
sensors::SensorPtr sensor3 = sensors::get_sensor(multicameraRotated1);
sensors::MultiCameraSensorPtr multicamera3 =
std::dynamic_pointer_cast<sensors::MultiCameraSensor>(sensor3);
physics::ModelPtr model4 = world->GetModel(modelRotated2);
sensors::SensorPtr sensor4 = sensors::get_sensor(multicameraRotated2);
sensors::MultiCameraSensorPtr multicamera4 =
std::dynamic_pointer_cast<sensors::MultiCameraSensor>(sensor4);
ASSERT_TRUE(model1 != NULL);
ASSERT_TRUE(model2 != NULL);
ASSERT_TRUE(model3 != NULL);
ASSERT_TRUE(model4 != NULL);
ASSERT_TRUE(multicamera1 != NULL);
ASSERT_TRUE(camera2 != NULL);
ASSERT_TRUE(multicamera3 != NULL);
ASSERT_TRUE(multicamera4 != NULL);
// check poses in world frame
// each multicamera have 2 cameras
// model 1
// multicamera1 sensor has zero pose offset from the model
EXPECT_EQ(model1->GetWorldPose().Ign(),
multicamera1->Pose() + model1->GetWorldPose().Ign());
EXPECT_EQ(model1->GetWorldPose().Ign(),
sensor1->Pose() + model1->GetWorldPose().Ign());
// multicamera1 sensor's camera 0 has a pose offset from the sensor
EXPECT_NE(multicamera1->Pose() + model1->GetWorldPose().Ign(),
multicamera1->Camera(0)->WorldPose());
// Get multicamera1's local pose. There is current no GetPose() in Camera,
// so grab it from it's ogre scene node
Ogre::SceneNode *cameraNode = multicamera1->Camera(0)->SceneNode();
ignition::math::Pose3d cameraPose(
rendering::Conversions::Convert(cameraNode->getPosition()).Ign(),
rendering::Conversions::Convert(cameraNode->getOrientation()).Ign());
// Wait for the AttachToVisual request msg to be processed so that the camera
// is attached to the parent visual.
int sleep = 0;
int maxSleep = 100;
while (cameraPose == multicamera1->Camera(0)->WorldPose()
&& sleep < maxSleep)
{
common::Time::MSleep(100);
sleep++;
}
// verify multicamera sensor's camera world pose
EXPECT_EQ(cameraPose + multicamera1->Pose() + model1->GetWorldPose().Ign(),
multicamera1->Camera(0)->WorldPose());
EXPECT_EQ(model1->GetWorldPose().rot.Ign() * multicamera1->Pose().Rot()
* cameraPose.Rot(),
multicamera1->Camera(0)->WorldRotation());
// multicamera1 sensor's camera 1 has zero pose offset from the sensor
EXPECT_EQ(multicamera1->Pose() + model1->GetWorldPose().Ign(),
multicamera1->Camera(1)->WorldPose());
gzdbg << "model1 [" << model1->GetWorldPose() << "]\n"
<< "sensor1 ["
<< sensor1->Pose() + model1->GetWorldPose().Ign() << "]\n"
<< "multicamera1 ["
<< multicamera1->Pose() + model1->GetWorldPose().Ign()
<< "]\n"
<< "camera left WorldPose ["
<< multicamera1->Camera(0)->WorldPose() << "]\n"
<< "camera right WorldPose ["
<< multicamera1->Camera(1)->WorldPose() << "]\n";
// model 2
// camera2 sensor has zero pose offset from the model
EXPECT_EQ(model2->GetWorldPose().Ign(),
camera2->Pose() + model2->GetWorldPose().Ign());
EXPECT_EQ(model2->GetWorldPose().Ign(),
sensor2->Pose() + model2->GetWorldPose().Ign());
// camera2 sensor's camera has zero pose offset from the sensor
EXPECT_EQ(model2->GetWorldPose().Ign(), camera2->Camera()->WorldPose());
gzdbg << "model2 [" << model2->GetWorldPose() << "]\n"
<< "sensor2 ["
<< sensor2->Pose() + model2->GetWorldPose().Ign() << "]\n"
<< "camera2 ["
<< camera2->Pose() + model2->GetWorldPose().Ign() << "]\n"
<< "camera WorldPose [" << camera2->Camera()->WorldPose()
<< "]\n";
// model 3
// multicamera3 sensor has zero pose offset from the model
EXPECT_EQ(model3->GetWorldPose().Ign(),
multicamera3->Pose() + model3->GetWorldPose().Ign());
EXPECT_EQ(model3->GetWorldPose().Ign(),
sensor3->Pose() + model3->GetWorldPose().Ign());
// multicamera3 sensor's camera 0 has a pose offset from the sensor
EXPECT_NE(multicamera3->Pose() + model3->GetWorldPose().Ign(),
multicamera3->Camera(0)->WorldPose());
// Get multicamera3 sensor's camera 0 local pose
cameraNode = multicamera3->Camera(0)->SceneNode();
cameraPose = ignition::math::Pose3d(
rendering::Conversions::Convert(cameraNode->getPosition()).Ign(),
rendering::Conversions::Convert(cameraNode->getOrientation()).Ign());
// Wait for the AttachToVisual request msg to be processed so that the camera
// is attached to the parent visual.
sleep = 0;
maxSleep = 100;
while (cameraPose == multicamera3->Camera(0)->WorldPose()
&& sleep < maxSleep)
{
common::Time::MSleep(100);
sleep++;
}
// verify multicamera sensor's camera 0 world pose
EXPECT_EQ(cameraPose + multicamera3->Pose() +
model3->GetWorldPose().Ign(),
multicamera3->Camera(0)->WorldPose());
EXPECT_EQ(model3->GetWorldPose().rot.Ign() * multicamera3->Pose().Rot()
* cameraPose.Rot(),
multicamera3->Camera(0)->WorldRotation());
// multicamera3 sensor's camera 1 has zero pose offset from the sensor
EXPECT_EQ(multicamera3->Pose() + model3->GetWorldPose().Ign(),
multicamera3->Camera(1)->WorldPose());
EXPECT_EQ(model3->GetWorldPose().Ign(),
multicamera3->Camera(1)->WorldPose());
gzdbg << "model3 [" << model3->GetWorldPose() << "]\n"
<< "sensor3 ["
<< sensor3->Pose() + model3->GetWorldPose().Ign() << "]\n"
<< "multicamera3 ["
<< multicamera3->Pose() + model3->GetWorldPose().Ign()
<< "]\n"
<< "camera left WorldPose ["
<< multicamera3->Camera(0)->WorldPose() << "]\n"
<< "camera right WorldPose ["
<< multicamera3->Camera(1)->WorldPose() << "]\n";
// model 4
// multicamera4 sensor has zero pose offset from the model
EXPECT_EQ(model4->GetWorldPose().Ign(),
multicamera4->Pose() + model4->GetWorldPose().Ign());
EXPECT_EQ(model4->GetWorldPose().Ign(),
sensor4->Pose() + model4->GetWorldPose().Ign());
// multicamera4 sensor's camera 0 has a pose offset from the sensor
EXPECT_NE(model4->GetWorldPose().Ign(),
multicamera4->Camera(0)->WorldPose());
// Get multicamera4's camera 0 local pose
cameraNode = multicamera4->Camera(0)->SceneNode();
cameraPose = ignition::math::Pose3d(
rendering::Conversions::Convert(cameraNode->getPosition()).Ign(),
rendering::Conversions::Convert(cameraNode->getOrientation()).Ign());
// Wait for the AttachToVisual request msg to be processed so that the camera
// is attached to the parent visual.
sleep = 0;
maxSleep = 100;
while (cameraPose == multicamera4->Camera(0)->WorldPose()
&& sleep < maxSleep)
{
common::Time::MSleep(100);
sleep++;
}
// verify multicamera sensor's camera 0 world pose
EXPECT_EQ(cameraPose + multicamera4->Pose() + model4->GetWorldPose().Ign(),
multicamera4->Camera(0)->WorldPose());
EXPECT_EQ(model4->GetWorldPose().rot * multicamera4->Pose().Rot()
* cameraPose.Rot(), multicamera4->Camera(0)->WorldRotation());
// multicamera4 sensor's camera 1 has a pose offset from the sensor
EXPECT_NE(model4->GetWorldPose().Ign(),
multicamera4->Camera(1)->WorldPose());
// Get multicamera4 sensor's camera 1 local pose
cameraNode = multicamera4->Camera(1)->SceneNode();
cameraPose = ignition::math::Pose3d(
rendering::Conversions::Convert(cameraNode->getPosition()).Ign(),
rendering::Conversions::Convert(cameraNode->getOrientation()).Ign());
// Wait for the AttachToVisual request msg to be processed so that the camera
// is attached to the parent visual.
sleep = 0;
maxSleep = 100;
while (cameraPose == multicamera4->Camera(1)->WorldPose()
&& sleep < maxSleep)
{
common::Time::MSleep(100);
sleep++;
}
// verify multicamera4 sensor's camera 1 world pose
EXPECT_EQ(cameraPose + multicamera4->Pose() + model4->GetWorldPose().Ign(),
multicamera4->Camera(1)->WorldPose());
EXPECT_EQ(model4->GetWorldPose().rot.Ign() * multicamera4->Pose().Rot()
* cameraPose.Rot(), multicamera4->Camera(1)->WorldRotation());
gzdbg << "model4 [" << model4->GetWorldPose() << "]\n"
<< "sensor4 ["
<< sensor4->Pose() + model4->GetWorldPose().Ign() << "]\n"
<< "multicamera4 ["
<< multicamera4->Pose() + model4->GetWorldPose().Ign()
<< "]\n"
<< "camera1 WorldPose [" << multicamera4->Camera(0)->WorldPose()
<< "]\n"
<< "camera2 WorldPose [" << multicamera4->Camera(1)->WorldPose()
<< "]\n";
}
/////////////////////////////////////////////////
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
Reference in New Issue View Git Blame Copy Permalink