Skip to content

Commit

Permalink
Merge pull request #341 from dartsim/parse_vectorxd
Browse files Browse the repository at this point in the history 
Support Eigen::VectorXd in parser
  • Loading branch information
@jslee02
jslee02 committed Mar 6, 2015
2 parents 7d8be94 + d907c45 commit 2df78a4
Show file tree
Hide file tree
Showing 3 changed files with 111 additions and 57 deletions.
72 changes: 67 additions & 5 deletions dart/utils/Parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,18 +83,34 @@ std::string toString(char _v)
return boost::lexical_cast<std::string>(_v);
}

//==============================================================================
std::string toString(const Eigen::Vector2d& _v)
{
Eigen::RowVector2d rowVector2d = _v.transpose();

return boost::lexical_cast<std::string>(rowVector2d);
return boost::lexical_cast<std::string>(_v.transpose());
}

//==============================================================================
std::string toString(const Eigen::Vector3d& _v)
{
Eigen::RowVector3d rowVector3d = _v.transpose();
return boost::lexical_cast<std::string>(_v.transpose());
}

//==============================================================================
std::string toString(const Eigen::Vector3i& _v)
{
return boost::lexical_cast<std::string>(_v.transpose());
}

return boost::lexical_cast<std::string>(rowVector3d);
//==============================================================================
std::string toString(const Eigen::Vector6d& _v)
{
return boost::lexical_cast<std::string>(_v.transpose());
}

//==============================================================================
std::string toString(const Eigen::VectorXd& _v)
{
return boost::lexical_cast<std::string>(_v.transpose());
}

std::string toString(const Eigen::Isometry3d& _v)
Expand Down Expand Up @@ -282,6 +298,40 @@ Eigen::Vector6d toVector6d(const std::string& _str)
return ret;
}

//==============================================================================
Eigen::VectorXd toVectorXd(const std::string& _str)
{
std::vector<std::string> pieces;
std::string trimedStr = boost::trim_copy(_str);
boost::split(pieces, trimedStr, boost::is_any_of(" "),
boost::token_compress_on);
assert(pieces.size() > 0);

Eigen::VectorXd ret(pieces.size());

for (size_t i = 0; i < pieces.size(); ++i)
{
if (pieces[i] != "")
{
try
{
ret(i) = boost::lexical_cast<double>(pieces[i].c_str());
}
catch(boost::bad_lexical_cast& e)
{
std::cerr << "value ["
<< pieces[i]
<< "] is not a valid double for Eigen::VectorXd["
<< i
<< "]"
<< std::endl;
}
}
}

return ret;
}

Eigen::Isometry3d toIsometry3d(const std::string& _str)
{
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
Expand Down Expand Up @@ -478,6 +528,18 @@ Eigen::Vector6d getValueVector6d(tinyxml2::XMLElement* _parentElement, const std
return toVector6d(str);
}

//==============================================================================
Eigen::VectorXd getValueVectorXd(tinyxml2::XMLElement* _parentElement,
const std::string& _name)
{
assert(_parentElement != NULL);
assert(!_name.empty());

std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText();

return toVectorXd(str);
}

Eigen::Vector3d getValueVec3(tinyxml2::XMLElement* _parentElement, const std::string& _name)
{
assert(_parentElement != NULL);
Expand Down
5 changes: 5 additions & 0 deletions dart/utils/Parser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,9 @@ std::string toString(double _v);
std::string toString(char _v);
std::string toString(const Eigen::Vector2d& _v);
std::string toString(const Eigen::Vector3d& _v);
std::string toString(const Eigen::Vector3i& _v);
std::string toString(const Eigen::Vector6d& _v);
std::string toString(const Eigen::VectorXd& _v);
std::string toString(const Eigen::Isometry3d& _v);

bool toBool (const std::string& _str);
Expand All @@ -68,6 +71,7 @@ Eigen::Vector2d toVector2d (const std::string& _str);
Eigen::Vector3d toVector3d (const std::string& _str);
Eigen::Vector3i toVector3i (const std::string& _str);
Eigen::Vector6d toVector6d (const std::string& _str);
Eigen::VectorXd toVectorXd (const std::string& _str);
// TODO: The definition of _str is not clear for transform (see: #250)
Eigen::Isometry3d toIsometry3d(const std::string& _str);
Eigen::Isometry3d toIsometry3dWithExtrinsicRotation(const std::string& _str);
Expand All @@ -83,6 +87,7 @@ Eigen::Vector2d getValueVector2d (tinyxml2::XMLElement* _parentElement, const
Eigen::Vector3d getValueVector3d (tinyxml2::XMLElement* _parentElement, const std::string& _name);
Eigen::Vector3i getValueVector3i (tinyxml2::XMLElement* _parentElement, const std::string& _name);
Eigen::Vector6d getValueVector6d (tinyxml2::XMLElement* _parentElement, const std::string& _name);
Eigen::VectorXd getValueVectorXd (tinyxml2::XMLElement* _parentElement, const std::string& _name);
Eigen::Isometry3d getValueIsometry3d(tinyxml2::XMLElement* _parentElement, const std::string& _name);
Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(tinyxml2::XMLElement* _parentElement, const std::string& _name);

Expand Down
91 changes: 39 additions & 52 deletions unittests/testParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,58 +56,45 @@ using namespace utils;
//==============================================================================
TEST(Parser, DataStructure)
{
bool v1 = true;
int v2 = -3;
unsigned int v3 = 1;
float v4 = -3.140f;
double v5 = 1.4576640;
char v6 = 'd';
Eigen::Vector2d v7 = Eigen::Vector2d::Ones();
Eigen::Vector3d v8 = Eigen::Vector3d::Ones();
//Eigen::Vector3d v9 = Eigen::Vector3d::Ones();
//math::SO3 v10;
Eigen::Isometry3d v11 = Eigen::Isometry3d::Identity();

std::string str1 = toString(v1);
std::string str2 = toString(v2);
std::string str3 = toString(v3);
std::string str4 = toString(v4);
std::string str5 = toString(v5);
std::string str6 = toString(v6);
std::string str7 = toString(v7);
std::string str8 = toString(v8);
//std::string str9 = toString(v9);
//std::string str10 = toString(v10);
std::string str11 = toString(v11);

bool b = toBool(str1);
int i = toInt(str2);
unsigned int ui = toUInt(str3);
float f = toFloat(str4);
double d = toDouble(str5);
char c = toChar(str6);
Eigen::Vector2d vec2 = toVector2d(str7);
Eigen::Vector3d vec3 = toVector3d(str8);
//Eigen::Vector3d valso3 = toVector3d(str9);
//math::SO3 valSO3 = toSO3(str10);
Eigen::Isometry3d valSE3 = toIsometry3d(str11);

EXPECT_EQ(b, v1);
EXPECT_EQ(i, v2);
EXPECT_EQ(ui, v3);
EXPECT_EQ(f, v4);
EXPECT_EQ(d, v5);
EXPECT_EQ(c, v6);
for (int i = 0; i < 2; i++)
EXPECT_EQ(vec2[i], v7[i]);
EXPECT_EQ(vec3, v8);
//EXPECT_EQ(valso3, v9);
//EXPECT_EQ(valSO3, v10);
for (int i = 0; i < 4; ++i)
{
for (int j = 0; j < 4; ++j)
EXPECT_EQ(valSE3(i,j), v11(i,j));
}
bool valBool = true;
int valInt = -3;
unsigned int valUInt = 1;
float valFloat = -3.140f;
double valDouble = 1.4576640;
char valChar = 'd';
Eigen::Vector2d valVector2d = Eigen::Vector2d::Random();
Eigen::Vector3d valVector3d = Eigen::Vector3d::Random();
Eigen::Vector3i valVector3i = Eigen::Vector3i::Random();
Eigen::Vector6d valVector6d = Eigen::Vector6d::Random();
Eigen::VectorXd valVectorXd = Eigen::VectorXd::Random(10);
Eigen::Isometry3d valIsometry3d = Eigen::Isometry3d::Identity();

std::string strBool = toString(valBool);
std::string strInt = toString(valInt);
std::string strUInt = toString(valUInt);
std::string strFloat = toString(valFloat);
std::string strDouble = toString(valDouble);
std::string strChar = toString(valChar);
std::string strVector2d = toString(valVector2d);
std::string strVector3d = toString(valVector3d);
std::string strVector3i = toString(valVector3i);
std::string strVector6d = toString(valVector6d);
std::string strVectorXd = toString(valVectorXd);
std::string strIsometry3d = toString(valIsometry3d);

EXPECT_EQ(valBool, toBool(strBool));
EXPECT_EQ(valInt, toInt(strInt));
EXPECT_EQ(valUInt, toUInt(strUInt));
EXPECT_EQ(valFloat, toFloat(strFloat));
EXPECT_EQ(valDouble, toDouble(strDouble));
EXPECT_EQ(valChar, toChar(strChar));
EXPECT_TRUE(equals(valVector2d, toVector2d(strVector2d)));
EXPECT_TRUE(equals(valVector3d, toVector3d(strVector3d)));
EXPECT_EQ(valVector3i, toVector3i(strVector3i));
EXPECT_TRUE(equals(valVector6d, toVector6d(strVector6d)));
EXPECT_TRUE(equals(valVectorXd, toVectorXd(strVectorXd)));
EXPECT_TRUE(equals(valIsometry3d.matrix(),
toIsometry3d(strIsometry3d).matrix()));
}

//==============================================================================
Expand Down

0 comments on commit 2df78a4

Please sign in to comment.