Skip to content

Commit

Permalink
Merge branch 'master' of https://github.com/Microsoft/AirSim
Browse files Browse the repository at this point in the history
  • Loading branch information
vsonims committed Sep 7, 2018
2 parents 581017f + 64bc4b3 commit cde0fa1
Show file tree
Hide file tree
Showing 46 changed files with 8,031 additions and 105 deletions.
2 changes: 2 additions & 0 deletions AirLib/AirLib.vcxproj
Expand Up @@ -37,6 +37,8 @@
<ClInclude Include="include\common\Common.hpp" />
<ClInclude Include="include\common\CommonStructs.hpp" />
<ClInclude Include="include\common\ClockFactory.hpp" />
<ClInclude Include="include\common\common_utils\bitmap_image.hpp" />
<ClInclude Include="include\common\common_utils\ColorUtils.hpp" />
<ClInclude Include="include\common\common_utils\ctpl_stl.h" />
<ClInclude Include="include\common\common_utils\EnumFlags.hpp" />
<ClInclude Include="include\common\common_utils\ExceptionUtils.hpp" />
Expand Down
6 changes: 6 additions & 0 deletions AirLib/AirLib.vcxproj.filters
Expand Up @@ -441,6 +441,12 @@
<ClInclude Include="include\common\common_utils\WindowsApisCommonPost.hpp">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="include\common\common_utils\bitmap_image.hpp">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="include\common\common_utils\ColorUtils.hpp">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup>
<ItemGroup>
<ClCompile Include="src\safety\ObstacleMap.cpp">
Expand Down
3 changes: 3 additions & 0 deletions AirLib/include/api/RpcLibClientBase.hpp
Expand Up @@ -81,6 +81,9 @@ class RpcLibClientBase {
msr::airlib::Pose simCharGetBonePose(const std::string& bone_name, const std::string& character_name = "") const;
void simCharResetBonePose(const std::string& bone_name, const std::string& character_name = "");
void simCharSetFacePreset(const std::string& preset_name, float value, const std::string& character_name = "");
void simSetFacePresets(const std::unordered_map<std::string, float>& presets, const std::string& character_name = "");
void simSetBonePoses(const std::unordered_map<std::string, msr::airlib::Pose>& poses, const std::string& character_name = "");
std::unordered_map<std::string, msr::airlib::Pose> simGetBonePoses(const std::vector<std::string>& bone_names, const std::string& character_name = "") const;

protected:
void* getClient();
Expand Down
4 changes: 3 additions & 1 deletion AirLib/include/api/WorldSimApiBase.hpp
Expand Up @@ -6,7 +6,6 @@

#include "common/CommonStructs.hpp"


namespace msr { namespace airlib {


Expand Down Expand Up @@ -42,6 +41,9 @@ class WorldSimApiBase {
virtual msr::airlib::Pose charGetBonePose(const std::string& bone_name, const std::string& character_name) const = 0;
virtual void charResetBonePose(const std::string& bone_name, const std::string& character_name) = 0;
virtual void charSetFacePreset(const std::string& preset_name, float value, const std::string& character_name) = 0;
virtual void charSetFacePresets(const std::unordered_map<std::string, float>& presets, const std::string& character_name) = 0;
virtual void charSetBonePoses(const std::unordered_map<std::string, msr::airlib::Pose>& poses, const std::string& character_name) = 0;
virtual std::unordered_map<std::string, msr::airlib::Pose> charGetBonePoses(const std::vector<std::string>& bone_names, const std::string& character_name) const = 0;

};

Expand Down
79 changes: 66 additions & 13 deletions AirLib/include/common/VectorMath.hpp
Expand Up @@ -183,32 +183,69 @@ class VectorMathT {
return q.conjugate()._transformVector(v);
}

static Vector3T transformToBodyFrame(const Vector3T& v_world, const QuaternionT& q, bool assume_unit_quat = true)
static QuaternionT rotateQuaternion(const QuaternionT& q, const QuaternionT& ref, bool assume_unit_quat)
{
QuaternionT ref_n = assume_unit_quat ? ref : ref.normalized();
QuaternionT ref_n_i = assume_unit_quat ? ref.conjugate() : ref.inverse();
return ref_n * q * ref_n_i;
}

static QuaternionT rotateQuaternionReverse(const QuaternionT& q, const QuaternionT& ref, bool assume_unit_quat)
{
QuaternionT ref_n = assume_unit_quat ? ref : ref.normalized();
QuaternionT ref_n_i = assume_unit_quat ? ref.conjugate() : ref.inverse();
return ref_n_i * q * ref_n;
}

static Vector3T transformToBodyFrame(const Vector3T& v_world, const QuaternionT& q_world, bool assume_unit_quat = true)
{
return rotateVectorReverse(v_world, q, assume_unit_quat);
return rotateVectorReverse(v_world, q_world, assume_unit_quat);
}

static Vector3T transformToBodyFrame(const Vector3T& v_world, const Pose& body_pose, bool assume_unit_quat = true)
static Vector3T transformToBodyFrame(const Vector3T& v_world, const Pose& body_world, bool assume_unit_quat = true)
{
//translate
Vector3T translated = v_world - body_pose.position;
Vector3T translated = v_world - body_world.position;
//rotate
return transformToBodyFrame(translated, body_pose.orientation, assume_unit_quat);
return transformToBodyFrame(translated, body_world.orientation, assume_unit_quat);
}

static Vector3T transformToWorldFrame(const Vector3T& v_body, const QuaternionT& q, bool assume_unit_quat = true)
static Pose transformToBodyFrame(const Pose& pose_world, const Pose& body_world, bool assume_unit_quat = true)
{
//translate
Vector3T translated = pose_world.position - body_world.position;
//rotate vector
Vector3T v_body = transformToBodyFrame(translated, body_world.orientation, assume_unit_quat);
//rotate orientation
QuaternionT q_body = rotateQuaternionReverse(pose_world.orientation, body_world.orientation, assume_unit_quat);

return Pose(v_body, q_body);
}

static Vector3T transformToWorldFrame(const Vector3T& v_body, const QuaternionT& q_world, bool assume_unit_quat = true)
{
return rotateVector(v_body, q, assume_unit_quat);
return rotateVector(v_body, q_world, assume_unit_quat);
}

static Vector3T transformToWorldFrame(const Vector3T& v_body, const Pose& body_pose, bool assume_unit_quat = true)
static Vector3T transformToWorldFrame(const Vector3T& v_body, const Pose& body_world, bool assume_unit_quat = true)
{
//rotate
Vector3T v_world = transformToWorldFrame(v_body, body_pose.orientation, assume_unit_quat);
Vector3T v_world = transformToWorldFrame(v_body, body_world.orientation, assume_unit_quat);
//translate
return v_world + body_pose.position;
return v_world + body_world.position;
}

//transform pose specified in body frame to world frame. The body frame in world coordinate is at body_world
static Pose transformToWorldFrame(const Pose& pose_body, const Pose& body_world, bool assume_unit_quat = true)
{
//rotate position
Vector3T v_world = transformToWorldFrame(pose_body.position, body_world.orientation, assume_unit_quat);
//rotate orientation
QuaternionT q_world = rotateQuaternion(pose_body.orientation, body_world.orientation, assume_unit_quat);
//translate
return Pose(v_world + body_world.position, q_world);
}

static QuaternionT negate(const QuaternionT& q)
{
//from Gazebo implementation
Expand Down Expand Up @@ -453,9 +490,9 @@ class VectorMathT {
static QuaternionT toQuaternion(const Vector3T& axis, RealT angle)
{
//Alternative:
//auto s = std::sinf(angle / 2);
//auto s = std::sin(angle / 2);
//auto u = axis.normalized();
//return Quaternionr(std::cosf(angle / 2), u.x() * s, u.y() * s, u.z() * s);
//return Quaternionr(std::cos(angle / 2), u.x() * s, u.y() * s, u.z() * s);

return QuaternionT(Eigen::AngleAxis<RealT>(angle, axis));
}
Expand All @@ -475,6 +512,8 @@ class VectorMathT {
//spherical lerp
static QuaternionT slerp(const QuaternionT& from, const QuaternionT& to, RealT alpha)
{
/*
//below is manual way to do this
RealT n_alpha = 1 - alpha;
RealT theta = acos(from.x()*to.x() + from.y()*to.y() + from.z()*to.z() + from.w()*to.w());
//Check for theta > 0 to avoid division by 0.
Expand All @@ -495,7 +534,9 @@ class VectorMathT {
else {
return to.normalized();
}
*/

return from.slerp(alpha, to);
}

static Vector3T lerp(const Vector3T& from, const Vector3T& to, RealT alpha)
Expand Down Expand Up @@ -543,8 +584,11 @@ class VectorMathT {
return lerp(from, to, alpha).normalized();
}

static QuaternionT lookAt(Vector3T sourcePoint, Vector3T destPoint)
//assuming you are looking at front() vector, what rotation you need to look at destPoint?
static QuaternionT lookAt(const Vector3T& sourcePoint, const Vector3T& destPoint)
{
/*
//below is manual way to do this without Eigen
Vector3T toVector = (destPoint - sourcePoint);
toVector.normalize(); //this is important!
Expand All @@ -558,8 +602,17 @@ class VectorMathT {
axis = axis.normalized();
return VectorMathT::toQuaternion(axis, ang);
*/

return QuaternionT::FromTwoVectors(VectorMathT::front(), destPoint - sourcePoint);
}

//what rotation we need to rotate "" vector to "to" vector (rotation is around intersection of two vectors)
static QuaternionT toQuaternion(const Vector3T& from, const Vector3T& to)
{
return QuaternionT::FromTwoVectors(from, to);
}

static const Vector3T front()
{
static Vector3T v(1, 0, 0);
Expand Down
110 changes: 110 additions & 0 deletions AirLib/include/common/common_utils/ColorUtils.hpp
@@ -0,0 +1,110 @@
#ifndef common_utils_OnlineStats_hpp
#define common_utils_OnlineStats_hpp

namespace common_utils {

class ColorUtils {
public:

static void valToRGB(double val0To1, unsigned char& r, unsigned char& g, unsigned char& b)
{//actual visible spectrum is 375 to 725 but outside of 400-700 things become too black
wavelengthToRGB(val0To1 * (700 - 400) + 400, r, g, b);
}

/**
* Convert a wavelength in the visible light spectrum to a RGB color value that is suitable to be displayed on a
* monitor
*
* @param wavelength wavelength in nm
* @return RGB color encoded in int. each color is represented with 8 bits and has a layout of
* 00000000RRRRRRRRGGGGGGGGBBBBBBBB where MSB is at the leftmost
*/
static void wavelengthToRGB(double wavelength, unsigned char& r, unsigned char& g, unsigned char& b) {
double x, y, z;
cie1931WavelengthToXYZFit(wavelength, x, y, z);
double dr, dg, db;
srgbXYZ2RGB(x, y, z, dr, dg, db);

r = static_cast<unsigned char>(static_cast<int>(dr * 0xFF) & 0xFF);
g = static_cast<unsigned char>(static_cast<int>(dg * 0xFF) & 0xFF);
b = static_cast<unsigned char>(static_cast<int>(db * 0xFF) & 0xFF);
}

/**
* Convert XYZ to RGB in the sRGB color space
* <p>
* The conversion matrix and color component transfer function is taken from http://www.color.org/srgb.pdf, which
* follows the International Electrotechnical Commission standard IEC 61966-2-1 "Multimedia systems and equipment -
* Colour measurement and management - Part 2-1: Colour management - Default RGB colour space - sRGB"
*
* @param xyz XYZ values in a double array in the order of X, Y, Z. each value in the range of [0.0, 1.0]
* @return RGB values in a double array, in the order of R, G, B. each value in the range of [0.0, 1.0]
*/
static void srgbXYZ2RGB(double x, double y, double z, double& r, double& g, double& b) {
double rl = 3.2406255 * x + -1.537208 * y + -0.4986286 * z;
double gl = -0.9689307 * x + 1.8757561 * y + 0.0415175 * z;
double bl = 0.0557101 * x + -0.2040211 * y + 1.0569959 * z;

r = srgbXYZ2RGBPostprocess(rl);
g = srgbXYZ2RGBPostprocess(gl);
b = srgbXYZ2RGBPostprocess(bl);
}

/**
* helper function for {@link #srgbXYZ2RGB(double[])}
*/
static double srgbXYZ2RGBPostprocess(double c) {
// clip if c is out of range
c = c > 1 ? 1 : (c < 0 ? 0 : c);

// apply the color component transfer function
c = c <= 0.0031308 ? c * 12.92 : 1.055 * std::pow(c, 1. / 2.4) - 0.055;

return c;
}

/**
* A multi-lobe, piecewise Gaussian fit of CIE 1931 XYZ Color Matching Functions by Wyman el al. from Nvidia. The
* code here is adopted from the Listing 1 of the paper authored by Wyman et al.
* <p>
* Reference: Chris Wyman, Peter-Pike Sloan, and Peter Shirley, Simple Analytic Approximations to the CIE XYZ Color
* Matching Functions, Journal of Computer Graphics Techniques (JCGT), vol. 2, no. 2, 1-11, 2013.
*
* @param wavelength wavelength in nm
* @return XYZ in a double array in the order of X, Y, Z. each value in the range of [0.0, 1.0]
*/
static void cie1931WavelengthToXYZFit(double wavelength, double& x, double& y, double& z) {
double wave = wavelength;

{
double t1 = (wave - 442.0) * ((wave < 442.0) ? 0.0624 : 0.0374);
double t2 = (wave - 599.8) * ((wave < 599.8) ? 0.0264 : 0.0323);
double t3 = (wave - 501.1) * ((wave < 501.1) ? 0.0490 : 0.0382);

x = 0.362 * std::exp(-0.5 * t1 * t1)
+ 1.056 * std::exp(-0.5 * t2 * t2)
- 0.065 * std::exp(-0.5 * t3 * t3);
}

{
double t1 = (wave - 568.8) * ((wave < 568.8) ? 0.0213 : 0.0247);
double t2 = (wave - 530.9) * ((wave < 530.9) ? 0.0613 : 0.0322);

y = 0.821 * std::exp(-0.5 * t1 * t1)
+ 0.286 * std::exp(-0.5 * t2 * t2);
}

{
double t1 = (wave - 437.0) * ((wave < 437.0) ? 0.0845 : 0.0278);
double t2 = (wave - 459.0) * ((wave < 459.0) ? 0.0385 : 0.0725);

z = 1.217 * std::exp(-0.5 * t1 * t1)
+ 0.681 * std::exp(-0.5 * t2 * t2);
}
}

};

} //namespace

#endif
1 change: 1 addition & 0 deletions AirLib/include/common/common_utils/RandomGenerator.hpp
Expand Up @@ -43,6 +43,7 @@ class RandomGenerator {
typedef RandomGenerator<double, std::uniform_real_distribution<double>> RandomGeneratorD;
typedef RandomGenerator<float, std::uniform_real_distribution<float>> RandomGeneratorF;
typedef RandomGenerator<int, std::uniform_int_distribution<int>> RandomGeneratorI;
typedef RandomGenerator<unsigned int, std::uniform_int_distribution<unsigned int>> RandomGeneratorUI;
//TODO: below we should have float instead of double but VC++2017 has a bug :(
typedef RandomGenerator<float, std::normal_distribution<double>> RandomGeneratorGaussianF;
typedef RandomGenerator<double, std::normal_distribution<double>> RandomGeneratorGaussianD;
Expand Down
5 changes: 4 additions & 1 deletion AirLib/include/common/common_utils/Utils.hpp
Expand Up @@ -28,8 +28,11 @@
#include <limits.h> // needed for CHAR_BIT used below
#endif

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#include <cmath>
#endif
#include <math.h>
//#include <cmath>

#ifndef M_PIf
#define M_PIf static_cast<float>(3.1415926535897932384626433832795028841972)
Expand Down

0 comments on commit cde0fa1

Please sign in to comment.