Related
I'm trying to Implement linear regression in python using the following gradient decent formulas (Notice that these formulas are after partial derive)
slope
y_intercept
but the code keeps giving me wearied results ,I think (I'm not sure) that the error is in the gradient_descent function
import numpy as np
class LinearRegression:
def __init__(self , x:np.ndarray ,y:np.ndarray):
self.x = x
self.m = len(x)
self.y = y
def calculate_predictions(self ,slope:int , y_intercept:int) -> np.ndarray: # Calculate y hat.
predictions = []
for x in self.x:
predictions.append(slope * x + y_intercept)
return predictions
def calculate_error_cost(self , y_hat:np.ndarray) -> int:
error_valuse = []
for i in range(self.m):
error_valuse.append((y_hat[i] - self.y[i] )** 2)
error = (1/(2*self.m)) * sum(error_valuse)
return error
def gradient_descent(self):
costs = []
# initialization values
temp_w = 0
temp_b = 0
a = 0.001 # Learning rate
while True:
y_hat = self.calculate_predictions(slope=temp_w , y_intercept= temp_b)
sum_w = 0
sum_b = 0
for i in range(len(self.x)):
sum_w += (y_hat[i] - self.y[i] ) * self.x[i]
sum_b += (y_hat[i] - self.y[i] )
w = temp_w - a * ((1/self.m) *sum_w)
b = temp_b - a * ((1/self.m) *sum_b)
temp_w = w
temp_b = b
costs.append(self.calculate_error_cost(y_hat))
try:
if costs[-1] > costs[-2]: # If global minimum reached
return [w,b]
except IndexError:
pass
I Used this dataset:-
https://www.kaggle.com/datasets/tanuprabhu/linear-regression-dataset?resource=download
after downloading it like this:
import pandas
p = pandas.read_csv('linear_regression_dataset.csv')
l = LinearRegression(x= p['X'] , y= p['Y'])
print(l.gradient_descent())
But It's giving me [-568.1905905426412, -2.833321633515304] Which is decently not accurate.
I want to implement the algorithm not using external modules like scikit-learn for learning purposes.
I tested the calculate_error_cost function and it worked as expected and I don't think that there is an error in the calculate_predictions function
One small problem you have is that you are returning the last values of w and b, when you should be returning the second-to-last parameters (because they yield a lower cost). This should not really matter that much... unless your learning rate is too high and you are immediately getting a higher value for the cost function on the second iteration. This I believe is your real problem, judging from the dataset you shared.
The algorithm does work on the dataset, but you need to change the learning rate. I ran it in the example below and it gave the result shown in the image. One caveat is that I added a limit to the iterations to avoid the algorithm from taking too long (and only marginally improving the result).
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
class LinearRegression:
def __init__(self , x:np.ndarray ,y:np.ndarray):
self.x = x
self.m = len(x)
self.y = y
def calculate_predictions(self ,slope:int , y_intercept:int) -> np.ndarray: # Calculate y hat.
predictions = []
for x in self.x:
predictions.append(slope * x + y_intercept)
return predictions
def calculate_error_cost(self , y_hat:np.ndarray) -> int:
error_valuse = []
for i in range(self.m):
error_valuse.append((y_hat[i] - self.y[i] )** 2)
error = (1/(2*self.m)) * sum(error_valuse)
return error
def gradient_descent(self):
costs = []
# initialization values
temp_w = 0
temp_b = 0
iteration = 0
a = 0.00001 # Learning rate
while iteration < 1000:
y_hat = self.calculate_predictions(slope=temp_w , y_intercept= temp_b)
sum_w = 0
sum_b = 0
for i in range(len(self.x)):
sum_w += (y_hat[i] - self.y[i] ) * self.x[i]
sum_b += (y_hat[i] - self.y[i] )
w = temp_w - a * ((1/self.m) *sum_w)
b = temp_b - a * ((1/self.m) *sum_b)
costs.append(self.calculate_error_cost(y_hat))
try:
if costs[-1] > costs[-2]: # If global minimum reached
print(costs)
return [temp_w,temp_b]
except IndexError:
pass
temp_w = w
temp_b = b
iteration += 1
print(iteration)
return [temp_w,temp_b]
p = pd.read_csv('linear_regression_dataset.csv')
x_data = p['X']
y_data = p['Y']
lin_reg = LinearRegression(x_data, y_data)
y_hat = lin_reg.calculate_predictions(*lin_reg.gradient_descent())
fig = plt.figure()
plt.plot(x_data, y_data, 'r.', label='Data')
plt.plot(x_data, y_hat, 'b-', label='Linear Regression')
plt.xlabel('x')
plt.ylabel('y')
plt.legend()
plt.show()
I'm working on a simulation of a Soft Robot using the Piecewise Constant Curvature (PCC) assumption and representing each PCC segment with an Augmented Rigid Body Model (ARBM). For this I first would like to implement a manual slider for curvature control, i.e., a slider where I can input the two curvature parameters (Theta and Phi) and after mapping it to the ARBM via some known map m(Theta, Phi) showcase the robot in meshcat.
I'm already displaying the ARBM, however, am struggling to get the slider to run. As a result, I'd like to have something similar to the Let's get you a Robot notebook from the Manipulation course. So ideally a kinematic simulation in which I can set the curvatures to show the resulting ARBM configuration.
As of right now, my approach is the following:
Create a Custom Slider Class that is based on LeafSystem that creates two slides
Create a Transformation System based on a VectorSystem that applies the map m(Theta, Phi) to the output of the sliders yielding the ARBM joint states on its output
Set the Robot Joints to this State (This is where I'm struggling)
The problem seems to be that I cannot connect the desired output directly to the Joint positions in the same way that, e.g., the Joint sliders are. Is there a way to do this or should I follow a different approach?
See below for the code of the individual instances:
CurvatureSlider.py:
from dataclasses import dataclass
import numpy as np
from pydrake.systems.framework import LeafSystem
class CurvatureSliders(LeafSystem):
#dataclass
class SliderDefault:
"""Default values for the meshcat sliders."""
name: str
"""The name that is used to add / query values from."""
default: float
"""The initial value of the slider."""
_THETA = SliderDefault("Theta", 0.0)
_PHI = SliderDefault("Phi", 0.0)
def __init__(self, meshcat):
"""
#param meshcat The already created pydrake.geometry.Meshcat instance.
"""
LeafSystem.__init__(self)
self.DeclareVectorOutputPort("theta_phi", 2, self.DoCalcOutput)
self.meshcat = meshcat
# Curvature Control Sliders
self.meshcat.AddSlider(
name=self._THETA.name, min=-2.0 * np.pi,
max=2.0 * np.pi, step=0.01,
value=self._THETA.default)
self.meshcat.AddSlider(
name=self._PHI.name, min=-2.0 * np.pi,
max=2.0 * np.pi, step=0.01,
value=self._PHI.default)
def SetConfiguration(self, config: tuple):
"""
#param pose Tuple of floats that is ordered (Theta, Phi)
"""
assert(len(config) == 2)
self.meshcat.SetSliderValue(self._THETA.name, config[0])
self.meshcat.SetSliderValue(self._PHI.name, config[1])
def DoCalcOutput(self, context, output):
theta = self.meshcat.GetSliderValue(self._THETA.name)
phi = self.meshcat.GetSliderValue(self._PHI.name)
output.SetAtIndex(0, theta)
output.SetAtIndex(1, phi)
CC2ARBM.py:
import numpy as np
from pydrake.systems.framework import VectorSystem
class CC2ARBM(VectorSystem):
def __init__(self, L: float):
"""
#param L The length of the segment.
"""
VectorSystem.__init__(self, 2, 10)
# Define length of the segment
self._L = L
def DoCalcVectorOutput(self, context, u, x, y):
# Extract Input
theta = u[0]
phi = u[1]
# Compute ARBM equivalent configuration
b = 0.5 * self._L
eta = 0
if not theta == 0:
b = self._L / theta * np.sqrt(1
+ 4 * np.sin(0.5 * theta) / theta
* (np.sin(0.5 * theta) / theta)
- np.cos(0.5 * theta))
eta = np.arccos(1 / b * self._L / theta * np.sin(0.5 * theta))
print("Computed ARBM Joint Position")
# Aggreate Output
y.SetAtIndex(0, phi)
y.SetAtIndex(1, 0.5 * theta - eta)
y.SetAtIndex(2, b)
y.SetAtIndex(3, eta)
y.SetAtIndex(4, - phi)
y.SetAtIndex(5, phi)
y.SetAtIndex(6, eta)
y.SetAtIndex(7, b)
y.SetAtIndex(8, 0.5 * theta - eta)
y.SetAtIndex(9, - phi)
Main.py:
import sys
import time
import matplotlib.pyplot as plt
import numpy as np
from CurvatureSliders import CurvatureSliders
from CC2ARBM import CC2ARBM
from pydrake.geometry import (
MeshcatVisualizerCpp,
MeshcatVisualizerParams,
Role,
StartMeshcat
)
from pydrake.math import (
RigidTransform,
RotationMatrix
)
from pydrake.multibody.meshcat import JointSliders
from pydrake.multibody.tree import FixedOffsetFrame
from pydrake.multibody.parsing import Parser
from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.primitives import LogVectorOutput
def do_main(teleop: bool,
target_realtime_rate: float,
simulation_time: float,
max_time_step: float,
description_path: str) -> None:
# Start the visualizer.
# The cell will output an HTTP link after the execution.
# Click the link and a MeshCat tab should appear in your browser.
meshcat = StartMeshcat()
# Reset Meshcat Simulator
meshcat.Delete()
meshcat.DeleteAddedControls()
# Init the Diagram Builder
builder = DiagramBuilder()
# Note: the time_step here is chosen arbitrarily.
plant, scene_graph = AddMultibodyPlantSceneGraph(
builder, time_step=max_time_step)
# Load the files into the plant/scene_graph.
parser = Parser(plant)
# L - Mount
mount = parser.AddModelFromFile("../mount.sdf")
# Robot
model = parser.AddModelFromFile(description_path)
# Create an offset frame located half the link height above the origin
base_frame = plant.GetFrameByName("mount_base")
offset_frame = plant.AddFrame(
frame=FixedOffsetFrame(
name="offset_frame",
P=plant.world_frame(),
X_PF=RigidTransform(
R=RotationMatrix.Identity(),
p=np.array([0, 0, 0.5])
),
model_instance=None)
)
# Weld the base link to the offset frame
plant.WeldFrames(offset_frame, base_frame)
# Weld the robot base to the L-Mount
robot_base_frame = plant.GetFrameByName("robot_base")
robot_mounting_frame = plant.GetFrameByName("robot_location")
plant.WeldFrames(robot_mounting_frame, robot_base_frame)
# Finalize Plant
plant.Finalize()
#############################################################
# Post Plant Finalization Code #
#############################################################
# Set the Default states of the Joints
for i in plant.GetJointIndices(model):
j = plant.get_joint(i)
if j.num_positions() == 1:
j.set_default_positions([-0.2])
# Make the control inputs of the model an input to the diagram.
builder.ExportInput(plant.get_actuation_input_port())
# Add two visualizers, one to publish the "visual" geometry, and one to
# publish the "collision" geometry.
visual = MeshcatVisualizerCpp.AddToBuilder(
builder, scene_graph, meshcat,
MeshcatVisualizerParams(
role=Role.kPerception, prefix="visual")
)
collision = MeshcatVisualizerCpp.AddToBuilder(
builder, scene_graph, meshcat,
MeshcatVisualizerParams(role=Role.kProximity, prefix="collision"))
# Disable the collision geometry at the start; it can be enabled by the
# checkbox in the meshcat controls.
meshcat.SetProperty("collision", "visible", False)
# Setup Loggers
plant_logger = LogVectorOutput(plant.get_state_output_port(), builder)
# Joint Sliders Work like this
# sliders = builder.AddSystem(JointSliders(meshcat, plant))
# Add Curvature Sliders
curv_sliders = builder.AddSystem(CurvatureSliders(meshcat))
cc2arbm = builder.AddSystem(CC2ARBM(0.2))
# Connect the Sliders to the transformation block
builder.Connect(curv_sliders.get_output_port(0),
cc2arbm.get_input_port(0))
# Build the diagram
diagram = builder.Build()
# Start runnin the teleoperation
# Start Running the Slider similar to the Joint slider
# e.g. sliders.Run(diagram)
if __name__ == '__main__':
if len(sys.argv) <= 1:
do_main(target_realtime_rate=1,
simulation_time=10.0,
max_time_step=1.0e-3,
description_path="single_cc_segment.sdf")
else:
do_main(target_realtime_rate=float(sys.argv[1]),
simulation_time=float(sys.argv[2]),
max_time_step=float(sys.argv[3]),
description_path=sys.argv[4])
There are a few moving parts here. First, you say "kinematic simulation", but the demonstration in "Let's get you a robot" does not simulate (physics), it only visualizes the kinematics as set by the sliders. Assuming that is sufficient for your goal, then you could pass a callback into your sliders.Run method (as I do in the notebook corresponding to this chapter), and I believe that if you call plant.SetPositions in that callback, it should work?
I eventually ran the custom slider using a reimplementation of JointSlider.Run(...). For me, the following was sufficient (Two planar constant curvature segments, represented by 4 rigid joints each):
from dataclasses import dataclass
import numpy as np
import functools
import operator
import logging
import time
from typing import List, Tuple
from pydrake.systems.framework import LeafSystem
class CurvatureSliders(LeafSystem):
#dataclass
class SliderDefault:
"""Default values for the meshcat sliders."""
name: str
"""The name that is used to add / query values from."""
default: float
"""The initial value of the slider."""
_Q1 = SliderDefault("q1", 0.0)
_Q2 = SliderDefault("q2", 0.0)
def __init__(self, meshcat, plant, L=0.2):
"""
#param meshcat The already created pydrake.geometry.Meshcat instance.
#param plant The plant the sliders are connected to
#param L the restlength of the segment
"""
# Call Super Constructor
LeafSystem.__init__(self)
# Declare System Output
output = self.DeclareVectorOutputPort(
"q1_q2", 2, self.DoCalcOutput)
output.disable_caching_by_default()
# Init Class Variables
self._meshcat = meshcat
self._plant = plant
self._L = L
# Curvature Control Sliders
self._meshcat.AddSlider(
name=self._Q1.name, min=-2.0 * np.pi,
max=2.0 * np.pi, step=0.1,
value=self._Q1.default)
self._meshcat.AddSlider(
name=self._Q2.name, min=-2.0 * np.pi,
max=2.0 * np.pi, step=0.1,
value=self._Q2.default)
def SetConfiguration(self, q: Tuple):
"""
#param q configuration for each CC segment descriped by (q1, q2)
"""
self._meshcat.SetSliderValue(self._Q1.name, q[0])
self._meshcat.SetSliderValue(self._Q2.name, q[1])
def CC2AGRB(self, q: Tuple) -> List[float]:
# Extract input
q1 = q[0]
q2 = q[1]
# Compute ARBM equivalent configuration
config1 = [0, 0.5 * self._L, 0.5 * self._L, 0]
config2 = [0, 0.5 * self._L, 0.5 * self._L, 0]
if q1 != 0:
config1 = [
0.5 * q1,
self._L * np.sin(0.5 * q1) / q1,
self._L * np.sin(0.5 * q1) / q1,
0.5 * q1
]
if q2 != 0:
config2 = [
0.5 * q2,
self._L * np.sin(0.5 * q2) / q2,
self._L * np.sin(0.5 * q2) / q2,
0.5 * q2
]
return functools.reduce(operator.iconcat, [config1, config2], [])
def DoCalcOutput(self, context, output):
q1 = self._meshcat.GetSliderValue(self._Q1.name)
q2 = self._meshcat.GetSliderValue(self._Q2.name)
output.SetAtIndex(0, q1)
output.SetAtIndex(1, q2)
def Run(self, diagram, timeout=1e5):
# Get all the contextes
root_context = diagram.CreateDefaultContext()
sliders_context = self.GetMyContextFromRoot(root_context)
plant_context = self._plant.GetMyMutableContextFromRoot(root_context)
# Add Stop Button
kButtonName = "Stop Curvature Sliders"
logging.info("Press the '{}' button in Meshcat to continue.",
kButtonName)
self._meshcat.AddButton(kButtonName)
# Greb current time to implement the timeout
t0 = time.time()
# Loop until the button is clicked, or
# the timeout (when given) is reached.
diagram.Publish(root_context)
while self._meshcat.GetButtonClicks(kButtonName) < 1:
# Break out of loop if timeout elapsed
elapsed_t = time.time() - t0
if elapsed_t >= timeout:
break
# If the sliders have not changed, avoid invalidating the context.
old_positions = self._plant.GetPositions(plant_context)
new_positions = self.CC2AGRB(
self.get_output_port().Eval(sliders_context))
if (np.abs(new_positions - old_positions) < 0.001).all():
time.sleep(0.01)
continue
# Publish the new positions.
self._plant.SetPositions(plant_context, new_positions)
diagram.Publish(root_context)
And then just add them using the builder and calling the Run() function.
I am using MD49 Motor Drive with its motors
https://www.robot-electronics.co.uk/htm/md49tech.htm
http://wiki.ros.org/md49_base_controller
How to subscribe (encoder_l and encoder_r) from md49_base_controller package and publish (vx , vy ,and vth ) as a form odom (nav_msgs/Odometry) ?
There are two problem:
1-The first is that the encoders outputs are not correct "the package needs to be modified.
2-The second is the I want to create a package that subscribe the right and left wheels encoder counts (encoder_l and encoder_r) and publish (vx , vy ,and vth) as a form odom (nav_msgs/Odometry) to be compatable wth imu MPU9250
http://wiki.ros.org/robot_pose_ekf
The proposed package is:
1- We have to convert (encoder_l and encoder_r) to (RPM_l and RPM_r) as follow:
if (speed_l>128){newposition1 = encoder_l;}
else if (speed_l<128){ newposition1 = 0xFFFFFFFF-encoder_l;}
else if (speed_l==128) {newposition1=0;}
newtime1 = millis();
RPM_l = ((newposition1-oldposition1)*1000*60)/((newtime1-oldtime1)*980);
oldposition1 = newposition1;
oldtime1 = newtime1;
delay(250);
if (speed_r>128){ newposition2 = encoder_r;}
else if (speed_r<128){ newposition2 = 0xFFFFFFFF-encoder_r;}
else if (speed_r==128) { newposition2=0;}
newtime2 = millis();
RPM_r = ((newposition2-oldposition2)*1000*60)/((newtime2-oldtime2)*980);
oldposition2 = newposition2;
oldtime2= newtime2;
delay(250);
2- We have to convert (RPM_l and RPM_r) to (vx, vy, and vth) as follow:
vx=(r/2)*RPM_l*math.cos(th)+(r/2)*RPM_r*math.cos(th);
vx=(r/2)*RPM_l*math.sin(th)+(r/2)*RPM_r*math.sin(th);
vth=(r/B)*omega_l-(r/B)*omega_r;
Hint: r and B are wheel radius and vehicle width respectively.
3- The odom (nav_msgs/Odometry) package is:
#!/usr/bin/env python
import math
from math import sin, cos, pi
import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from md49_messages.msg import md49_encoders
rospy.init_node('odometry_publisher')
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()
x = 0.0
y = 0.0
th = 0.0
vx =0.1
vy = -0.1
vth = 0.1
current_time = rospy.Time.now()
last_time = rospy.Time.now()
r = rospy.Rate(1.0)
while not rospy.is_shutdown():
current_time = rospy.Time.now()
# compute odometry in a typical way given the velocities of the robot
dt = (current_time - last_time).to_sec()
delta_x = (vx * cos(th) - vy * sin(th)) * dt
delta_y = (vx * sin(th) + vy * cos(th)) * dt
delta_th = vth * dt
x += delta_x
y += delta_y
th += delta_th
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
# first, we'll publish the transform over tf
odom_broadcaster.sendTransform(
(x, y, 0.),
odom_quat,
current_time,
"base_link",
"odom"
)
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
# publish the message
odom_pub.publish(odom)
last_time = current_time
r.sleep()
The problem was in serial comunication setup not for the code.
Setup UART on the raspi 3 GPIO
For some strange reason the default for Pi3 using the latest 4.4.9 kernel is to DISABLE UART. To enable it you need to change enable_uart=1 in /boot/config.txt. (There is no longer necessary to add core_freq=250 to fix the core frequency to get stable baudrate.)
If you don’t use Bluetooth (or have undemanding uses) it is possible to swap the ports back in the Device Tree. There is a pi3-miniuart-bt and pi3-disable-bt module which are described in /boot/overlays/README here.
As mentioned, by default the new GPIO UART is not enabled so the first thing to do is to edit the config.txt file to enable the serial UART:
sudo nano /boot/config.txt
and add the line (at the bottom):
enable_uart=1
You have to reboot for the changes to take effect.
To check where your serial ports are pointing you can use the list command with the long “-l” listing format:
ls -l /dev
In the long listing you will find:
serial0 -> ttyS0
serial1 -> ttyAMA0
Thus on a Raspberry Pi 3 and Raspberry Pi Zero W, serial0 will point to GPIO J8 pins 8 and 10 and use the /dev/ttyS0. On other Raspberry Pi’s it will point to /dev/ttyAMA0.
So where possible refer to the serial port via it’s alias of “serial0” and your code should work on both Raspberry Pi 3 and other Raspberry Pi’s.
You also need to remove the console from the cmdline.txt. If you edit this with:
sudo nano /boot/cmdline.txt
you will see something like this:
dwc_otg.lpm_enable=0 console=serial0,115200 console=tty1 root=/dev/mmcblk0p2 rootfstype=ext4 elevator=deadline fsck.repair=yes root wait
remove the line: console=serial0,115200 and save and reboot for changes to take effect:
reboot
First off, you need to import nav_msgs/Odometry as the following:
from nav_msgs.msg import Odometry
You must have a function that performs those conversions and then in rospy.Subscriber import those variables, like this:
def example(data):
data.vx=<conversion>
data.vth=<conversion>
def listener():
rospy.Subscriber('*topic*', Odometry, example)
rospy.spin()
if __name__ == 'main':
listener()
I think this would work
I have a measurement system, which responds to a step (green line) with an exponential decline (blue line, which would be the measured data).
I want to go back from the blue line to the green line using deconvolution. Is this step-response already sufficient information for the deconvolution or would it be necessary to have the impulse response?
Thanks for your help,
I had the same problem. I think that it can be addressed using of the fact that Dirac delta is a derivative of Heaviside function. You just need to take numerical derivative of your step response and use it as impulse response for the deconvolution.
Here is an example:
import numpy as np
from scipy.special import erfinv, erf
from scipy.signal import deconvolve, convolve, resample, decimate, resample_poly
from numpy.fft import fft, ifft, ifftshift
def deconvolve_fun(obs, signal):
"""Find convolution filter
Finds convolution filter from observation and impulse response.
Noise-free signal is assumed.
"""
signal = np.hstack((signal, np.zeros(len(obs) - len(signal))))
Fobs = np.fft.fft(obs)
Fsignal = np.fft.fft(signal)
filt = np.fft.ifft(Fobs/Fsignal)
return filt
def wiener_deconvolution(signal, kernel, lambd=1e-3):
"""Applies Wiener deconvolution to find true observation from signal and filter
The function can be also used to estimate filter from true signal and observation
"""
# zero pad the kernel to same length
kernel = np.hstack((kernel, np.zeros(len(signal) - len(kernel))))
H = fft(kernel)
deconvolved = np.real(ifft(fft(signal)*np.conj(H)/(H*np.conj(H) + lambd**2)))
return deconvolved
def get_signal(time, offset_x, offset_y, reps=4, lambd=1e-3):
"""Model step response as error function
"""
ramp_up = erf(time * multiplier)
ramp_down = 1 - ramp_up
if (reps % 1) == 0.5:
signal = np.hstack(( np.zeros(offset_x),
ramp_up)) + offset_y
else:
signal = np.hstack(( np.zeros(offset_x),
np.tile(np.hstack((ramp_up, ramp_down)), reps),
np.zeros(offset_x))) + offset_y
signal += np.random.randn(*signal.shape) * lambd
return signal
def make_filter(signal, offset_x):
"""Obtain filter from response to step function
Takes derivative of Heaviside to get Dirac. Avoid zeros at both ends.
"""
# impulse response. Step function is integration of dirac delta
hvsd = signal[(offset_x):]
dirac = np.gradient(hvsd)# + offset_y
dirac = dirac[dirac > 0.0001]
return dirac, hvsd
def get_step(time, offset_x, offset_y, reps=4):
""""Creates true step response
"""
ramp_up = np.heaviside(time, 0)
ramp_down = 1 - ramp_up
step = np.hstack(( np.zeros(offset_x),
np.tile(np.hstack((ramp_up, ramp_down)), reps),
np.zeros(offset_x))) + offset_y
return step
# Worst case scenario from specs : signal Time t98% < 60 s at 25 °C
multiplier = erfinv(0.98) / 60
offset_y = .01
offset_x = 300
reps = 1
time = np.arange(301)
lambd = 0
sampling_time = 3 #s
signal = get_step(time, offset_x, offset_y, reps=reps)
filter = get_signal( time, offset_x, offset_y, reps=0.5, lambd=lambd)
filter, hvsd = make_filter(filter, offset_x)
observation = get_signal(time, offset_x, offset_y, reps=reps, lambd=lambd)
assert len(signal) == len(observation)
observation_est = convolve(signal, filter, mode="full")[:len(observation)]
signal_est = wiener_deconvolution(observation, filter, lambd)[:len(observation)]
filt_est = wiener_deconvolution(observation, signal, lambd)[:len(filter)]
This will allow you to obtain these two figures:
Heaviside and Dirac
Signal and Filter Estimate
You should also benefit from checking other related posts and the example of Wiener deconvolution that I partly use in my code.
Let me know if this helps.
Can anybody guide me to some existing implementations of anisotropic diffusion, preferably the perona-malik diffusion?
translate the following MATLAB code :
% pm2.m - Anisotropic Diffusion routines
function ZN = pm2(ZN,K,iterate);
[m,n] = size(ZN);
% lambda = 0.250;
lambda = .025;
%K=16;
rowC = [1:m]; rowN = [1 1:m-1]; rowS = [2:m m];
colC = [1:n]; colE = [2:n n]; colW = [1 1:n-1];
result_save=0;
for i = 1:iterate,
%i;
% result=PSNR(Z,ZN);
% if result>result_save
% result_save=result;
% else
% break;
% end
deltaN = ZN(rowN,colC) - ZN(rowC,colC);
deltaS = ZN(rowS,colC) - ZN(rowC,colC);
deltaE = ZN(rowC,colE) - ZN(rowC,colC);
deltaW = ZN(rowC,colW) - ZN(rowC,colC);
% deltaN = deltaN .*abs(deltaN<K);
% deltaS = deltaS .*abs(deltaS<K);
% deltaE = deltaE .*abs(deltaE<K);
% deltaW = deltaW .*abs(deltaW<K);
fluxN = deltaN .* exp(-((abs(deltaN) ./ K).^2) );
fluxS = deltaS .* exp(-((abs(deltaS) ./ K).^2) );
fluxE = deltaE .* exp(-((abs(deltaE) ./ K).^2) );
fluxW = deltaW .* exp(-((abs(deltaW) ./ K).^2) );
ZN = ZN + lambda*(fluxN +fluxS + fluxE + fluxW);
%ZN=max(0,ZN);ZN=min(255,ZN);
end
the code is not mine and has been taken from: http://www.csee.wvu.edu/~xinl/code/pm2.m
OpenCV Implementation (It needs 3 channel image):
from cv2.ximgproc import anisotropicDiffusion
ultrasound_ad_cv2 = anisotropicDiffusion(im,0.075 ,80, 100)
Juxtapose comparison
From scratch in Python: (For grayscale image only)
import scipy.ndimage.filters as flt
import numpy as np
import warnings
def anisodiff(img,niter=1,kappa=50,gamma=0.1,step=(1.,1.),sigma=0, option=1,ploton=False):
"""
Anisotropic diffusion.
Usage:
imgout = anisodiff(im, niter, kappa, gamma, option)
Arguments:
img - input image
niter - number of iterations
kappa - conduction coefficient 20-100 ?
gamma - max value of .25 for stability
step - tuple, the distance between adjacent pixels in (y,x)
option - 1 Perona Malik diffusion equation No 1
2 Perona Malik diffusion equation No 2
ploton - if True, the image will be plotted on every iteration
Returns:
imgout - diffused image.
kappa controls conduction as a function of gradient. If kappa is low
small intensity gradients are able to block conduction and hence diffusion
across step edges. A large value reduces the influence of intensity
gradients on conduction.
gamma controls speed of diffusion (you usually want it at a maximum of
0.25)
step is used to scale the gradients in case the spacing between adjacent
pixels differs in the x and y axes
Diffusion equation 1 favours high contrast edges over low contrast ones.
Diffusion equation 2 favours wide regions over smaller ones.
"""
# ...you could always diffuse each color channel independently if you
# really want
if img.ndim == 3:
warnings.warn("Only grayscale images allowed, converting to 2D matrix")
img = img.mean(2)
# initialize output array
img = img.astype('float32')
imgout = img.copy()
# initialize some internal variables
deltaS = np.zeros_like(imgout)
deltaE = deltaS.copy()
NS = deltaS.copy()
EW = deltaS.copy()
gS = np.ones_like(imgout)
gE = gS.copy()
# create the plot figure, if requested
if ploton:
import pylab as pl
from time import sleep
fig = pl.figure(figsize=(20,5.5),num="Anisotropic diffusion")
ax1,ax2 = fig.add_subplot(1,2,1),fig.add_subplot(1,2,2)
ax1.imshow(img,interpolation='nearest')
ih = ax2.imshow(imgout,interpolation='nearest',animated=True)
ax1.set_title("Original image")
ax2.set_title("Iteration 0")
fig.canvas.draw()
for ii in np.arange(1,niter):
# calculate the diffs
deltaS[:-1,: ] = np.diff(imgout,axis=0)
deltaE[: ,:-1] = np.diff(imgout,axis=1)
if 0<sigma:
deltaSf=flt.gaussian_filter(deltaS,sigma);
deltaEf=flt.gaussian_filter(deltaE,sigma);
else:
deltaSf=deltaS;
deltaEf=deltaE;
# conduction gradients (only need to compute one per dim!)
if option == 1:
gS = np.exp(-(deltaSf/kappa)**2.)/step[0]
gE = np.exp(-(deltaEf/kappa)**2.)/step[1]
elif option == 2:
gS = 1./(1.+(deltaSf/kappa)**2.)/step[0]
gE = 1./(1.+(deltaEf/kappa)**2.)/step[1]
# update matrices
E = gE*deltaE
S = gS*deltaS
# subtract a copy that has been shifted 'North/West' by one
# pixel. don't as questions. just do it. trust me.
NS[:] = S
EW[:] = E
NS[1:,:] -= S[:-1,:]
EW[:,1:] -= E[:,:-1]
# update the image
imgout += gamma*(NS+EW)
if ploton:
iterstring = "Iteration %i" %(ii+1)
ih.set_data(imgout)
ax2.set_title(iterstring)
fig.canvas.draw()
# sleep(0.01)
return imgout
Usage
:
#anisodiff(img,niter=1,kappa=50,gamma=0.1,step=(1.,1.),sigma=0, option=1,ploton=False)
us_im_ad = anisodiff(ultrasound,100,80,0.075,(1,1),2.5,1)
Source
Juxtapose comparison