cv::Mat getZYZRotationMat(double yaw1, double pitch,
double yaw2, bool rad)
{
cv::Mat rotYaw1 = cv::Mat::eye(3,3,CV_64F);
cv::Mat rotPitch = cv::Mat::eye(3,3,CV_64F);
cv::Mat rotYaw2 = cv::Mat::eye(3,3,CV_64F);
if (!rad)
{
yaw1 = deg2Rad(yaw1);
pitch = deg2Rad(pitch);
yaw2 = deg2Rad(yaw2);
}
rotPitch.at<double>(0,0) = cos( pitch ); //pitch y
rotPitch.at<double>(0,2) = sin( pitch );
rotPitch.at<double>(2,0) = -sin( pitch );
rotPitch.at<double>(2,2) = cos( pitch );
rotYaw1.at<double>(0,0) = cos( yaw1 ); //yaw1 z
rotYaw1.at<double>(0,1) = -sin( yaw1 );
rotYaw1.at<double>(1,0) = sin( yaw1 );
rotYaw1.at<double>(1,1) = cos( yaw1 );
rotYaw2.at<double>(0,0) = cos( yaw2 ); //yaw2 z
rotYaw2.at<double>(0,1) = -sin( yaw2 );
rotYaw2.at<double>(1,0) = sin( yaw2 );
rotYaw2.at<double>(1,1) = cos( yaw2 );
return rotYaw1 * rotPitch * rotYaw2;
}
void
operator <<= (SensorPositionIDL& position, const QDomNode& node)
{
if (!node.isNull()) {
QDomNode n1 = node.firstChild();
while(!n1.isNull() ) {
QDomNode n2 = n1.firstChild();
if (!n2.isNull()) {
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really a text element.
if (n1.nodeName() == "height") {
position.height = t.data().toInt();
}
else if (n1.nodeName() == "distance") {
position.distance = t.data().toInt();
}
else if (n1.nodeName() == "alpha") {
position.alpha = deg2Rad(t.data().toDouble());
}
else if (n1.nodeName() == "beta") {
position.beta = deg2Rad(t.data().toDouble());
}
else if (n1.nodeName() == "gamma") {
position.gamma = deg2Rad(t.data().toDouble());
}
else if (n1.nodeName() == "masked") {
position.masked = (t.data() == "true");
}
}
}
n1 = n1.nextSibling();
}
}
}
cv::Mat getRotationMat(float roll, float pitch,
float yaw, bool rad)
{
cv::Mat rotYaw = cv::Mat::eye(3, 3, CV_32F);
cv::Mat rotPitch = cv::Mat::eye(3, 3, CV_32F);
cv::Mat rotRoll = cv::Mat::eye(3, 3, CV_32F);
if (!rad)
{
yaw = deg2Rad(yaw);
pitch = deg2Rad(pitch);
roll = deg2Rad(roll);
}
rotPitch.at<float>(0,0) = cos( pitch ); //pitch
rotPitch.at<float>(0,2) = sin( pitch );
rotPitch.at<float>(2,0) = -sin( pitch );
rotPitch.at<float>(2,2) = cos( pitch );
rotYaw.at<float>(0,0) = cos( yaw ); //yaw
rotYaw.at<float>(0,1) = -sin( yaw );
rotYaw.at<float>(1,0) = sin( yaw );
rotYaw.at<float>(1,1) = cos( yaw );
rotRoll.at<float>(1,1) = cos( roll ); //roll
rotRoll.at<float>(1,2) = -sin( roll );
rotRoll.at<float>(2,1) = sin( roll );
rotRoll.at<float>(2,2) = cos( roll );
return rotYaw * rotPitch * rotRoll;
}
void
operator <<= (Miro::SensorGroupIDL& group, const QDomNode& node)
{
if (!node.isNull()) {
// set sensor defaults
Miro::SensorPositionIDL sensor;
sensor.masked = false;
QDomNamedNodeMap map = node.attributes();
QDomNode n;
n = map.namedItem("height");
if (!n.isNull()) {
QDomAttr attr = n.toAttr();
if (!attr.isNull())
sensor.height = attr.value().toInt();
}
n = map.namedItem("distance");
if (!n.isNull()) {
QDomAttr attr = n.toAttr();
if (!attr.isNull())
sensor.distance =attr.value().toInt();
}
n = map.namedItem("alpha");
if (!n.isNull()) {
QDomAttr attr = n.toAttr();
if (!attr.isNull())
sensor.alpha = deg2Rad(attr.value().toDouble());
}
n = map.namedItem("beta");
if (!n.isNull()) {
QDomAttr attr = n.toAttr();
if (!attr.isNull())
sensor.beta = deg2Rad(attr.value().toDouble());
}
n = map.namedItem("gamma");
if (!n.isNull()) {
QDomAttr attr = n.toAttr();
if (!attr.isNull())
sensor.gamma = deg2Rad(attr.value().toDouble());
}
QDomNode n1 = node.firstChild();
while(!n1.isNull() ) {
if (n1.nodeName() == "description") {
group.description <<= n1;
}
else if (n1.nodeName() == "sensor") {
group.sensor.length(group.sensor.length() + 1);
group.sensor[group.sensor.length() - 1] = sensor;
group.sensor[group.sensor.length() - 1] <<= n1;
}
n1 = n1.nextSibling();
}
}
}
/// Gets distance in meters for given coordinate
static double distance(const GeoCoordinate &p1, const GeoCoordinate &p2) {
double dLat = deg2Rad(p1.latitude - p2.latitude);
double dLon = deg2Rad(p1.longitude - p2.longitude);
double lat1 = deg2Rad(p1.latitude);
double lat2 = deg2Rad(p2.latitude);
double a = std::sin(dLat/2)*std::sin(dLat/2) +
std::sin(dLon/2)*std::sin(dLon/2)*std::cos(lat1)*std::cos(lat2);
double c = 2*std::atan2(std::sqrt(a), std::sqrt(1 - a));
double radius = wgs84EarthRadius(dLat);
return radius*c;
}
void
operator <<= (SensorDescriptionIDL& sensor, const QDomNode& node)
{
if (!node.isNull()) {
QDomNode n1 = node.firstChild();
while(!n1.isNull() ) {
QDomNode n2 = n1.firstChild();
if (!n2.isNull()) {
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really a text element.
if (n1.nodeName() == "minrange") {
sensor.minRange = t.data().toInt();
}
else if (n1.nodeName() == "maxrange") {
sensor.maxRange = t.data().toInt();
}
else if (n1.nodeName() == "focus") {
sensor.focus = deg2Rad(t.data().toDouble());
}
}
}
n1 = n1.nextSibling();
}
}
}
// gets offset in meters
static double getOffset(const GeoCoordinate& point, double offsetInMeters)
{
double lat = deg2Rad(point.latitude);
// Radius of Earth at given latitude
double radius = wgs84EarthRadius(lat);
double dWidth = offsetInMeters / (2 * radius);
return rad2Deg(dWidth);
}
// init
//--------------------------------------------------------------------------
Math::Math()
{
for (int i = 0; i <= 360; i++) {
float radians = deg2Rad(i);
cosTable[i] = cos(radians);
sinTable[i] = sin(radians);
}
} // end Math::init
Base::Base() :
Element()
{
float motorDistance = 46.188;
// Positions des moteurs par rapport au referenciel des moteurs
Vec3f motor1(sin(deg2Rad(36.28f))*motorDistance, cos(deg2Rad(36.28f))
*motorDistance, 0);
Vec3f motor3(-(cos(deg2Rad(36.28f-30.0f))*motorDistance),
sin(deg2Rad(36.28f-30.0f))*motorDistance, 0);
Vec3f motor2(sin(deg2Rad(60.0f-36.28f))*motorDistance, -(cos(deg2Rad(60.0f
-36.28f))*motorDistance), 0);
osg::Node* node = osgDB::readNodeFile("data/base.stl");
node->setStateSet(createTransparency(0.5));
// Centering the model
osg::MatrixTransform* modelTransform = new osg::MatrixTransform();
modelTransform->setMatrix(osg::Matrix::translate(0, 0, -5));
modelTransform->addChild(node);
osg::MatrixTransform* transform = new osg::MatrixTransform();
transform->addChild(modelTransform);
transform->setMatrix(osg::Matrix::translate( 0, 0, 0));
model->addChild(transform);
}
void Train::warpImage(const Mat &src,
double yaw,
double pitch,
double roll,
double scale,
double fovy,
Mat &dst,
Mat &M,
vector<Point2f> &corners)
{
//Warp Image
//Half of vertical field of view
double halfFovy = fovy * 0.5;
//Compute d
double d = hypot(src.cols, src.rows);
//Compute side length of square
double sideLength = scale * d / cos(deg2Rad(halfFovy));
//Compute warp matrix and set vector of corners
warpMatrix(src.size(), yaw, pitch, roll, scale, fovy, M, &corners);
//Perform actual warp to finish the method
warpPerspective(src,dst,M,Size(sideLength,sideLength));
}
void
CameraParameters::operator <<= (const QDomNode& node)
{
if (!node.isNull()) {
QDomNode n1 = node.firstChild();
while(!n1.isNull() ) {
if (n1.nodeName() == "ncx") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
ncx = t.data().toDouble();
}
}
else if (n1.nodeName() == "nfx") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
nfx = t.data().toDouble();
}
}
else if (n1.nodeName() == "dx") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
dx = t.data().toDouble();
}
}
else if (n1.nodeName() == "dy") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
dy = t.data().toDouble();
}
}
else if (n1.nodeName() == "cx") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
cx = t.data().toDouble();
}
}
else if (n1.nodeName() == "cy") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
cy = t.data().toDouble();
}
}
else if (n1.nodeName() == "sx") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
sx = t.data().toDouble();
}
}
else if (n1.nodeName() == "f") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
f = t.data().toDouble();
}
}
else if (n1.nodeName() == "kappa") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
kappa = t.data().toDouble();
}
}
else if (n1.nodeName() == "height") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
height = t.data().toDouble();
}
}
else if (n1.nodeName() == "alpha") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
alpha = deg2Rad(t.data().toDouble());
}
}
else if (n1.nodeName() == "latency") {
QDomNode n2 = n1.firstChild();
QDomText t = n2.toText(); // try to convert the node to a text
if(!t.isNull() ) { // the node was really an element.
double d = t.data().toDouble();
latency.sec((int)floor(d));
latency.usec((int)floor((d - floor(d)) * 1000000.));
}
}
n1 = n1.nextSibling();
}
}
}
double
ColorLevels::CIEDE2000(
const LAB &lab1,
const LAB &lab2)
{
/*
* "For these and all other numerical/graphical delta E00 values
* reported in this article, we set the parametric weighting factors
* to unity(i.e., k_L = k_C = k_H = 1.0)." (Page 27).
*/
const double k_L = 1.0, k_C = 1.0, k_H = 1.0;
const double deg360InRad = ColorLevels::deg2Rad(360.0);
const double deg180InRad = ColorLevels::deg2Rad(180.0);
const double pow25To7 = 6103515625.0; /* pow(25, 7) */
/*
* Step 1
*/
/* Equation 2 */
double C1 = sqrt((lab1.a * lab1.a) + (lab1.b * lab1.b));
double C2 = sqrt((lab2.a * lab2.a) + (lab2.b * lab2.b));
/* Equation 3 */
double barC = (C1 + C2) / 2.0;
/* Equation 4 */
double G = 0.5 * (1 - sqrt(pow(barC, 7) / (pow(barC, 7) + pow25To7)));
/* Equation 5 */
double a1Prime = (1.0 + G) * lab1.a;
double a2Prime = (1.0 + G) * lab2.a;
/* Equation 6 */
double CPrime1 = sqrt((a1Prime * a1Prime) + (lab1.b * lab1.b));
double CPrime2 = sqrt((a2Prime * a2Prime) + (lab2.b * lab2.b));
/* Equation 7 */
double hPrime1;
if (lab1.b == 0 && a1Prime == 0)
hPrime1 = 0.0;
else
hPrime1 = atan2(lab1.b, a1Prime);
/*
* This must be converted to a hue angle in degrees between 0
* and 360 by addition of 2 to negative hue angles.
*/
if (hPrime1 < 0)
hPrime1 += deg360InRad;
double hPrime2;
if (lab2.b == 0 && a2Prime == 0)
hPrime2 = 0.0;
else
hPrime2 = atan2(lab2.b, a2Prime);
/*
* This must be converted to a hue angle in degrees between 0
* and 360 by addition of 2 to negative hue angles.
*/
if (hPrime2 < 0)
hPrime2 += deg360InRad;
/*
* Step 2
*/
/* Equation 8 */
double deltaLPrime = lab2.l - lab1.l;
/* Equation 9 */
double deltaCPrime = CPrime2 - CPrime1;
/* Equation 10 */
double deltahPrime;
double CPrimeProduct = CPrime1 * CPrime2;
if (CPrimeProduct == 0)
deltahPrime = 0;
else {
/* Avoid the fabs() call */
deltahPrime = hPrime2 - hPrime1;
if (deltahPrime < -deg180InRad)
deltahPrime += deg360InRad;
else if (deltahPrime > deg180InRad)
deltahPrime -= deg360InRad;
}
/* Equation 11 */
double deltaHPrime = 2.0 * sqrt(CPrimeProduct) *
sin(deltahPrime / 2.0);
/*
* Step 3
*/
/* Equation 12 */
double barLPrime = (lab1.l + lab2.l) / 2.0;
/* Equation 13 */
double barCPrime = (CPrime1 + CPrime2) / 2.0;
/* Equation 14 */
double barhPrime, hPrimeSum = hPrime1 + hPrime2;
if (CPrime1 * CPrime2 == 0) {
barhPrime = hPrimeSum;
} else {
if (fabs(hPrime1 - hPrime2) <= deg180InRad)
barhPrime = hPrimeSum / 2.0;
else {
if (hPrimeSum < deg360InRad)
barhPrime = (hPrimeSum + deg360InRad) / 2.0;
else
barhPrime = (hPrimeSum - deg360InRad) / 2.0;
}
}
/* Equation 15 */
double T = 1.0 - (0.17 * cos(barhPrime - ColorLevels::deg2Rad(30.0))) +
(0.24 * cos(2.0 * barhPrime)) +
(0.32 * cos((3.0 * barhPrime) + ColorLevels::deg2Rad(6.0))) -
(0.20 * cos((4.0 * barhPrime) - ColorLevels::deg2Rad(63.0)));
/* Equation 16 */
double deltaTheta = ColorLevels::deg2Rad(30.0) *
exp(-pow((barhPrime - deg2Rad(275.0)) / deg2Rad(25.0), 2.0));
/* Equation 17 */
double R_C = 2.0 * sqrt(pow(barCPrime, 7.0) /
(pow(barCPrime, 7.0) + pow25To7));
/* Equation 18 */
double S_L = 1 + ((0.015 * pow(barLPrime - 50.0, 2.0)) /
sqrt(20 + pow(barLPrime - 50.0, 2.0)));
/* Equation 19 */
double S_C = 1 + (0.045 * barCPrime);
/* Equation 20 */
double S_H = 1 + (0.015 * barCPrime * T);
/* Equation 21 */
double R_T = (-sin(2.0 * deltaTheta)) * R_C;
/* Equation 22 */
double deltaE = sqrt(
pow(deltaLPrime / (k_L * S_L), 2.0) +
pow(deltaCPrime / (k_C * S_C), 2.0) +
pow(deltaHPrime / (k_H * S_H), 2.0) +
(R_T * (deltaCPrime / (k_C * S_C)) * (deltaHPrime / (k_H * S_H))));
return (deltaE);
}
//------------------------------------------------------------------------------
InstancePlacer::InstancePlacer(unsigned layer_num,
const TerrainDataClient * terrain_data,
const std::vector<bbm::DetailTexInfo> & tex_info) :
cur_camera_cell_x_(-1000),
cur_camera_cell_z_(-1000),
terrain_(terrain_data),
instances_per_cell_(0),
layer_num_(layer_num)
{
cos_threshold_steepness_ = cosf(deg2Rad(s_params.get<float>("instances.threshold_steepness")));
unsigned num_layers = s_params.get<unsigned>("instances.num_layers");
if (s_params.get<std::vector<float> >("instances.density_factor").size() != num_layers)
{
throw Exception("density factor not specified for all layers.");
}
cell_resolution_ = s_params.get<unsigned>("instances.cell_resolution");
cell_size_ = s_params.get<float> ("instances.base_draw_distance")*(layer_num+1) / cell_resolution_;
if (!PlacedInstance::dummy_desc_.get())
{
PlacedInstance::dummy_desc_.reset(new InstancedGeometryDescription("dummy"));
}
assert(cell_resolution_);
cell_resolution_ |= 1; // Make sure we always have an odd number of cells
instance_probability_.resize(tex_info.size());
instance_prototype_. resize(tex_info.size());
// The maximum density determines the number of instances allocated
float max_density = std::numeric_limits<float>::min();
for (unsigned terrain_type=0; terrain_type<tex_info.size(); ++terrain_type)
{
max_density = std::max(max_density, tex_info[terrain_type].grass_density_);
// Copy probabilities...
instance_probability_[terrain_type].resize(tex_info[terrain_type].zone_info_.size());
for (unsigned t=0; t < instance_probability_[terrain_type].size(); ++t)
{
instance_probability_[terrain_type][t] = tex_info[terrain_type].zone_info_[t].probability_;
}
// Load model prototypes
instance_prototype_[terrain_type].resize(tex_info[terrain_type].zone_info_.size());
for (unsigned obj=0; obj<instance_prototype_[terrain_type].size(); ++obj)
{
osg::ref_ptr<InstanceProxy> proxy;
try
{
proxy = dynamic_cast<InstanceProxy*>(
ReaderWriterBbm::loadModel(tex_info[terrain_type].zone_info_[obj].model_).get());
if (!proxy.get())
{
Exception e("Error loading automatically placed object \"");
e << tex_info[terrain_type].zone_info_[obj].model_
<< "\". Perhaps it's not instanced?";
throw e;
}
} catch (Exception & e)
{
e.addHistory("InstancePlacer::InstancePlacer()");
throw e;
}
instance_prototype_[terrain_type][obj] = proxy;
}
}
// Depending on the layer we are in, we want to have a different
// instance density. max_density is the maximum density in
// instances/m^2 specified for any terrain type and determines the
// number of prototypes we have to allocate per cell. Terrain
// types with smaller density have some prototypes unused
// (probability doesn't sum up to one).
// convert density to instances per cell,
// adjust for layer number.
instances_per_cell_ =
(unsigned)(cell_size_*cell_size_ * max_density *
s_params.get<std::vector<float> >("instances.density_factor")[layer_num_]*
s_params.get<float>("instances.user_density"));
// Perhaps there are no instances in this level, or user chose
// zero density
if (instances_per_cell_ == 0) return;
normalizeProbabilities(instance_probability_, max_density, tex_info);
initCells();
s_scheduler.addTask(PeriodicTaskCallback(this, &InstancePlacer::update),
s_params.get<float>("instances.update_dt"),
"InstancePlacer::update",
&fp_group_);
}
void Train::warpMatrix(Size sz,
double yaw,
double pitch,
double roll,
double scale,
double fovy,
Mat &M,
vector<Point2f>* corners)
{
double st=sin(deg2Rad(roll));
double ct=cos(deg2Rad(roll));
double sp=sin(deg2Rad(pitch));
double cp=cos(deg2Rad(pitch));
double sg=sin(deg2Rad(yaw));
double cg=cos(deg2Rad(yaw));
double halfFovy=fovy*0.5;
double d=hypot(sz.width,sz.height);
double sideLength=scale*d/cos(deg2Rad(halfFovy));
double h=d/(2.0*sin(deg2Rad(halfFovy)));
double n=h-(d/2.0);
double f=h+(d/2.0);
Mat F=Mat(4,4,CV_64FC1);
Mat Rroll=Mat::eye(4,4,CV_64FC1);
Mat Rpitch=Mat::eye(4,4,CV_64FC1);
Mat Ryaw=Mat::eye(4,4,CV_64FC1);
Mat T=Mat::eye(4,4,CV_64FC1);
Mat P=Mat::zeros(4,4,CV_64FC1);
Rroll.at<double>(0,0)=Rroll.at<double>(1,1)=ct;
Rroll.at<double>(0,1)=-st;Rroll.at<double>(1,0)=st;
Rpitch.at<double>(1,1)=Rpitch.at<double>(2,2)=cp;
Rpitch.at<double>(1,2)=-sp;Rpitch.at<double>(2,1)=sp;
Ryaw.at<double>(0,0)=Ryaw.at<double>(2,2)=cg;
Ryaw.at<double>(0,2)=sg;Ryaw.at<double>(2,0)=sg;
T.at<double>(2,3)=-h;
P.at<double>(0,0)=P.at<double>(1,1)=1.0/tan(deg2Rad(halfFovy));
P.at<double>(2,2)=-(f+n)/(f-n);
P.at<double>(2,3)=-(2.0*f*n)/(f-n);
P.at<double>(3,2)=-1.0;
F=P*T*Rpitch*Rroll*Ryaw;
double ptsIn [4*3];
double ptsOut[4*3];
double halfW=sz.width/2, halfH=sz.height/2;
ptsIn[0]=-halfW;ptsIn[ 1]= halfH;
ptsIn[3]= halfW;ptsIn[ 4]= halfH;
ptsIn[6]= halfW;ptsIn[ 7]=-halfH;
ptsIn[9]=-halfW;ptsIn[10]=-halfH;
ptsIn[2]=ptsIn[5]=ptsIn[8]=ptsIn[11]=0;
Mat ptsInMat(1,4,CV_64FC3,ptsIn);Mat ptsOutMat(1,4,CV_64FC3,ptsOut);
perspectiveTransform(ptsInMat,ptsOutMat,F);
Point2f ptsInPt2f[4];Point2f ptsOutPt2f[4];
for(int i=0;i<4;i++)
{
Point2f ptIn(ptsIn[i*3+0],ptsIn[i*3+1]);
Point2f ptOut(ptsOut[i*3+0],ptsOut[i*3+1]);
ptsInPt2f[i]=ptIn+Point2f(halfW,halfH);
ptsOutPt2f[i]=(ptOut+Point2f(1,1))*(sideLength*0.5);
}
M=getPerspectiveTransform(ptsInPt2f,ptsOutPt2f);
if(corners!=NULL)
{
corners->clear();
corners->push_back(ptsOutPt2f[0]);
corners->push_back(ptsOutPt2f[1]);
corners->push_back(ptsOutPt2f[2]);
corners->push_back(ptsOutPt2f[3]);
}
}