Skip to content

Commit 538856f

Browse files
iche033iche033
committed
Merge branch 'wide_angle_camera_12' of github.com:ignitionrobotics/sdformat into wide_angle_camera_12
2 parents 8aacb98 + ef6fe84 commit 538856f

27 files changed

+1333
-1
lines changed

‎include/sdf/AirPressure.hh

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,11 @@ namespace sdf
8484
/// \returen True if 'this' != _mag.
8585
public: bool operator!=(const AirPressure &_air) const;
8686

87+
/// \brief Fill the provided _elem with data from this sensor type.
88+
/// \param[out] _elem SDF element point to populate
89+
/// \return True if successful.
90+
public: bool PopulateElement(sdf::ElementPtr _elem) const;
91+
8792
/// \brief Private data pointer.
8893
IGN_UTILS_IMPL_PTR(dataPtr)
8994
};

‎include/sdf/Altimeter.hh

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,11 @@ namespace sdf
7676
/// \returen True if 'this' != _alt.
7777
public: bool operator!=(const Altimeter &_alt) const;
7878

79+
/// \brief Fill the provided _elem with data from this sensor type.
80+
/// \param[out] _elem SDF element point to populate
81+
/// \return True if successful.
82+
public: bool PopulateElement(sdf::ElementPtr _elem) const;
83+
7984
/// \brief Private data pointer.
8085
IGN_UTILS_IMPL_PTR(dataPtr)
8186
};

‎include/sdf/Camera.hh

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -468,6 +468,15 @@ namespace sdf
468468
/// \param[in] _mask visibility mask
469469
public: void SetVisibilityMask(uint32_t _mask);
470470

471+
/// \brief Get whether or not the camera has instrinsics values set
472+
/// \return True if the camera has instrinsics values set, false otherwise
473+
public: bool HasLensIntrinsics() const;
474+
475+
/// \brief Fill the provided _elem with data from this sensor type.
476+
/// \param[out] _elem SDF element point to populate
477+
/// \return True if successful.
478+
public: bool PopulateElement(sdf::ElementPtr _elem) const;
479+
471480
/// \brief Private data pointer.
472481
IGN_UTILS_IMPL_PTR(dataPtr)
473482
};

‎include/sdf/ForceTorque.hh

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -154,6 +154,11 @@ namespace sdf
154154
/// \returen True if 'this' != _ft.
155155
public: bool operator!=(const ForceTorque &_ft) const;
156156

157+
/// \brief Fill the provided _elem with data from this sensor type.
158+
/// \param[out] _elem SDF element point to populate
159+
/// \return True if successful.
160+
public: bool PopulateElement(sdf::ElementPtr _elem) const;
161+
157162
/// \brief Private data pointer.
158163
IGN_UTILS_IMPL_PTR(dataPtr)
159164
};

‎include/sdf/Imu.hh

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -253,6 +253,11 @@ namespace sdf
253253
/// \returen True if 'this' != _imu.
254254
public: bool operator!=(const Imu &_imu) const;
255255

256+
/// \brief Fill the provided _elem with data from this sensor type.
257+
/// \param[out] _elem SDF element point to populate
258+
/// \return True if successful.
259+
public: bool PopulateElement(sdf::ElementPtr _elem) const;
260+
256261
/// \brief Private data pointer.
257262
IGN_UTILS_IMPL_PTR(dataPtr)
258263
};

‎include/sdf/Lidar.hh

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -232,6 +232,11 @@ namespace sdf
232232
/// \return True if 'this' != _lidar.
233233
public: bool operator!=(const Lidar &_lidar) const;
234234

235+
/// \brief Fill the provided _elem with data from this sensor type.
236+
/// \param[out] _elem SDF element point to populate
237+
/// \return True if successful.
238+
public: bool PopulateElement(sdf::ElementPtr _elem) const;
239+
235240
/// \brief Private data pointer.
236241
IGN_UTILS_IMPL_PTR(dataPtr)
237242
};

‎include/sdf/Light.hh

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -268,6 +268,11 @@ namespace sdf
268268
private: void SetPoseRelativeToGraph(
269269
sdf::ScopedGraph<PoseRelativeToGraph> _graph);
270270

271+
/// \brief Create and return an SDF element filled with data from this
272+
/// light object.
273+
/// \return SDF element pointer with updated light values.
274+
public: sdf::ElementPtr ToElement() const;
275+
271276
/// \brief Allow Link::SetPoseRelativeToGraph or World::Load to call
272277
/// SetXmlParentName and SetPoseRelativeToGraph,
273278
/// but Link::SetPoseRelativeToGraph is a private function, so we need

‎include/sdf/Magnetometer.hh

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -85,6 +85,11 @@ namespace sdf
8585
/// \returen True if 'this' != _mag.
8686
public: bool operator!=(const Magnetometer &_mag) const;
8787

88+
/// \brief Fill the provided _elem with data from this sensor type.
89+
/// \param[out] _elem SDF element point to populate
90+
/// \return True if successful.
91+
public: bool PopulateElement(sdf::ElementPtr _elem) const;
92+
8893
/// \brief Private data pointer.
8994
IGN_UTILS_IMPL_PTR(dataPtr)
9095
};

‎include/sdf/Noise.hh

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -162,6 +162,14 @@ namespace sdf
162162
/// not been called.
163163
public: sdf::ElementPtr Element() const;
164164

165+
/// \brief Fill the provided _elem with data from this sensor type.
166+
/// \param[out] _elem SDF element point to populate
167+
/// \param[in] _simpleNoise True if the element should be populated with
168+
/// just type, mean, and standard deviation.
169+
/// \return True if successful.
170+
public: bool PopulateElement(sdf::ElementPtr _elem,
171+
bool _simpleNoise = false) const;
172+
165173
/// \brief Private data pointer.
166174
IGN_UTILS_IMPL_PTR(dataPtr)
167175
};

‎include/sdf/Sensor.hh

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -206,6 +206,11 @@ namespace sdf
206206
/// not been called.
207207
public: sdf::ElementPtr Element() const;
208208

209+
/// \brief Fill the provided _elem with data from this sensor.
210+
/// \param[out] _elem SDF element point to populate
211+
/// \return True if successful.
212+
public: bool PopulateElement(sdf::ElementPtr _elem) const;
213+
209214
/// \brief Get the sensor type.
210215
/// \return The sensor type.
211216
public: SensorType Type() const;

‎src/AirPressure.cc

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -113,3 +113,13 @@ void AirPressure::SetPressureNoise(const Noise &_noise)
113113
{
114114
this->dataPtr->noise = _noise;
115115
}
116+
117+
/////////////////////////////////////////////////
118+
bool AirPressure::PopulateElement(sdf::ElementPtr _elem) const
119+
{
120+
_elem->GetElement("reference_altitude")->Set<double>(
121+
this->ReferenceAltitude());
122+
sdf::ElementPtr pressureElem = _elem->GetElement("pressure");
123+
sdf::ElementPtr noiseElem = pressureElem->GetElement("noise");
124+
return this->dataPtr->noise.PopulateElement(noiseElem);
125+
}

‎src/Altimeter.cc

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -127,3 +127,18 @@ bool Altimeter::operator==(const Altimeter &_alt) const
127127

128128
return true;
129129
}
130+
131+
/////////////////////////////////////////////////
132+
bool Altimeter::PopulateElement(sdf::ElementPtr _elem) const
133+
{
134+
sdf::ElementPtr verticalPosElem = _elem->GetElement("vertical_position");
135+
sdf::ElementPtr verticalPosNoiseElem = verticalPosElem->GetElement("noise");
136+
137+
sdf::ElementPtr verticalVelElem = _elem->GetElement("vertical_velocity");
138+
sdf::ElementPtr verticalVelNoiseElem = verticalVelElem->GetElement("noise");
139+
140+
return
141+
this->dataPtr->verticalPositionNoise.PopulateElement(verticalPosNoiseElem)
142+
&&
143+
this->dataPtr->verticalVelocityNoise.PopulateElement(verticalVelNoiseElem);
144+
}

‎src/Camera.cc

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -174,6 +174,9 @@ class sdf::Camera::Implementation
174174
/// \brief lens instrinsics s.
175175
public: double lensIntrinsicsS{1.0};
176176

177+
/// \brief True if this camera has custom intrinsics values
178+
public: bool hasIntrinsics = false;
179+
177180
/// \brief Visibility mask of a camera. Defaults to 0xFFFFFFFF
178181
public: uint32_t visibilityMask{4294967295u};
179182
};
@@ -367,6 +370,7 @@ Errors Camera::Load(ElementPtr _sdf)
367370
this->dataPtr->lensIntrinsicsCy).first;
368371
this->dataPtr->lensIntrinsicsS = intrinsics->Get<double>("s",
369372
this->dataPtr->lensIntrinsicsS).first;
373+
this->dataPtr->hasIntrinsics = true;
370374
}
371375
}
372376

@@ -865,6 +869,7 @@ double Camera::LensIntrinsicsFx() const
865869
void Camera::SetLensIntrinsicsFx(double _fx)
866870
{
867871
this->dataPtr->lensIntrinsicsFx = _fx;
872+
this->dataPtr->hasIntrinsics = true;
868873
}
869874

870875
/////////////////////////////////////////////////
@@ -877,6 +882,7 @@ double Camera::LensIntrinsicsFy() const
877882
void Camera::SetLensIntrinsicsFy(double _fy)
878883
{
879884
this->dataPtr->lensIntrinsicsFy = _fy;
885+
this->dataPtr->hasIntrinsics = true;
880886
}
881887

882888
/////////////////////////////////////////////////
@@ -889,6 +895,7 @@ double Camera::LensIntrinsicsCx() const
889895
void Camera::SetLensIntrinsicsCx(double _cx)
890896
{
891897
this->dataPtr->lensIntrinsicsCx = _cx;
898+
this->dataPtr->hasIntrinsics = true;
892899
}
893900

894901
/////////////////////////////////////////////////
@@ -901,6 +908,7 @@ double Camera::LensIntrinsicsCy() const
901908
void Camera::SetLensIntrinsicsCy(double _cy)
902909
{
903910
this->dataPtr->lensIntrinsicsCy = _cy;
911+
this->dataPtr->hasIntrinsics = true;
904912
}
905913

906914
/////////////////////////////////////////////////
@@ -913,6 +921,7 @@ double Camera::LensIntrinsicsSkew() const
913921
void Camera::SetLensIntrinsicsSkew(double _s)
914922
{
915923
this->dataPtr->lensIntrinsicsS = _s;
924+
this->dataPtr->hasIntrinsics = true;
916925
}
917926

918927
/////////////////////////////////////////////////
@@ -968,3 +977,85 @@ void Camera::SetVisibilityMask(uint32_t _mask)
968977
{
969978
this->dataPtr->visibilityMask = _mask;
970979
}
980+
981+
/////////////////////////////////////////////////
982+
bool Camera::HasLensIntrinsics() const
983+
{
984+
return this->dataPtr->hasIntrinsics;
985+
}
986+
987+
/////////////////////////////////////////////////
988+
bool Camera::PopulateElement(sdf::ElementPtr _elem) const
989+
{
990+
_elem->GetAttribute("name")->Set<std::string>(this->Name());
991+
_elem->GetElement("pose")->Set<ignition::math::Pose3d>(this->RawPose());
992+
_elem->GetElement("horizontal_fov")->Set<double>(
993+
this->HorizontalFov().Radian());
994+
sdf::ElementPtr imageElem = _elem->GetElement("image");
995+
imageElem->GetElement("width")->Set<double>(this->ImageWidth());
996+
imageElem->GetElement("height")->Set<double>(this->ImageHeight());
997+
imageElem->GetElement("format")->Set<std::string>(this->PixelFormatStr());
998+
sdf::ElementPtr clipElem = _elem->GetElement("clip");
999+
clipElem->GetElement("near")->Set<double>(this->NearClip());
1000+
clipElem->GetElement("far")->Set<double>(this->FarClip());
1001+
1002+
if (this->dataPtr->hasDepthCamera)
1003+
{
1004+
sdf::ElementPtr depthElem = _elem->GetElement("depth_camera");
1005+
sdf::ElementPtr depthClipElem = depthElem->GetElement("clip");
1006+
depthClipElem->GetElement("near")->Set<double>(this->DepthNearClip());
1007+
depthClipElem->GetElement("far")->Set<double>(this->DepthFarClip());
1008+
}
1009+
1010+
sdf::ElementPtr saveElem = _elem->GetElement("save");
1011+
saveElem->GetAttribute("enabled")->Set<bool>(this->SaveFrames());
1012+
if (this->SaveFrames())
1013+
saveElem->GetElement("path")->Set<std::string>(this->SaveFramesPath());
1014+
1015+
sdf::ElementPtr noiseElem = _elem->GetElement("noise");
1016+
this->dataPtr->imageNoise.PopulateElement(noiseElem, true);
1017+
1018+
sdf::ElementPtr distortionElem = _elem->GetElement("distortion");
1019+
distortionElem->GetElement("k1")->Set<double>(this->DistortionK1());
1020+
distortionElem->GetElement("k2")->Set<double>(this->DistortionK2());
1021+
distortionElem->GetElement("k3")->Set<double>(this->DistortionK3());
1022+
distortionElem->GetElement("p1")->Set<double>(this->DistortionP1());
1023+
distortionElem->GetElement("p2")->Set<double>(this->DistortionP2());
1024+
distortionElem->GetElement("center")->Set<ignition::math::Vector2d>(
1025+
this->DistortionCenter());
1026+
1027+
sdf::ElementPtr lensElem = _elem->GetElement("lens");
1028+
lensElem->GetElement("type")->Set<std::string>(this->LensType());
1029+
lensElem->GetElement("scale_to_hfov")->Set<bool>(this->LensScaleToHfov());
1030+
lensElem->GetElement("cutoff_angle")->Set<double>(
1031+
this->LensCutoffAngle().Radian());
1032+
lensElem->GetElement("env_texture_size")->Set<double>(
1033+
this->LensEnvironmentTextureSize());
1034+
if (this->LensType() == "custom")
1035+
{
1036+
sdf::ElementPtr customLensElem = lensElem->GetElement("custom_function");
1037+
customLensElem->GetElement("c1")->Set<double>(this->LensC1());
1038+
customLensElem->GetElement("c2")->Set<double>(this->LensC2());
1039+
customLensElem->GetElement("c3")->Set<double>(this->LensC3());
1040+
customLensElem->GetElement("f")->Set<double>(this->LensFocalLength());
1041+
customLensElem->GetElement("fun")->Set<std::string>(
1042+
this->LensFunction());
1043+
}
1044+
if (this->HasLensIntrinsics())
1045+
{
1046+
sdf::ElementPtr intrinsicsElem = lensElem->GetElement("intrinsics");
1047+
intrinsicsElem->GetElement("fx")->Set<double>(this->LensIntrinsicsFx());
1048+
intrinsicsElem->GetElement("fy")->Set<double>(this->LensIntrinsicsFy());
1049+
intrinsicsElem->GetElement("cx")->Set<double>(this->LensIntrinsicsCx());
1050+
intrinsicsElem->GetElement("cy")->Set<double>(this->LensIntrinsicsCy());
1051+
intrinsicsElem->GetElement("s")->Set<double>(this->LensIntrinsicsSkew());
1052+
}
1053+
1054+
if (this->HasSegmentationType())
1055+
{
1056+
_elem->GetElement("segmentation_type")->Set<std::string>(
1057+
this->SegmentationType());
1058+
}
1059+
1060+
return true;
1061+
}

‎src/Camera_TEST.cc

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -174,6 +174,8 @@ TEST(DOMCamera, Construction)
174174
cam.SetLensIntrinsicsSkew(2.3);
175175
EXPECT_DOUBLE_EQ(2.3, cam.LensIntrinsicsSkew());
176176

177+
EXPECT_TRUE(cam.HasLensIntrinsics());
178+
177179
EXPECT_EQ(4294967295u, cam.VisibilityMask());
178180
cam.SetVisibilityMask(123u);
179181
EXPECT_EQ(123u, cam.VisibilityMask());

‎src/ForceTorque.cc

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -277,3 +277,79 @@ void ForceTorque::SetMeasureDirection(
277277
{
278278
this->dataPtr->measure_direction = _direction;
279279
}
280+
281+
/////////////////////////////////////////////////
282+
bool ForceTorque::PopulateElement(sdf::ElementPtr _elem) const
283+
{
284+
bool result = true;
285+
std::string frame;
286+
switch (this->Frame())
287+
{
288+
case sdf::ForceTorqueFrame::PARENT:
289+
frame = "parent";
290+
break;
291+
case sdf::ForceTorqueFrame::CHILD:
292+
frame = "child";
293+
break;
294+
case sdf::ForceTorqueFrame::SENSOR:
295+
frame = "sensor";
296+
break;
297+
case sdf::ForceTorqueFrame::INVALID:
298+
default:
299+
break;
300+
}
301+
if (!frame.empty())
302+
_elem->GetElement("frame")->Set<std::string>(frame);
303+
304+
std::string measureDirection;
305+
switch (this->MeasureDirection())
306+
{
307+
case sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD:
308+
measureDirection = "parent_to_child";
309+
break;
310+
case sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT:
311+
measureDirection = "child_to_parent";
312+
break;
313+
case sdf::ForceTorqueMeasureDirection::INVALID:
314+
default:
315+
break;
316+
}
317+
if (!measureDirection.empty())
318+
{
319+
_elem->GetElement("measure_direction")->Set<std::string>(measureDirection);
320+
}
321+
322+
sdf::ElementPtr forceElem = _elem->GetElement("force");
323+
sdf::ElementPtr forceXElem = forceElem->GetElement("x");
324+
sdf::ElementPtr forceXNoiseElem = forceXElem->GetElement("noise");
325+
result = result &&
326+
this->dataPtr->forceXNoise.PopulateElement(forceXNoiseElem);
327+
328+
sdf::ElementPtr forceYElem = forceElem->GetElement("y");
329+
sdf::ElementPtr forceYNoiseElem = forceYElem->GetElement("noise");
330+
result = result &&
331+
this->dataPtr->forceYNoise.PopulateElement(forceYNoiseElem);
332+
333+
sdf::ElementPtr forceZElem = forceElem->GetElement("z");
334+
sdf::ElementPtr forceZNoiseElem = forceZElem->GetElement("noise");
335+
result = result &&
336+
this->dataPtr->forceZNoise.PopulateElement(forceZNoiseElem);
337+
338+
sdf::ElementPtr torqueElem = _elem->GetElement("torque");
339+
sdf::ElementPtr torqueXElem = torqueElem->GetElement("x");
340+
sdf::ElementPtr torqueXNoiseElem = torqueXElem->GetElement("noise");
341+
result = result &&
342+
this->dataPtr->torqueXNoise.PopulateElement(torqueXNoiseElem);
343+
344+
sdf::ElementPtr torqueYElem = torqueElem->GetElement("y");
345+
sdf::ElementPtr torqueYNoiseElem = torqueYElem->GetElement("noise");
346+
result = result &&
347+
this->dataPtr->torqueYNoise.PopulateElement(torqueYNoiseElem);
348+
349+
sdf::ElementPtr torqueZElem = torqueElem->GetElement("z");
350+
sdf::ElementPtr torqueZNoiseElem = torqueZElem->GetElement("noise");
351+
result = result &&
352+
this->dataPtr->torqueZNoise.PopulateElement(torqueZNoiseElem);
353+
354+
return result;
355+
}

0 commit comments

Comments
 (0)