I added two .eval() just in case. I got no compilation error, and no run time warning. Just segfault.
Thanks for helping me to fix this.
Test:
#include <Eigen/Eigen>
#include <iostream>
using namespace Eigen;
int main() {
Matrix<float, Dynamic, Dynamic> mat_b;
Matrix<float, Dynamic, Dynamic> mat_c;
mat_b << 1.0, 0.0, 0.5, 0.5,
0.0, 1.0, 0.5, 0.5,
1.0, 0.0, 1.0, 0.0,
0.0, 1.0, 0.0, 1.0;
mat_c << 0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
1.0, 0.0, 1.0, 0.0, 0.0, 0.0,
1.0, 0.0, 0.0, 1.0, 0.0, 0.0;
std::cout << (mat_b.transpose().eval() * mat_c).eval() << "\n";
}
Result:
Segmentation fault (core dumped)
As stated in documentatipon
The comma initializer
Eigen offers a comma initializer syntax which allows the user to easily set all the coefficients of a matrix, vector or array. Simply list the coefficients, starting at the top-left corner and moving from left to right and from the top to the bottom. The size of the object needs to be specified beforehand. If you list too few or too many coefficients, Eigen will complain.
emphasis is mine. If you expect that Matrix ctor would deduce size from your formatting, that simply not possible in C++. Looks like you created 16x1 and 24x1 matrix and then try to multiply 1x16 (transposed first one) to 24x1 which is not legal.
Related
I appreciate I am being somewhat vague about what is exactly my issue, but I think that the fundamental question is clear. Please bear with me for a moment.
In brief, I have a static constexpr array of points which are used to find certain bounds I need to use. These bounds depend only on the array, so they can be precomputed. However, we want to be able to change these points and it is a pain to go and change every value every time we try to test something.
For example, let's say that I have the following setup:
The static constexpr array is
static constexpr double CHECK_POINTS[7] = { -1.5, -1.0, -0.5, 0.0, -0.5, 1.0, 1.5 };
and then in a function I'm calling, I have the following block of code:
std::vector<double> bounds = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
for(int i=0; i<bounds.size(); i++)
{
bounds[i] = std::exp(CHECK_POINTS[i]);
}
Clearly, the values of bounds can be computed during compilation. Is there anyway I can make gcc do that?
EDIT: The vector in my code block is not essential, an array will do.
constexpt std::vector does not work here, but you can use std::array.
std::exp is not constexpr
so you need to find constexpr alternatives
it would work in gcc as an extension.
static constexpr double CHECK_POINTS[7] = { -1.5, -1.0, -0.5, 0.0, -0.5, 1.0, 1.5 };
static constexpr auto vec = [](){
std::array bounds = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
for(int i=0; i<bounds.size(); i++)
{
bounds[i] = std::exp(CHECK_POINTS[i]);
}
return bounds;
}();
it's would compile fine with gcc https://godbolt.org/z/x5a9q9M1d
(constexpr std::exp is an gcc extension, thanks to #phuclv to point out)
I have velocity data from the sensor, but somehow it is not in the robot body frame. It's been in a world frame, so it's not correct. So I need to transform it in the robot body frame as that how should be in the correct way. So I need a listener that will listen for transformations to be broadcast. Once broadcast I can use the listener to get the transform and apply it to your data.
I made a ROS nodes for broadcast and listener. But is still not correct the velocity. Here is the sensor data that include the linear velocity and its looks like this:
header:
seq: 182
stamp:
secs: 39
nsecs: 244000000
frame_id: "world"
child_frame_id: "rexrov2/base_link"
pose:
pose:
position:
x: -0.0462157448086
y: -0.0175201465699
z: -18.7747349396
orientation:
x: 0.00029722877228
y: -0.000159403004117
z: -0.00433868890864
w: 0.999990530967
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist:
twist:
linear:
x: -0.00672636768741
y: -0.00264084973057
z: 0.204243325029
angular:
x: -7.4723382137e-05
y: -0.00310117164775
z: -0.00108493763869
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Then the robot frame is rexrov2/base_linkand the sensor is rexrov2/pose_sensor_link_default. The frame broadcaster is this one
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf2/LinearMath/Quaternion.h>
void poseCallback(const nav_msgs::Odometry::ConstPtr& msg){
static tf2_ros::TransformBroadcaster br;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "world";
transformStamped.child_frame_id = "rexrov2/base_link";
transformStamped.transform.translation.x = msg->pose.pose.position.x;
transformStamped.transform.translation.y = msg->pose.pose.position.y;
transformStamped.transform.translation.z = msg->pose.pose.position.z;
transformStamped.transform.rotation.x = msg->pose.pose.orientation.x;
transformStamped.transform.rotation.y = msg->pose.pose.orientation.y;
transformStamped.transform.rotation.z = msg->pose.pose.orientation.z;
transformStamped.transform.rotation.w = msg->pose.pose.orientation.w;
br.sendTransform(transformStamped);
}
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf2_broadcaster");
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe("rexrov2/pose_gt", 10, &poseCallback);
ros::spin();
return 0;
};
And the listener node is this one
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf2_listener");
ros::NodeHandle node;
ros::Publisher robot_vel =node.advertise<geometry_msgs::Twist>("robot/vel", 10);
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10.0);
while (node.ok()){
geometry_msgs::TransformStamped transformStamped;
try{
transformStamped = tfBuffer.lookupTransform("rexrov2/base_link", "world", ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
geometry_msgs::Twist vel_msg;
vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2));
robot_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
The sensor data that is not in the correct frame is published under rexrov2/pose_gt and the robot base link is rexrov2/base_link. The warnings I'm getting are:
[ WARN] [1635767328.332529320, 14554.020000000]: "rexrov2/pose_gt" passed to lookupTransform argument target_frame does not exist.
Any help?Thanks
The warning you are getting is because you don't have the TF frames connected. Please check them out with
rosrun rqt_tf_tree rqt_tf_tree
It seems you are not publishing anywhere the transform between "rexrov2/pose_gt" and "rexrov2/base_link". So run this command in a separate terminal to publish the transform so your node can get it:
rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 1 rexrov2/pose_gt resrov2/base_link
If you stop receiving the warning running that command, it means that you need to include it in a launch file along with your node in order to run all at the same time
<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_gt_broadcaster" args="1 0 0 0 0 0 1 resrov2/base_link rexrov2/pose_gt" />
</launch>
And replace the transformation 1 0 0 0 0 0 1 (x y z qx qy qz qw) by the correct one
In addition to this, I don't see any subscriber in your code, but I think it's because you're using the node to do preliminar tests with the ROS integration
I am working on vtk volume rendering of mammography. I have a 50 DICOM slices in folder to construct volume. Here I need to give vtkColorTransferFunction and vtkPiecewiseFunction to set RGB color and scalar opacity.
I am not getting exact values of color and opacity with respect to mammo images (breast images). I need values for color and opacity with respect to breast x-ray images.
Any suggestions will be helpful.
vtkGPUVolumeRayCastMapper *volumeGPUmapper =
vtkGPUVolumeRayCastMapper::New();
volumeGPUmapper->SetInputConnection(clip->GetOutputPort());
// RGB and alpha funcions
double skinOnBlueMap[28][5] =
{
{0, 0.987853825092316, 1.0, 1.0, 0.9},
{10000, 0.987853825092316, 1.0, 1.0, 0.9},
{20000, 0.987853825092316, 1.0, 1.0, 1.0},
{30000, 0.987853825092316, 1.0, 1.0, 1.0},
{40000, 0.0, 0.0, 0.0, 1.0},
{50000, 1.0, 0.0, 0.0, 1.0},
{60000, 1.0, 0.999206542968750, 0.0, 1.0},
{70000, 1.0, 1.0, 1.0, 1.0}
};
vtkSmartPointer<vtkPiecewiseFunction> alphaChannelFunc = vtkSmartPointer<vtkPiecewiseFunction>::New();
vtkSmartPointer<vtkColorTransferFunction> colorFunc = vtkSmartPointer<vtkColorTransferFunction>::New();
for(int i = 0; i < sizeof(skinOnBlueMap)/(5*sizeof(double)); i++)
{
colorFunc->AddRGBPoint(skinOnBlueMap[i][0], skinOnBlueMap[i][1], skinOnBlueMap[i][2], skinOnBlueMap[i][3]);
alphaChannelFunc->AddPoint(skinOnBlueMap[i][0], skinOnBlueMap[i][4]);
}
vtkSmartPointer<vtkVolumeProperty> volumeProperty = vtkSmartPointer<vtkVolumeProperty>::New();
volumeProperty->SetColor(colorFunc);
volumeProperty->SetInterpolationTypeToLinear();
volumeProperty->SetScalarOpacity(alphaChannelFunc);
vtkSmartPointer<vtkVolume> VTKvolume = vtkSmartPointer<vtkVolume>::New();
VTKvolume->SetMapper(volumeGPUmapper);
VTKvolume->SetProperty(volumeProperty);
Those scalar values seem strange to me. It could be the reason your results don't look correct. But to be sure we need the image stack and rendered result.
The scalar values are not even inside the range of the unsigned short type, that is generally used for the voxels.
I'm trying to use evaluators to create a plane:
void Plane::draw(float texS, float texT)
{
float div = v.at(0);
GLfloat ctrlpoints[4][3] = {
{-0.5, 0.0, 0.5}, {-0.5, 0.0 ,-0.5},
{0.5, 0.0, 0.5}, {0.5, 0.0, -0.5}};
GLfloat texturepoints[4][2] = {
{0.0, 0.0}, {0.0, 1.0/texT},
{1.0/texS, 0.0}, {1.0/texS, 1.0/texT}};
glMap2f(GL_MAP2_VERTEX_3, 0.0, 1.0, 3, 2, 0.0, 1.0, 2 * 3, 2, &ctrlpoints[0][0]);
glMap2f(GL_MAP2_TEXTURE_COORD_2, 0.0, 1.0, 2, 2, 0.0, 1.0, 2 * 2, 2, &texturepoints[0][0]);
glEnable(GL_MAP2_VERTEX_3);
glEnable(GL_MAP2_TEXTURE_COORD_2);
glEnable(GL_AUTO_NORMAL);
glMapGrid2f(div, 0.0, 1.0, div, 0.0, 1.0);
glEvalMesh2(GL_FILL,0, div, 0, div);
}
It displays the plane correctly, it gives me a 50*50 grid, for example, and the texture I apply to it is also displayed properly. However, if I try to apply a golden appearance to it, it just gives me a dull brown color.
I know I can get what I want by creating a rectangle with with quad or triangle strip, but the point here is to use evaluators.
One answer I found said that evaluators calculate normals automatically with the enabling of GL_AUTO_NORMAL, and that that was the only necessary instruction. But even then, the author of the question couldn't do what he wanted.
And I do have GL_NORMALIZE enabled in the initialization.
To me this should just work, so the fact it does not, almost certainly means I am the one in the wrong. Even though in principle a Transform< double, 3, Affine > is the same as a Matrix< double, 4, 4 >, they cannot be used together sensibly:
Affine3d rotMat( AngleAxisd( 45.0, ( Vector3d() << 0.0, 1.0, 0.0 ).finished() ) );
Matrix4d m;
m << 1.0, 0.0, 0.0, 6.0,
0.0, 1.0, 0.0, 6.0,
0.0, 0.0, 1.0, 6.0,
0.0, 0.0, 0.0, 1.0;
m = m * rotMat;
Results in a 'no match for operator=' error on the last line, and the in-place multiplication operator results in the same, trying to initialise a Matrix4d with Affine3d does not work either. Does anybody know how to actually use the Transform class in any useful way?
Thanks,
Cam
Just write:
m = m * rotMat.matrix();
I don't know if it is an oversight that Eigen doesn't define this multiplication implicitly or if it might interfere with other use cases of the library.