124 lines
3.8 KiB
C++
124 lines
3.8 KiB
C++
|
/*
|
||
|
|
* Copyright (C) 2017 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 <gtest/gtest.h>
|
||
|
|
#include <string>
|
||
|
|
#include <stdlib.h>
|
||
|
|
|
||
|
|
#include "gazebo/rendering/RenderingIface.hh"
|
||
|
|
#include "gazebo/test/ServerFixture.hh"
|
||
|
|
#include "gazebo/rendering/Distortion.hh"
|
||
|
|
#include "gazebo/rendering/Camera.hh"
|
||
|
|
|
||
|
|
#include "test/util.hh"
|
||
|
|
|
||
|
|
|
||
|
|
using namespace gazebo;
|
||
|
|
|
||
|
|
class Distortion_TEST : public RenderingFixture { };
|
||
|
|
|
||
|
|
sdf::ElementPtr CreateDistortionSDFElement(double _k1, double _k2,
|
||
|
|
double _k3, double _p1, double _p2, double _cx, double _cy)
|
||
|
|
{
|
||
|
|
std::ostringstream newModelStr;
|
||
|
|
|
||
|
|
newModelStr << "<sdf version='" << SDF_VERSION << "'>"
|
||
|
|
<< "<model name ='distorted_cam'>"
|
||
|
|
<< " <static>true</static>"
|
||
|
|
<< " <pose>0 0 0 0 0 0</pose>"
|
||
|
|
<< " <link name ='body'>"
|
||
|
|
<< " <sensor name ='distorted_cam' type ='camera'>"
|
||
|
|
<< " <always_on>1</always_on>"
|
||
|
|
<< " <update_rate>30</update_rate>"
|
||
|
|
<< " <visualize>true</visualize>"
|
||
|
|
<< " <camera>"
|
||
|
|
<< " <horizontal_fov>0.78539816339744828</horizontal_fov>"
|
||
|
|
<< " <image>"
|
||
|
|
<< " <width>100</width>"
|
||
|
|
<< " <height>100</height>"
|
||
|
|
<< " <format>R8G8B8</format>"
|
||
|
|
<< " </image>"
|
||
|
|
<< " <clip>"
|
||
|
|
<< " <near>0.1</near><far>100</far>"
|
||
|
|
<< " </clip>"
|
||
|
|
<< " <distortion>"
|
||
|
|
<< " <k1>" << _k1 << "</k1>"
|
||
|
|
<< " <k2>" << _k2 << "</k2>"
|
||
|
|
<< " <k3>" << _k3 << "</k3>"
|
||
|
|
<< " <p1>" << _p1 << "</p1>"
|
||
|
|
<< " <p2>" << _p2 << "</p2>"
|
||
|
|
<< " <center>" << _cx << " " << _cy << "</center>"
|
||
|
|
<< " </distortion>"
|
||
|
|
<< " </camera>"
|
||
|
|
<< " </sensor>"
|
||
|
|
<< " </link>"
|
||
|
|
<< "</model>"
|
||
|
|
<< "</sdf>";
|
||
|
|
|
||
|
|
sdf::SDFPtr cameraSDF(new sdf::SDF);
|
||
|
|
cameraSDF->SetFromString(newModelStr.str());
|
||
|
|
|
||
|
|
sdf::ElementPtr elem = cameraSDF->Root()
|
||
|
|
->GetElement("model")
|
||
|
|
->GetElement("link")
|
||
|
|
->GetElement("sensor")
|
||
|
|
->GetElement("camera")
|
||
|
|
->GetElement("distortion");
|
||
|
|
return elem;
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
TEST_F(Distortion_TEST, DistortionModelBasics)
|
||
|
|
{
|
||
|
|
Load("worlds/empty.world");
|
||
|
|
|
||
|
|
rendering::Distortion distortion;
|
||
|
|
|
||
|
|
// check the default crop behavior of the different distortion models
|
||
|
|
// undistorted
|
||
|
|
distortion.Load(CreateDistortionSDFElement(0, 0, 0, 0, 0, 0.5, 0.5));
|
||
|
|
EXPECT_EQ(distortion.Crop(), false);
|
||
|
|
|
||
|
|
// barrel distortion
|
||
|
|
distortion.Load(
|
||
|
|
CreateDistortionSDFElement(-0.1, -0.05, -0.01, 0, 0, 0.5, 0.5));
|
||
|
|
EXPECT_EQ(distortion.Crop(), true);
|
||
|
|
|
||
|
|
// pincushion distortion
|
||
|
|
distortion.Load(CreateDistortionSDFElement(0.1, 0.05, 0.01, 0, 0, 0.5, 0.5));
|
||
|
|
EXPECT_EQ(distortion.Crop(), false);
|
||
|
|
|
||
|
|
// getters and setters
|
||
|
|
distortion.SetCrop(false);
|
||
|
|
EXPECT_EQ(distortion.Crop(), false);
|
||
|
|
distortion.SetCrop(true);
|
||
|
|
EXPECT_EQ(distortion.Crop(), true);
|
||
|
|
EXPECT_DOUBLE_EQ(distortion.GetK1(), 0.1);
|
||
|
|
EXPECT_DOUBLE_EQ(distortion.GetK2(), 0.05);
|
||
|
|
EXPECT_DOUBLE_EQ(distortion.GetK3(), 0.01);
|
||
|
|
EXPECT_DOUBLE_EQ(distortion.GetP1(), 0.0);
|
||
|
|
EXPECT_DOUBLE_EQ(distortion.GetP2(), 0.0);
|
||
|
|
EXPECT_DOUBLE_EQ(distortion.GetCenter().Ign().X(), 0.5);
|
||
|
|
EXPECT_DOUBLE_EQ(distortion.GetCenter().Ign().Y(), 0.5);
|
||
|
|
}
|
||
|
|
|
||
|
|
int main(int argc, char **argv)
|
||
|
|
{
|
||
|
|
::testing::InitGoogleTest(&argc, argv);
|
||
|
|
return RUN_ALL_TESTS();
|
||
|
|
}
|