I want to operate on a vector object directly using OpenCVs matrix functions.
What I am currently doing is using memcpy to copy the data to a matrix, but I would really like operate on it directly because I've very concerned about performance. The following is my current implementation that works as expected.
Working Implementation
void TrajectorySmoother::SmoothTrajectoryOnce(
std::vector<navstack_msgs::location>& path,
const unsigned int& end_remove)
{
cv::Mat_<double> matrix_orig = cv::Mat(path.size(), sizeof(navstack_msgs::location)/sizeof(double), CV_64F);
memcpy(matrix_orig.data, path.data(), path.size()*sizeof(navstack_msgs::location));
ROS_INFO_STREAM("mat: " << matrix_orig);
}
I tried using static_cast
and reinterpret_cast
but they don't seem to work so I tried casting a pointer to the data as a pointer to an openCV matrix.
Pointer Casting Approach
void TrajectorySmoother::SmoothTrajectoryOnce(
std::vector<navstack_msgs::location>& path,
const unsigned int& end_remove)
{
cv::Mat_<double> matrix_orig = cv::Mat(path.size(), sizeof(navstack_msgs::location)/sizeof(double), CV_64F);
cv::Mat_<double> * matrix_ptr = reinterpret_cast<cv::Mat_<double> *>(path&);
ROS_INFO_STREAM("mat: " << (*matrix_ptr));
}
The output from this is a stream of semicolons. I thought it might need to be reshaped, but I get an error that it is not continuous.