Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support Eigen::VectorXd in parser #341

Merged
merged 2 commits into from
Mar 6, 2015
Merged

Support Eigen::VectorXd in parser #341

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8ab72b1
Support Eigen::VectorXd in parser
jslee02 Feb 28, 2015
d907c45
Remove accidentally added lines in testParser.cpp
jslee02 Feb 28, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
72 changes: 67 additions & 5 deletions dart/utils/Parser.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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