void FootstepDisplay::update(float wall_dt, float ros_dt)
{
for (size_t i = 0; i < shapes_.size(); i++) {
ShapePtr shape = shapes_[i];
texts_[i]->setVisible(show_name_); // TODO
jsk_footstep_msgs::Footstep footstep = latest_footstep_->footsteps[i];
// color
if (use_group_coloring_) {
std_msgs::ColorRGBA color
= jsk_topic_tools::colorCategory20(footstep.footstep_group);
shape->setColor(color.r, color.g, color.b, alpha_);
}
else {
if (footstep.leg == jsk_footstep_msgs::Footstep::LEFT) {
shape->setColor(0, 1, 0, alpha_);
}
else if (footstep.leg == jsk_footstep_msgs::Footstep::RIGHT) {
shape->setColor(1, 0, 0, alpha_);
}
else {
shape->setColor(1, 1, 1, alpha_);
}
}
}
}
void FootstepDisplay::allocateCubes(size_t num) {
if (num > shapes_.size()) {
// need to allocate
for (size_t i = shapes_.size(); i < num; i++) {
ShapePtr shape;
shape.reset(new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(),
scene_node_));
shapes_.push_back(shape);
}
}
else if (num < shapes_.size()) {
// need to remove
shapes_.resize(num);
}
}
void Optimizer::FitRest(mat& alpha,
mat& beta,
mat& rho,
mat& lamda,
InputPtr input,
ModelPtr model,
MeshPtr mesh,
ShapePtr shape,
TexturePtr texture)
{
mat alpha_gradient = mat(PrincipalNum, 1, fill::zeros);
mat alpha_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);
mat beta_gradient = mat(PrincipalNum, 1, fill::zeros);
mat beta_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);
Rho rho_para(rho, input, model, mesh, shape, texture);
Lamda lamda_para(lamda, model, mesh, shape, texture);
// 2500 1000 700 500 300 200
model->InitialRandomGenerator(ModelImage::REST);
double weight = 1.0 / 200;
for (int l = 0; l < 1000; ++l)
{
Alpha alpha_para(alpha, input, model, mesh, shape, texture);
Beta beta_para(beta, model, mesh, shape, texture);
// Generate random points
GenerateRandomPoints(model, GradientRandomNum);
double function_value = 0;
for (int i = 0; i < PrincipalNum; ++i)
{
double variance = shape->GetVariance(i);
alpha_gradient[i] = weight * ComputeIntensityGradient(input, &alpha_para, i) + 2 * alpha[i] / variance;
}
for (int i = 0; i < PrincipalNum; ++i)
{
double variance = texture->GetVariance(i);
beta_gradient[i] = weight * ComputeIntensityGradient(input, &beta_para, i) + 2 * beta[i] / variance;
}
alpha -= alpha_para.Step*alpha_gradient;
beta -= beta_para.Step*beta_gradient;
}
}
SkeletonPtr createGround()
{
// Create a Skeleton to represent the ground
SkeletonPtr ground = Skeleton::create("ground");
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
double thickness = 0.01;
tf.translation() = Eigen::Vector3d(0,0,-thickness/2.0);
WeldJoint::Properties joint;
joint.mT_ParentBodyToJoint = tf;
ground->createJointAndBodyNodePair<WeldJoint>(nullptr, joint);
ShapePtr groundShape =
std::make_shared<BoxShape>(Eigen::Vector3d(10,10,thickness));
groundShape->setColor(dart::Color::Blue(0.2));
ground->getBodyNode(0)->addVisualizationShape(groundShape);
ground->getBodyNode(0)->addCollisionShape(groundShape);
return ground;
}
SkeletonPtr createAtlas()
{
// Parse in the atlas model
DartLoader urdf;
SkeletonPtr atlas =
urdf.parseSkeleton(DART_DATA_PATH"sdf/atlas/atlas_v3_no_head.urdf");
// Add a box to the root node to make it easier to click and drag
double scale = 0.25;
ShapePtr boxShape =
std::make_shared<BoxShape>(scale*Eigen::Vector3d(1.0, 1.0, 0.5));
boxShape->setColor(dart::Color::Black());
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
tf.translation() = Eigen::Vector3d(0.1*Eigen::Vector3d(0.0, 0.0, 1.0));
boxShape->setLocalTransform(tf);
atlas->getBodyNode(0)->addVisualizationShape(boxShape);
return atlas;
}
void FootstepDisplay::updateAlpha()
{
float alpha = alpha_property_->getFloat();
for (size_t i = 0; i < shapes_.size(); i++)
{
ShapePtr shape = shapes_[i];
jsk_footstep_msgs::Footstep footstep = latest_footstep_->footsteps[i];
if (footstep.leg == jsk_footstep_msgs::Footstep::LEFT)
{
shape->setColor(0, 1, 0, alpha);
}
else if (footstep.leg == jsk_footstep_msgs::Footstep::RIGHT)
{
shape->setColor(1, 0, 0, alpha);
}
else
{
shape->setColor(1, 1, 1, alpha);
}
}
}
void ClipVisitor::visit(ShapePtr shape)
{
ShapeRendererStatePtr renderer_state = shape->getRendererState(renderer);
D2DShapeRendererStatePtr shape_renderer = dynamic_pointer_cast<D2DShapeRendererState>(renderer_state);
vector<ID2D1Geometry*> temp_geometry;
shape_renderer->getGeometry(&temp_geometry);
for (size_t i = 0; i < temp_geometry.size(); i++) {
ID2D1TransformedGeometry* transformed_geometry;
HRESULT hr = renderer->m_pFactory->CreateTransformedGeometry(
temp_geometry[i], &d2dMatrix(matrix_stack.top()), &transformed_geometry);
if (SUCCEEDED(hr)) {
geometry.push_back(transformed_geometry);
}
}
}
void Optimizer::FitAll(mat& alpha,
mat& beta,
mat& rho,
mat& lamda,
InputPtr input,
ModelPtr model,
MeshPtr mesh,
ShapePtr shape,
TexturePtr texture)
{
mat alpha_gradient = mat(PrincipalNum, 1, fill::zeros);
mat alpha_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);
mat beta_gradient = mat(PrincipalNum, 1, fill::zeros);
mat beta_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);
mat rho_gradient = mat(RhoNum, 1, fill::zeros);
mat rho_hessian_inv = mat(RhoNum, RhoNum, fill::zeros);
mat lamda_gradient = mat(LamdaNum, 1, fill::zeros);
mat lamda_hessian_inv = mat(LamdaNum, LamdaNum, fill::zeros);
vec step(RhoNum, 1);
step.rows(0, 2).fill(0.00000006);
step.rows(3, 5).fill(0.0002);
step.rows(6, 6).fill(2.0);
mat step_mat = diagmat(step);
SetStartingValue(rho, lamda);
// 2500 1000 700 500 300 200
array<double, 6> weight_intensity = { { 1.0 / 1000, 1.0 / 900, 1.0 / 800, 1.0 / 700, 1.0 / 500, 1.0 / 300 } };
array<int, 6> para_num = { { 10, 15, 25, 35, 55, 99 } }; // 20 40
array<int, 6> iterations = { { 1000, 1000, 1000, 1000, 1000, 1000 } };
//array<double, 4> weight_intensity = { { 1.0 / 900, 1.0 / 700, 1.0 / 500, 1.0 / 400 } };
//array<int, 4> para_num = { { 10, 20, 40, 99 } }; // 20 40
//array<int, 4> iterations = { { 1000, 1000, 800, 600 } };
int counter = 0;
for (int c = 0; c < 6; ++c)
{
for (int l = 0; l < iterations[c]; ++l)
{
if (counter == 0 || counter % 1000 == 0)
{
Face3dModel face3d_model(shape, texture);
mesh = face3d_model.Construction(alpha, beta);
TwoPassZbuffer(rho, lamda, mesh, model);
model->EnableIterator();
model->InitialRandomGenerator();
//GenerateRandomPoints(model, HessianRandomNum);
//for (int i = 0; i < para_num[c]; ++i)
//{
// double variance = shape->GetVariance(i);
// mat alpha1 = alpha;
// alpha1[i] -= H;
// Alpha alpha_para1(alpha1, input, model, mesh, shape, texture);
// double first_derivative1 = weight_feature[c] * ComputeLandmarkGradient(input, &alpha_para1, i) +
// weight_intensity[c] * ComputeIntensityGradient(input, &alpha_para1, i);
// mat alpha2 = alpha;
// alpha2[i] += H;
// Alpha alpha_para2(alpha2, input, model, mesh, shape, texture);
// double first_derivative2 = weight_feature[c] * ComputeLandmarkGradient(input, &alpha_para2, i) +
// weight_intensity[c] * ComputeIntensityGradient(input, &alpha_para2, i);
// double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
// alpha_hessian_inv(i, i) = 1 / (second_derivative +2 / variance);
//}
/* for (int i = 0; i < para_num[c]; ++i)
{
double variance = texture->GetVariance(i);
mat beta1 = beta;
beta1[i] -= H;
Beta beta_para1(beta1, model, mesh, shape, texture);
double first_derivative1 = weight_intensity[c] * ComputeIntensityGradient(input, &beta_para1, i);
mat beta2 = beta;
beta2[i] += H;
Beta beta_para2(beta2, model, mesh, shape, texture);
double first_derivative2 = weight_intensity[c] * ComputeIntensityGradient(input, &beta_para2, i);
double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
beta_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);
}*/
//for (int i = 0; i < RhoNum; ++i)
//{
// double variance = GetRhoVariance(i);
// mat rho1 = rho;
// rho1[i] -= H;
// Rho rho_para1(rho1, input, model, mesh, shape, texture);
// double first_derivative1 =/* weight_feature[c] * ComputeLandmarkGradient(input, &rho_para1, i) +*/
// weight_intensity[c] * ComputeIntensityGradient(input, &rho_para1, i);
// mat rho2 = rho;
// rho2[i] += H;
// Rho rho_para2(rho2, input, model, mesh, shape, texture);
// double first_derivative2 = /*weight_feature[c] * ComputeLandmarkGradient(input, &rho_para2, i) +*/
// weight_intensity[c] * ComputeIntensityGradient(input, &rho_para2, i);
// double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
// rho_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);
//}
//for (int i = 0; i < LamdaNum - 7; ++i)
//{
// double variance = GetLamdaVariance(i);
// mat lamda1 = lamda;
// lamda1[i] -= H;
// Lamda lamda_para1(lamda1, model, mesh, shape, texture);
// double first_derivative1 = weight_intensity[c] * ComputeIntensityGradient(input, &lamda_para1, i);
// mat lamda2 = lamda;
// lamda2[i] += H;
// Lamda lamda_para2(lamda2, model, mesh, shape, texture);
// double first_derivative2 = weight_intensity[c] * ComputeIntensityGradient(input, &lamda_para2, i);
// double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
// lamda_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);
//}
}
Alpha alpha_para(alpha, input, model, mesh, shape, texture);
Beta beta_para(beta, model, mesh, shape, texture);
Rho rho_para(rho, input, model, mesh, shape, texture);
Lamda lamda_para(lamda, model, mesh, shape, texture);
GenerateRandomPoints(model, GradientRandomNum);
//double function_value = 0;
//function_value = ComputeCost(input, model, alpha_para);
//ofstream cost;
//cost.open("all_cost", ios::app);
//cost << function_value << "\n";
//cost.close();
for (int i = 0; i < para_num[c]; ++i)
{
double variance = shape->GetVariance(i);
alpha_gradient[i] = /*weight_feature[c] * ComputeLandmarkGradient(input, &alpha_para, i) +*/
weight_intensity[c] * ComputeIntensityGradient(input, &alpha_para, i) +2 * alpha[i] / variance;
}
for (int i = 0; i < para_num[c]; ++i)
{
double variance = texture->GetVariance(i);
beta_gradient[i] = weight_intensity[c] * ComputeIntensityGradient(input, &beta_para, i) + 2 * beta[i] / variance;
}
for (int i = 0; i < RhoNum; ++i)
{
//double variance = GetRhoVariance(i);
//double mean = GetRhoMean(i);
rho_gradient[i] = /*weight_feature[c] * ComputeLandmarkGradient(input, &rho_para, i) +*/
weight_intensity[c] * ComputeIntensityGradient(input, &rho_para, i);// +2 * (rho[i] - mean) / variance;
}
for (int i = 0; i < LamdaNum; ++i)
{
double variance = GetLamdaVariance(i);
double mean = GetLamdaMean(i);
lamda_gradient[i] = weight_intensity[c] * ComputeIntensityGradient(input, &lamda_para, i) +2 * (lamda[i] - mean) / variance;
}
//alpha -= 0.00003*alpha_hessian_inv*alpha_gradient; // 0.01 0.00003
//beta -= 0.01*beta_hessian_inv*beta_gradient;
//lamda -= 0.03*lamda_hessian_inv*lamda_gradient;
//rho -= 0.0001*rho_hessian_inv*rho_gradient;
alpha -= alpha_para.Step*alpha_gradient;
beta -= beta_para.Step*beta_gradient;
rho -= step_mat*rho_gradient; // gradient descent
lamda -= lamda_para.Step*lamda_gradient;
++counter;
}
Face3dModel face3d_model(shape, texture);
mesh = face3d_model.Construction(alpha, beta);
VisualizeResult(rho, lamda, input, mesh, "segmented", c);
}
}
void Optimizer::FitFirstPrincipalComponents(mat& alpha,
mat& beta,
mat& rho,
mat& lamda,
InputPtr input,
ModelPtr model,
MeshPtr mesh,
ShapePtr shape,
TexturePtr texture,
const cv::Mat& ready)
{
mat alpha_gradient = mat(PrincipalNum, 1, fill::zeros);
mat alpha_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);
mat beta_gradient = mat(PrincipalNum, 1, fill::zeros);
mat rho_gradient = mat(RhoNum, 1, fill::zeros);
mat lamda_gradient = mat(LamdaNum, 1, fill::zeros);
vec step(RhoNum, 1);
step.rows(0, 2).fill(0.00000006);
step.rows(3, 5).fill(0.0002);
step.rows(6, 6).fill(2.0);
mat step_mat = diagmat(step);
// down sampled version
cv::Mat down_sampled;
pyrDown(ready, down_sampled, Size(IMAGE_WIDTH / 2, IMAGE_HEIGHT / 2));
input = make_shared<InputImage>(down_sampled);
model->SetSize(IMAGE_WIDTH / 2, IMAGE_HEIGHT / 2);
rho[FOCAL] /= 2;
//SetStartingValue(rho, lamda);
double para_num = 10;
double weight = 1.0 / 900;
int counter = 0;
while (counter < 1000)
{
Alpha alpha_para(alpha, input, model, mesh, shape, texture);
Beta beta_para(beta, model, mesh, shape, texture);
Rho rho_para(rho, input, model, mesh, shape, texture);
Lamda lamda_para(lamda, model, mesh, shape, texture);
if (counter == 0 || counter % 1000 == 0)
{
Face3dModel face3d_model(shape, texture);
mesh = face3d_model.Construction(alpha, beta);
TwoPassZbuffer(rho, lamda, mesh, model);
model->EnableIterator();
model->InitialRandomGenerator();
//GenerateRandomPoints(model, HessianRandomNum);
//for (int i = 0; i < para_num; ++i)
//{
// double variance = shape->GetVariance(i);
// mat alpha1 = alpha;
// alpha1[i] -= H;
// Alpha alpha_para1(alpha1, input, model, mesh, shape, texture);
// double first_derivative1 = weight * ComputeIntensityGradient(input, &alpha_para1, i) + 2 * alpha1[i] / variance;
// mat alpha2 = alpha;
// alpha2[i] += H;
// Alpha alpha_para2(alpha2, input, model, mesh, shape, texture);
// double first_derivative2 = weight * ComputeIntensityGradient(input, &alpha_para2, i) + 2 * alpha2[i] / variance;
// double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
// alpha_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);
//}
}
// Generate random points
GenerateRandomPoints(model, GradientRandomNum);
double function_value = 0;
function_value = ComputeCost(input, model, alpha_para);
ofstream cost;
cost.open("first_cost", ios::app);
cost << function_value << "\n";
cost.close();
for (int i = 0; i < para_num; ++i)
{
double variance = shape->GetVariance(i);
alpha_gradient[i] = weight * ComputeIntensityGradient(input, &alpha_para, i) + 2 * alpha[i] / variance;
}
//for (int i = 0; i < 10; ++i)
//{
// double variance = texture->GetVariance(i);
// beta_gradient[i] = weight *ComputeIntensityGradient(input, &beta_para, i) + 2 * beta[i] / variance;
//}
//for (int i = 0; i < RhoNum; ++i)
//{
// double first_derivative1 = 1.0 / 1000*ComputeIntensityGradient(input, &rho_para, i);
// //double variance = rho_para.GetVariance(i);
// //double mean = rho_para.GetMean(i);
// rho_gradient[i] = first_derivative1;// +2 * (rho[i] - mean) / variance;
//}
//for (int i = 0; i < LamdaNum-7; ++i)
//{
// double first_derivative1 = 1.0 / 1000 * ComputeIntensityGradient(input, &lamda_para, i);
// //double variance = lamda_para.GetVariance(i);
// //double mean = lamda_para.GetMean(i);
// lamda_gradient[i] = first_derivative1; //+2 * (lamda[i] - mean) / variance;
//}
alpha -= alpha_para.Step*alpha_gradient;
//beta -= beta_para.Step*beta_gradient;
//rho -= step_mat*rho_gradient;
//lamda -= lamda_para.Step*lamda_gradient;
//alpha -= 0.00008*alpha_hessian_inv*alpha_gradient;
++counter;
}
// restore original version
input= make_shared<InputImage>(ready);
model->SetSize(IMAGE_WIDTH, IMAGE_HEIGHT);
rho[FOCAL] *= 2;
Face3dModel face3d_model(shape, texture);
mesh = face3d_model.Construction(alpha, beta);
VisualizeResult(rho, lamda, input, mesh, "first");
}
void Optimizer::FitPose(mat& alpha,
mat& rho,
mat& lamda,
InputPtr input,
ModelPtr model,
MeshPtr mesh,
ShapePtr shape,
TexturePtr texture)
{
mat alpha_gradient=mat(PrincipalNum, 1,fill::zeros);
mat alpha_hessian_inv = mat(PrincipalNum, PrincipalNum, fill::zeros);
mat rho_gradient=mat(RhoNum, 1,fill::zeros);
mat rho_hessian_inv = mat(RhoNum, RhoNum, fill::zeros);
double weight = 0.05;
int para_num = 10;
int counter = 0;
while (counter<300) // 300
{
Alpha alpha_para(alpha, input, model, mesh, shape, texture);
Rho rho_para(rho, input, model, mesh);
Lamda lamda_para(lamda, model, mesh);
//stringstream file;
//double residual = 0;
//residual = ComputeCost(input, alpha_para);
//ofstream cost;
//cost.open("cost", ios::app);
//cost << residual << "\n";
//cost.close();
if (counter == 0 || counter % 1000 == 0)
{
for (int i = 0; i < para_num; ++i)
{
double variance = shape->GetVariance(i);
mat alpha1 = alpha;
alpha1[i] -= H;
Alpha alpha_para1(alpha1, input, model, mesh, shape, texture);
double first_derivative1 = weight * ComputeLandmarkGradient(input, &alpha_para1, i);
mat alpha2 = alpha;
alpha2[i] += H;
Alpha alpha_para2(alpha2, input, model, mesh, shape, texture);
double first_derivative2 = weight * ComputeLandmarkGradient(input, &alpha_para2, i);
double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
alpha_hessian_inv(i, i) = 1 / (second_derivative + 2 / variance);
}
for (int i = 0; i < RhoNum;++i)
{
// rho
mat rho1 = rho;
rho1[i] -= H;
Rho rho_para1(rho1, input, model, mesh);
double first_derivative1 = weight * ComputeLandmarkGradient(input, &rho_para1, i);
mat rho2 = rho;
rho1[i] += H;
Rho rho_para2(rho2, input, model, mesh);
double first_derivative2 = weight * ComputeLandmarkGradient(input, &rho_para2, i);
double second_derivative = (first_derivative2 - first_derivative1) / (2 * H);
rho_hessian_inv(i, i) = 1 / second_derivative;
}
}
for (int i = 0; i < para_num; ++i)
{
double variance = shape->GetVariance(i);
alpha_gradient[i] = weight*ComputeLandmarkGradient(input, &alpha_para, i)+2 * alpha[i] / variance;
}
for (int i = 0; i < RhoNum; ++i)
{
rho_gradient[i] = weight*ComputeLandmarkGradient(input, &rho_para, i);
}
alpha -= 0.0006*alpha_hessian_inv*alpha_gradient;
rho -= 0.01*rho_hessian_inv*rho_gradient;
++counter;
}
}
void FootstepDisplay::processMessage(const jsk_footstep_msgs::FootstepArray::ConstPtr& msg)
{
if (!validateFloats(*msg)) {
setStatus(rviz::StatusProperty::Error, "Topic", "message contained invalid floating point values (nans or infs)");
return;
}
latest_footstep_ = msg;
Ogre::Quaternion orientation;
Ogre::Vector3 position;
if(!context_->getFrameManager()->getTransform( msg->header.frame_id,
msg->header.stamp,
position, orientation)) {
ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
return;
}
// check thhe length of the shapes_
allocateCubes(msg->footsteps.size());
allocateTexts(msg->footsteps.size());
line_->clear();
line_->setLineWidth(0.01);
line_->setNumLines(1);
line_->setMaxPointsPerLine(1024);
for (size_t i = 0; i < msg->footsteps.size(); i++)
{
ShapePtr shape = shapes_[i];
rviz::MovableText* text = texts_[i];
Ogre::SceneNode* node = text_nodes_[i];
jsk_footstep_msgs::Footstep footstep = msg->footsteps[i];
Ogre::Vector3 step_position;
Ogre::Quaternion step_quaternion;
if( !context_->getFrameManager()->transform( msg->header, footstep.pose,
step_position,
step_quaternion ))
{
ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
qPrintable( getName() ), msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
return;
}
shape->setPosition(step_position);
shape->setOrientation(step_quaternion);
// size of shape
Ogre::Vector3 scale;
if (footstep.dimensions.x == 0 &&
footstep.dimensions.y == 0 &&
footstep.dimensions.z == 0) {
scale[0] = depth_;
scale[1] = width_;
scale[2] = height_;
}
else {
scale[0] = footstep.dimensions.x;
scale[1] = footstep.dimensions.y;
scale[2] = footstep.dimensions.z;
}
shape->setScale(scale);
// update the size of text
if (footstep.leg == jsk_footstep_msgs::Footstep::LEFT) {
text->setCaption("L");
}
else if (footstep.leg == jsk_footstep_msgs::Footstep::RIGHT) {
text->setCaption("R");
}
else {
text->setCaption("unknown");
}
text->setCharacterHeight(estimateTextSize(footstep));
node->setPosition(step_position);
node->setOrientation(step_quaternion);
text->setVisible(show_name_); // TODO
line_->addPoint(step_position);
}
//updateFootstepSize();
updateAlpha();
context_->queueRender();
}
void FootstepDisplay::processMessage(const jsk_footstep_msgs::FootstepArray::ConstPtr& msg)
{
if (!validateFloats(*msg)) {
setStatus(rviz::StatusProperty::Error, "Topic", "message contained invalid floating point values (nans or infs)");
return;
}
latest_footstep_ = msg;
Ogre::Quaternion orientation;
Ogre::Vector3 position;
if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
msg->header.stamp,
position, orientation ))
{
ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
return;
}
// check thhe length of the shapes_
if (msg->footsteps.size() > shapes_.size())
{
// need to allocate
for (size_t i = shapes_.size(); i < msg->footsteps.size(); i++)
{
ShapePtr shape;
shape.reset(new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(),
scene_node_));
shapes_.push_back(shape);
}
}
else if (msg->footsteps.size() < shapes_.size())
{
// need to remove
shapes_.resize(msg->footsteps.size());
}
line_->clear();
line_->setLineWidth(0.01);
line_->setNumLines(1);
line_->setMaxPointsPerLine(1024);
for (size_t i = 0; i < msg->footsteps.size(); i++)
{
ShapePtr shape = shapes_[i];
jsk_footstep_msgs::Footstep footstep = msg->footsteps[i];
Ogre::Vector3 step_position;
Ogre::Quaternion step_quaternion;
if( !context_->getFrameManager()->transform( msg->header, footstep.pose,
step_position,
step_quaternion ))
{
ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
qPrintable( getName() ), msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
return;
}
shape->setPosition(step_position);
shape->setOrientation(step_quaternion);
line_->addPoint(step_position);
}
updateFootstepSize();
updateAlpha();
context_->queueRender();
}
void Draw::visit(ShapePtr shape)
{
ShapeRendererStatePtr renderer_state = shape->getRendererState(renderer);
D2DShapeRendererStatePtr shape_renderer = dynamic_pointer_cast<D2DShapeRendererState>(renderer_state);
shape_renderer->draw();
}