michalpelka

# This example assumes we have a mesh object selected

import bpy
import bmesh
import numpy as np

def getCentroid(A):
    return np.mean(A,axis=0)

def getSE3FromPointcloud(A):

michalpelka

import matplotlib.pyplot as plt
import numpy as np

def gaussian(x, mu, var):
    return 1.0/np.sqrt(2.0*np.pi*var) * np.exp(-0.5*(mu-x)*(1.0/var)*(mu-x))


X = np.array([[0],[0]])
X_sigma = np.identity(2)*0.1

michalpelka

import serial
import struct

def crc8_dvb_s2(crc, ch):
    """CRC for MSPV2
    *copied from inav-configurator
    """
    crc ^= ch
    for _ in range(8):
        if (crc & 0x80):

michalpelka

import numpy as np



def quat_len(q):
    return np.sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3])

def quat_norm(q):
    d = quat_len(q)
    if (d==0): return np.array([0,0,0,0]) 

michalpelka

import numpy as np
import matplotlib.pyplot as plt


trajectory_gt = np.array([[-0.3867186903953552, -0.3169364035129547, 0.0],[-0.23194852471351624, -0.1902160346508026, 0.0],[-0.07601896673440933, -0.06425309926271439, 0.0],[0.08405143022537231, 0.05900496616959572, 0.0],[0.2532316744327545, 0.17631226778030396, 0.0],[0.43847835063934326, 0.28312453627586365, 0.0],[0.6483954787254333, 0.3736981153488159, 0.0],[0.889494001865387, 0.44217759370803833, 0.0],[1.1624500751495361, 0.4836835265159607, 0.0],[1.4617658853530884, 0.49441155791282654, 0.0],[1.775768756866455, 0.47163209319114685, 0.0],[2.0870449542999268, 0.41373151540756226, 0.0],[2.3772077560424805, 0.3206641376018524, 0.0],[2.631664514541626, 0.194403275847435, 0.0],[2.8400490283966064, 0.03898302838206291, 0.0],[2.9962236881256104, -0.13950233161449432, 0.0],[3.0981533527374268, -0.3330237865447998, 0.0],[3.1465418338775635, -0.533002495765686, 0.0],[3.143460988998413, -0.7316943407058716, 0.0],[3.092233180999756, -0.9223166108131409, 0.0],[2.9974

michalpelka

//
// Created by michal on 14.01.2022.
//


#include <ceres/loss_function.h>
#include <fstream>

int main()
{

michalpelka

Eigen::Affine3d m3d_utils::orthogonize(const Eigen::Affine3d& p )
{
    Eigen::Matrix4d ret = p.matrix();
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(ret.block<3,3>(0,0), Eigen::ComputeFullU | Eigen::ComputeFullV);
    double d = (svd.matrixU() * svd.matrixV().transpose()).determinant();
    Eigen::Matrix3d diag = Eigen::Matrix3d::Identity() * d;
    ret.block<3,3>(0,0) = svd.matrixU() * diag * svd.matrixV().transpose();
    return Eigen::Affine3d (ret);
//    Eigen::Affine3d pose_orto(Eigen::Affine3d::Identity());
//    Eigen::Quaterniond q1(p.matrix().block<3,3>(0,0)); q1.normalize();

michalpelka

process[imu_an-1]: started with pid [10260]
[ INFO] [1631230172.663555986]: Loaded imu_topic: /imu/data
[ INFO] [1631230172.664508247]: Loaded imu_name: xsens
[ INFO] [1631230172.664728939]: Loaded data_save_path: /home/michal/catkin_ws/src/imu_utils/data/
[ INFO] [1631230172.664927664]: Loaded max_time_min: 200
[ INFO] [1631230172.665163926]: Loaded max_cluster: 100
gyr x  num of Cluster 100
gyr y  num of Cluster 100
gyr z  num of Cluster 100
acc x  num of Cluster 100

michalpelka

#include <iostream>
#include <ros/ros.h>
#include <ros/subscriber.h>
#include <ros/publisher.h>
#include <pcl_ros/point_cloud.h>
std::unique_ptr<ros::Publisher> publisher;
void cloud_callback (const pcl::PointCloud<pcl::PointXYZI>& cloud_msg){
    ROS_INFO("inside callback");
    pcl::PointCloud<pcl::PointXYZI> cloud_out  = cloud_msg;
    publisher->publish(cloud_out);

michalpelka

# This example assumes we have a mesh object selected

import bpy
import bmesh
import numpy as np


obj = bpy.context.active_object
P = []
A = []